Bygga en Arduino Gripper Robot med en DynamixShield. (20 / 21 steg)

Steg 20: Programmera roboten.

Du kommer att behöva ladda ner källkoden för denna robot från min github förråd. Eftersom du redan tilldelat de servo-ID bör du ha biblioteken installeras på rätt plats. Vi kommer att använda den DynamixSerial och CommanderHS-klasserna. DynamixSerial används för att kontrollera Dynamixel servon, medan klassen Commander används för att bearbeta kommandon från Arbotix Commander joystick som kommer från XBee. Skissen du vill ladda finns på DynamixShield/sketches/ProbingRobot/ProbingRobot.ino. Jag kommer att gå över de viktigaste delarna av denna skiss här att förklara hur det fungerar.

1. vi börjar med inklusive rubriker för de bibliotek som behövs.

#include //include servo biblioteket att styra RobotGeek servon

#include "CommanderHS.h"

#include

2. sedan förklara vi vissa konstanter kommer vi att definierar ID: N för servon och några min och max värden för rörelser.

Servo 1: Vänster, bakre hjulet

#define LEFT_REAR_WHEEL_ID 1

Servo 2: Vänster, framhjul

#define LEFT_FRONT_WHEEL_ID 2

Servo 3: Höger, framhjul

#define RIGHT_FRONT_WHEEL_ID 3

Servo 4: Höger, bakhjulet

#define RIGHT_REAR_WHEEL_ID 4

Servo 5: Klo handled

#define CLAW_WRIST_ID 5

Standard Servo vänster klo

#define GRIPPER_SERVO_PIN 2

#define GRIPPER_MIN 0

#define GRIPPER_MAX 150

Servo gripperServo; skapa ett servo objekt för 9g FT-FS90MG micro servo

#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; Börja på 150 grader

int gripperAdd = 0; Start vid 0 grader Lägg till

int currentLeftSpeed = 0; int currentRightSpeed = 0;

3. definiera klasserna DynamixSerial och befälhavare. Klassen DynamixSerial förutsätter att det använder Serial1 för kommunikation till Dynamixel servon. Dock om du passerar i hårdvara seriella portar här kan du åsidosätta den. Detta är så att programvaran kan användas på egna projekt som inte använder DynamixShield. För befälhavare klass måste du passera i en hårdvara seriell klass. Jag har också tagit en version av klassen Commander som låter dig använda mjukvaran följetong också. Om du vill använda detta istället då inkluderar CommanderSS.

CommanderHS kommando = CommanderHS (& Serial3);

DynamixelSerial Dynamixel(); eller Dynamixel(&Serial1);

4. i uppsättning startar vi debug seriell anslutning om debugging är aktiverad. Vi sedan starta befälhavaren på 38,400 baud, och sedan börja klassen DynamixSerial. Standardinställningen är en 1 Mbaud kommunikation hastighet och PIN-koden som används för att styra den halv duplex Dynamixel protokollet anges baserat på tavlan du använder. Så på en vederbörlig det är stift 22, men noll är det stift 2. Dock har du också möjlighet att ange dessa parametrar i anropet till metoden begin att åsidosätta vad som används för egna projekt. Efter det är gjort då samtal en metod för att konfigurera servon och commander förskjutningar. Jag går inte in dessa metoder i detalj. De huvudsakligen bara setup servon i rätt läge och återställa allt till en standardposition.

void setup() {

#ifdef ENABLE_DEBUG

Serial.BEGIN(57600);

tag (!. Seriell);

Serial.println ("starta installationsprogrammet");

#endif

Command.BEGIN(38400);

Dynamixel.BEGIN (); eller Dynamixel.begin (1000000, 22);

configureServos();

configureCommanderOffsets();

}

5. huvudloop är mycket enkel. Det kräver bara checkCommander och sedan förseningar 10 millisekunder.

void loop() {

checkCommander();

Delay(10);

}

6. kärnan i metoden checkCommander visas. Det kontrollerar först om kommandot har fått några meddelanden. Om den har kontrolleras det om knappar för en snabb sväng har drabbats. Om inte, då den behandlar knapparna för att flytta hjulen. Det bearbetar sedan knapparna för handled och gripare.

void checkCommander() {

IF(Command. ReadMsgs() > 0) {

Om vi vänder snabbt sedan använda den till kontroll

hjulen. Annars använda vanliga joystick.

IF(!processFastTurns()) {

processWheels();

}

processWrist();

processGripper();

7. du kan använda de sex knapparna på ovansidan av befälhavaren för att göra roboten vända på plats. De tre knapparna på höger sida kommer att snurra höger vid olika hastigheter, medan de tre knapparna till vänster gör det snurra vänster vid olika hastigheter. Också, höger och vänster knapparna på framsidan av befälhavaren gör det snurra höger och vänster på snabbaste hastigheten. Vi kan se detta nedan där vi ställa in vänster och höger hjulen att snurra i motsatta riktningar med olika hastighet värden beroende på vilken knapp är hit.

bool processFastTurns() {

om (command.buttons & BUT_R1 ||

Command.buttons & BUT_RT) {

setLeftWheels(1023);

setRightWheels(-1023);

return true;

}

annars om (command.buttons & BUT_L6 ||

Command.buttons & BUT_LT) {

setLeftWheels(-1023);

setRightWheels(1023);

return true;

}

annars om (command.buttons & BUT_R2) {

setLeftWheels(512);

setRightWheels(-512);

return true;

}

annars om (command.buttons & BUT_L5) {

setLeftWheels(-512);

setRightWheels(512);

return true;

}

annars om (command.buttons & BUT_R3) {

setLeftWheels(256);

setRightWheels(-256);

return true;

}

annars om (command.buttons & BUT_L4) {

setLeftWheels(-256);

setRightWheels(256);

return true;

}

returnera false;

}

8. om du inte är i snabbt vända läge, sedan kontrollerar den om du använder joysticken för att kontrollera förflyttning av roboten. metoden processWheels gör detta genom att hitta omfattningen och vinkel av rörelsen av promenad joysticken. Promenad joysticken är vänstra av commanderen. Dessa värden används för att bestämma att använda vänster och höger motor rusar.

void processWheels() {

Först kan hitta den totala längden av walkV vektorn

Detta kommer att kontrollera den totala hastigheten

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

(command.walkH*command.walkH));

flyta speedNorm = (float) hastighet / (float) 144,0;

int leftSpeed = 0, rightSpeed = 0;

Vinkeln på vertikalt till horisontellt kommer att kontrollera hur mycket tur det är

om (hastighet > 0) {

float ratio = (float) (command.walkV)/ hastighet;

flyta leftRatio = 0, rightRatio = 0;

om (command.walkH > 0) {

leftRatio = sign(ratio) * speedNorm;

rightRatio = förhållandet * speedNorm;

}

annat {

rightRatio = sign(ratio) * speedNorm;

leftRatio = förhållandet * speedNorm;

}

De värden som anges tillbaka från den arbotix befälhavaren är inte cirkulär

De är mer rektangulär. Så om du normalisera det sedan på max

vanliga och omvända inställningar är det endast på ca 70% styrka. Detta

multiplikatorn hjälper till att få max hastighet när du går framåt eller bakåt.

flyta multiplikator = 1;

om (((ratio > = 0,90) & & (förhållandet mellan < = 1,0)) ||

((förhållandet mellan < =-0.90) & & (ratio > =-1.0))) {

multiplikator = 1.4141f;

}

leftSpeed = 1023 * leftRatio * multiplikator;

rightSpeed = 1023 * rightRatio * multiplikator;

setLeftWheels(leftSpeed);

setRightWheels(rightSpeed);

}

9. nästa, vi kontrollerar handleden i metoden processWrist. Det tar titt joystick vertikala signalen att styra upp/förflyttningar av gripdon armen. Joysticken Titta är den till höger av commanderen.

void processWrist() {

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

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

(wristPos + wristAdd < = WRIST_POS_MAX)) {

wristPos += wristAdd;

}

om (wristAdd! = 0) {

Dynamixel.moveSpeed (CLAW_WRIST_ID, wristPos, 700);

Delay(10);

}

}

10. så vi kolla om någon försöker stänga griparen genom att titta på den horisontella titt signalen.

void processGripper() {

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

om (gripperAdd! = 0) {

gripperPos += gripperAdd;

IF(gripperPos > GRIPPER_MAX) {

gripperPos = GRIPPER_MAX;

}

annat if(gripperPos < GRIPPER_MIN) {

gripperPos = GRIPPER_MIN;

}

gripperServo.write(gripperPos);

}

}

11. Slutligen, här är en beskrivning av setRightWheels. Denna metod styr rätt hjul för roboten att få dem att flytta. SetLeftWheels är ungefär samma, men styr hjulen på andra sidan.

void setRightWheels (int hastighet) {

om (hastighet! = currentRightSpeed) {

currentRightSpeed = hastighet;

om (hastighet > 0) {

om (hastighet > 1023) {

hastighet = 1023;

}

Dynamixel.turn (RIGHT_REAR_WHEEL_ID, 1, hastighet);

Delay(10);

Dynamixel.turn (RIGHT_FRONT_WHEEL_ID, 1, hastighet);

Delay(10);

}

annat {

om (hastighet <-1023) {

hastighet =-1023;

}

Dynamixel.turn (RIGHT_REAR_WHEEL_ID, 0,-hastighet);

Delay(10);

Dynamixel.turn (RIGHT_FRONT_WHEEL_ID, 0,-hastighet);

Delay(10);

}

}

}

Och det är det. Det finns vissa andra saker i skissen jag inte diskutera här, men det är främst för felsökning ändamål. Du kan aktivera felsökning av uncommenting //#define ENABLE_DEBUG 1 linje överst. Detta kommer att skriva ut vad som händer när du kör skissen. Använda Arduino IDE för att programmet du skissen till din microcontroller och sedan kan köra vår nya robot!