Consultas, desarrollo de programas y petición de presupuestos:

sábado, 19 de mayo de 2012

Gambas Y Arduino: Moviendo dos servomotores



Gambas Y Arduino:
3º Ejemplo
Moviendo dos Servomotores


En el ejemplo anterior ( lectura y graficos de datos de arduino con gambas), vimos como gambas podía reconocer una trama de datos que enviaba arduino, y se encargaba de mostrarlos en una gráfica.

Esta vez, vamos a hacer el paso contrario: Pasar "ordenes" desde Gambas a Arduino, y que este las interprete y ejecute lo que le estemos diciendo (mueva el motor indicado una serie de grados).


Para ello la tramo de datos, (protocolo)  es la siguiente:
12333

siendo:
1: Letra  que indica cual dispositivo es sobre hay que actuar. Usamos la "M" para indicar servomotor
2: Numero que indica cual de los dispositivos vamos a mover. (si hay varios servomotores, el 0, sera el primero, el 1 será el segundo... etc, el limite son 10 (de 0 a 9)
333: Indica los grados que se van a girar los servomotores.

Por ejemplo, si le mandamos desde gambas a arduino este comando:
M0180  -> Girara, el servomotor (M), nº 0, 180º grados.

M190 -> Girará, el servomotor (M), nº 1, 90 º grados.


Para facilitar al usuario, esta labor, lo que hacemos es usar  unos botones gráficos tipo "DIAL"





y  el programa en Gambas, crea y manda las ordenes a Arduino, que se encarga de leer la orden, interpretarla y ejecutarla.











Aqui teneis el montaje:

 

Nota: se usan los puertos digitales (PWM) para conectar los servomotores con arduino. Es importante no usar el pin nº 0 ni el pin nº 1, ya que ambos se usan para la comunicación en Serie-USB con el ordenador.

Aqui teneis el código del programa en Gambas:




' Gambas class file

Public sport As New SerialPort

Public conectado As Boolean

Public mensaje As String

Public Sub _new()

End

Public Sub Form_Open()

Me.Center
conectado = False
End

Public Sub ToolButton1_Click()

If conectado = False Then

If Sport.Status = Net.Active Then
Close Sport
Else
sport.PortName = "/dev/ttyUSB0" 'esto puede cambiar segun al puerto al que conectes tu arduino
sport.speed = 9600 
  'tiene que ser la misma velocidad que tenga el programa de arduino.
sport.Parity = SerialPort.None
sport.DataBits = SerialPort.Bits8
sport.StopBits = SerialPort.Bits1
sport.FlowControl = SerialPort.None
Try sport.Open()
If Error Then
Message.Error("Error al intentar conectar con puerto: "& sport.PortName)
conectado = False
Else
'le mando el comando "S" para que me conteste con los sensores que me va a devolver...

Try Print #sport, "S"
If Error Then
Message.Error("Error al intentar enviar datos...")
Print "No he podido conectarme"
conectado = False
Else
Print "ok...Conectado"
ToolButton1.picture = Picture["icon:/16/stop"]
conectado = True
mensaje = "M00"
EscribeEnArduino(mensaje)

Endif

'creo las clases para graficar los datos que voy a recibir
Endif
Endif

Else
conectado = False
ToolButton1.picture = Picture["icon:/16/play"]
Close Sport
Endif

End

Public Sub DialServo1_Change()

LCDNumber1.value = DialServo1.Value
'mortor 0, lo giro lo que indique el LCDNumber1
mensaje = "M0" & Str$(LCDNumber1.Value)
EscribeEnArduino(mensaje)

End

Public Sub DialServo2_Change()

LCDNumber2.value = DialServo2.Value
'mortor 1, lo giro lo que indique el LCDNumber1
mensaje = "M1" & Str$(LCDNumber2.Value)
EscribeEnArduino(mensaje)

End

Public Sub DialServo3_Change()

LCDNumber3.value = DialServo3.Value

DialServo1.Value = LCDNumber3.value
DialServo2.Value = LCDNumber3.value
'mortor 1, lo giro lo que indique el LCDNumber1
mensaje = "M0" & Str$(LCDNumber3.Value)
EscribeEnArduino(mensaje)
Wait 0.1
mensaje = "M1" & Str$(LCDNumber3.Value)
EscribeEnArduino(mensaje)

End

Public Sub EscribeEnArduino(mensaje As String)

Try Print #sport, mensaje
If Error Then
Message.Error("Error al intentar enviar datos...")
Print "No he podido conectarme"
Endif
Print MENSAJE

End


Y el código de Arduino:



#include <Servo.h>

const int TotalServos=2; // indico el numero de servos que tengo conectados
Servo servo[TotalServos]; //defino un array de N elementos de instancia de la clase Servo
int pos[TotalServos]; // guarda posicion de los servos
byte a; // contador


void setup()
{
Serial.begin(9600);
Serial.println("Arduino conectado...");
Serial.println(" a la espera de ordenes");
// Definos los pin donde ira conectado cada servo: TotalServos

servo[0].attach(2);// el servo 0, le asigno el pin 2; alservo 1, le asigno el pin 3,....
servo[1].attach(3);
}
void loop()
{
//Formato de orden de reconozco:
//M0180
//M=Motor
//0=Nº del motor (si recibo N entoces son todos los motores)
//180= grados de giro..
int dato; // leo
byte contador=0;


if (Serial.available()) {
dato=Serial.read();

Serial.println("Recibido: ");
Serial.println(dato);

if (dato==83) {
Serial.println("Recibido S");
}
if (dato==77)
{
// entrada de orden de motor
Serial.println("Recibido M");
dato=Serial.read();
vermotor(dato);
}
// entrada para ampliar a otras ordenes
// … (para añadir mas sensores... por ejemplo)

}
}

void vermotor(int dato) {
// convierto el caracter en numero (que indicara el servo a mover)
int valor=0;
int b=0;

if (dato==78) {
Serial.println("Recibido N");
Serial.print("Todos los motores los muevos grados: ");
valor= recibirnumero();

Serial.println(valor);
// coloco a los servos al valor que me han pasado...
for (b=0;b<=TotalServos-1;b++)
{
pos[b]=valor;
Serial.println("Servo (");
Serial.println(b);
Serial.println("): ");
Serial.println(valor);
servo[b].write(valor);
}
}

if ((dato>=48) && (dato<=57)) {
Serial.println("escribo en motor");
movermotor(dato);
}
}


int recibirnumero()
{
boolean entrada=true;
char cadena[24];
int n;
a=0;
while (Serial.available()>0)
{
// delay(5);
cadena[a]=Serial.read();
a++;
}

cadena[a]='\0';

Serial.println("En letra :");
Serial.println(cadena);
n=atoi(cadena);
Serial.println("En numero :");
Serial.println(n);

return n;
}


void movermotor(int dato){
int valor;
Serial.print("Muevo el motor (");
Serial.print(dato-48);
Serial.print("): ");
valor=recibirnumero();
Serial.println("valor: ");
Serial.println(valor);
pos[dato-48]=valor;
servo[dato-48].write(valor);
delay(150);
dato=0;
}


Como veis tiene varias subrutinas (vermotor, movermotor y recibirnumero), que ayudan a que sea más claro el programa.

Lo que me costó más trabajo, fue la lectura de las ordenes del puerto serie, ya que Arduino lee caracteres, o mejor dicho la conversión a ASCII de ellos, por eso cuando analizo la orden, no analizo caracteres sino números:


if
 (dato==78) {
Serial.println("Recibido N");

N en ASCII es el nº 78,


Además teneia que "unir" los caracteres que formaban el nº que indica los grados a girar, eso lo solucioné usando un array del tipo Char, como podeis ver en el código de la subrutina recibirnumero.






Os dejo un video donde muestro como se usa el programa:



Os dejo los enlaces de descarga del cógido fuente de los dos programas, ya que el  "copiar y pegar" os puede dar muchos problemas.

Códigos fuentes:
 DosServoMotores.pde
ControlServos.tar.gz 

No hay comentarios:

Publicar un comentario en la entrada