Construir un Robot Arduino pinza con un DynamixShield. (20 / 21 paso)

Paso 20: Programar el robot.

Usted tendrá que descargar el código fuente de este robot desde mi repositorio de github. Puesto que usted ya asignado el ID del servo debe tener las bibliotecas instaladas en el lugar correcto. Utilizaremos la clase DynamixSerial y la clase CommanderHS. DynamixSerial se utiliza para controlar los servos Dynamixel, mientras que la clase de comandante se utiliza para procesar los comandos de la palanca de mando del comandante Arbotix que vienen el XBee. El bosquejo que quiere cargar se encuentra en DynamixShield/sketches/ProbingRobot/ProbingRobot.ino. Voy a entrar en las partes principales de este esbozo aquí para explicar cómo funciona.

1. empezamos por incluyendo los encabezados de las librerías que necesitamos.

#include //include la biblioteca de servo para el control de los Servos RobotGeek

#include "CommanderHS.h"

#include

2. a continuación declaramos algunas constantes tendremos que definen los identificadores para los servos y unos valores min y max para los movimientos.

Servo 1: Izquierda, rueda trasera

#define LEFT_REAR_WHEEL_ID 1

Servo 2: Izquierda, rueda delantera

#define LEFT_FRONT_WHEEL_ID 2

Servo 3: Derecha, rueda delantera

#define RIGHT_FRONT_WHEEL_ID 3

Servo 4: Derecha, rueda trasera

#define RIGHT_REAR_WHEEL_ID 4

Servo 5: Muñeca garra

#define CLAW_WRIST_ID 5

Garra izquierda de Servo estándar

#define GRIPPER_SERVO_PIN 2

#define GRIPPER_MIN 0

#define GRIPPER_MAX 150

Servo gripperServo; crea un objeto servo para el servo micro 9g FT-FS90MG

#define WRIST_POS_MIN 312

#define WRIST_POS_MAX 612

#define LEFT_CLAW_POS_MIN 412

#define LEFT_CLAW_POS_MAX 712

#define RIGHT_CLAW_POS_MIN 312

#define RIGHT_CLAW_POS_MAX 612

#define ENABLE_DEBUG 1

int wristPos = 512; int leftClawPos = 512; int rightClawPos = 512;

int gripperPos = 150; Empezar a 150 grados

int gripperAdd = 0; Añadir de arranque a 0 grados

int currentLeftSpeed = 0; int currentRightSpeed = 0;

3. a continuación se definen las clases DynamixSerial y comandante. La clase DynamixSerial se supone que está utilizando Serial1 para comunicaciones a los servos Dynamixel. Sin embargo, si pasa en un puerto serial hardware puede reemplazarlo. Esto es por lo que el software puede utilizarse en proyectos personalizados que no usan el DynamixShield. Para la clase de comandante debe pasar en una clase de serie de hardware. También he incluido una versión de la clase de comandante que le permite utilizar el software de serie así. Si desea utilizar esto en su lugar entonces incluir la CommanderSS.

Comando de CommanderHS = CommanderHS (& Serial3);

DynamixelSerial Dynamixel(); o Dynamixel(&Serial1);

4. en la configuración, primero empezamos la conexión serie de depuración si está habilitada la depuración. Entonces desde el comandante 38.400 baudios y luego comenzar la clase de DynamixSerial. Por defecto una tarifa de comunicaciones 1 Mbaud y el pin que utiliza para controlar el medio-duplex del protocolo Dynamixel está basado en el tablero que se utiliza. Así sucesivamente un debido es pin 22, pero en el cero es pin 2. Sin embargo, también tienes la opción de especificar estos parámetros en la llamada al método begin para reemplazar lo que se utiliza para proyectos personalizados. Después de que se hace a continuación, llama a un método para configurar los servos y los desvíos de comandante. No entraré en estos métodos en detalle. Principalmente a los servos en el modo correcto de instalación y restablecer todo a una posición predeterminada.

void setup() {}

#ifdef ENABLE_DEBUG

Serial.Begin(57600);

mientras (!. Serie);

Serial.println ("configuración inicial");

#endif

Command.Begin(38400);

Dynamixel.Begin (); o Dynamixel.begin (1000000, 22);

configureServos();

configureCommanderOffsets();

}

5. el bucle principal es muy simple. Solo llama checkCommander y luego retrasos de 10 milisegundos.

void loop() {}

checkCommander();

Delay(10);

}

6. el núcleo del método checkCommander se muestra. Primero comprueba si el comando ha recibido los mensajes. Si tiene a continuación comprueba si se han visto afectados los botones para una vuelta rápida. Si no es así, entonces procesa los botones para mover las ruedas. Luego procesa los botones para la muñeca y la pinza.

void checkCommander() {}

if(Command. ReadMsgs() > 0) {}

Si nos volvemos rápido entonces utilizarlo para control

las ruedas. De lo contrario usar joystick normal.

{if(!processFastTurns())}

processWheels();

}

processWrist();

processGripper();

7. puede usar los seis botones en la parte superior el comandante para hacer que el robot gire en su lugar. Los tres botones a la derecha hará girar derecha a diferentes velocidades, mientras que los tres botones de la izquierda hará girar izquierda a diferentes velocidades. Además, los botones derecho e izquierdos en la parte delantera del comandante hará giro derecha y la izquierda a la velocidad más rápida. Lo vemos a continuación donde montamos las ruedas izquierdas y derecha para girar en sentidos opuestos con valores diferentes dependiendo de que botón se golpea.

bool processFastTurns() {}

Si (command.buttons y BUT_R1 ||

Command.Buttons & BUT_RT) {}

setLeftWheels(1023);

setRightWheels(-1023);

verdaderas;

}

else if (command.buttons & BUT_L6 ||

Command.Buttons & BUT_LT) {}

setLeftWheels(-1023);

setRightWheels(1023);

verdaderas;

}

else if (command.buttons & BUT_R2) {}

setLeftWheels(512);

setRightWheels(-512);

verdaderas;

}

else if (command.buttons & BUT_L5) {}

setLeftWheels(-512);

setRightWheels(512);

verdaderas;

}

else if (command.buttons & BUT_R3) {}

setLeftWheels(256);

setRightWheels(-256);

verdaderas;

}

else if (command.buttons & BUT_L4) {}

setLeftWheels(-256);

setRightWheels(256);

verdaderas;

}

devuelven el valor false;

}

8. Si usted no está en la rápida Gire a modo, luego comprueba si usas el joystick para controlar el movimiento del robot. el método processWheels para ello encontrar la magnitud y el ángulo del movimiento de la palanca de mando a pie. El joystick de a pie es el de la izquierda del comandante. Utiliza estos valores para determinar las velocidades con motor derecho e izquierdas.

void processWheels() {}

En primer lugar, permite encontrar la longitud total del vector walkV

Esto controlará la velocidad general

int velocidad = sqrt ((command.walkV*command.walkV) +

(command.walkH*command.walkH));

Float speedNorm = (float) velocidad / (float) 144,0;

int leftSpeed = 0, rightSpeed = 0;

El ángulo de la vertical a la horizontal controla cuánto se es

Si (velocidad > 0) {}

relación de flotante = (float) (velocidad de command.walkV)/;

Float leftRatio = 0, rightRatio = 0;

Si (command.walkH > 0) {}

leftRatio = sign(ratio) * speedNorm;

rightRatio = cociente * speedNorm;

}

Else {}

rightRatio = sign(ratio) * speedNorm;

leftRatio = cociente * speedNorm;

}

Los valores devueltos del comandante arbotix no son circulares

Son más rectangulares. Así que si se normalizan luego en el máximo

adelante y atrás ajustes sólo en alrededor del 70% fuerza. Esto

multiplicador ayuda a conseguir velocidad máxima cuando va hacia adelante o hacia atrás.

Float multiplicador = 1;

Si (((ratio > = 0,90) & & (cociente < = 1.0)) ||

((cociente < = -0,90) & & (ratio > = -1,0))) {

multiplicador = 1.4141f;

}

leftSpeed = 1023 * leftRatio * multiplicador;

rightSpeed = 1023 * rightRatio * multiplicador;

setLeftWheels(leftSpeed);

setRightWheels(rightSpeed);

}

9. a continuación, comprobamos la muñeca en el método processWrist. Toma la señal vertical de vistazo joystick para controlar los movimientos de arriba/abajo del brazo de agarre. El joystick de la mirada es la de la derecha del comandante.

void processWrist() {}

int wristAdd = mapa (command.lookV, -102, 102, -10, 10);

Si ((wristPos + wristAdd > = WRIST_POS_MIN) & &

(wristPos + wristAdd < = WRIST_POS_MAX)) {

wristPos += wristAdd;

}

Si (wristAdd! = 0) {}

Dynamixel.moveSpeed (CLAW_WRIST_ID, wristPos, 700);

Delay(10);

}

}

10. luego comprobamos si alguien está tratando de cerrar a la pinza mirando la señal de la mirada horizontal.

void processGripper() {}

int gripperAdd = mapa (command.lookH, -102, 102, -10, 10);

Si (gripperAdd! = 0) {}

gripperPos += gripperAdd;

if(gripperPos > GRIPPER_MAX) {}

gripperPos = GRIPPER_MAX;

}

otra cosa if(gripperPos < GRIPPER_MIN) {}

gripperPos = GRIPPER_MIN;

}

gripperServo.write(gripperPos);

}

}

11. Finalmente, aquí está una descripción de la setRightWheels. Este método controla las ruedas derecha del robot para hacerlos mover. El setLeftWheels es el mismo, sino que controla las ruedas en el otro lado.

void setRightWheels (int velocidad) {}

Si (velocidad! = currentRightSpeed) {}

currentRightSpeed = velocidad;

Si (velocidad > 0) {}

Si {} (velocidad > 1023)

velocidad = 1023;

}

Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 1, velocidad);

Delay(10);

Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 1, velocidad);

Delay(10);

}

Else {}

Si (velocidad <-1023) {}

velocidad =-1023;

}

Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 0,-velocidad);

Delay(10);

Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 0,-velocidad);

Delay(10);

}

}

}

Y eso es todo. Hay algunas otras cosas en el sketch que no discutir aquí, sino que es principalmente para propósitos de depuración. Usted puede activar la depuración por tenga //#define ENABLE_DEBUG 1 línea en la parte superior. Esto imprimirá lo que está sucediendo como ejecutar el sketch. Utilice el IDE de Arduino para programar el bosquejo en el microcontrolador y permite ejecuta nuestro nuevo robot!