Stap 20: Programma de robot.
U moet downloaden van de broncode voor deze robot uit mijn github repository. Aangezien u al de servo-id's zijn toegewezen, moet u de libraries geinstalleerd op de juiste plaats. We zullen gebruiken de DynamixSerial-klasse en de klasse CommanderHS. DynamixSerial wordt gebruikt om te controleren de Dynamixel servo's, terwijl de commandant klasse wordt gebruikt voor het verwerken van de opdrachten van de joystick van de commandant van de Arbotix die de XBee vandaan. De schets die u wilt laden is te vinden op DynamixShield/sketches/ProbingRobot/ProbingRobot.ino. Ik zal ingaan op de belangrijkste onderdelen van deze schets hier uit te leggen hoe het werkt.
1. we beginnen door met inbegrip van de koppen voor de bibliotheken die we zullen moeten.
#include //include de servo-bibliotheek om te controleren de RobotGeek servo 's
#include "CommanderHS.h"
#include
2. vervolgens declareren we sommige constanten zullen we die de id's voor de servo's definiëren en sommige min en max waarden voor bewegingen.
Servo 1: Links, achterwiel
#define LEFT_REAR_WHEEL_ID 1
Servo 2: Links, voorwiel
#define LEFT_FRONT_WHEEL_ID 2
Servo 3: Recht, voorwiel
#define RIGHT_FRONT_WHEEL_ID 3
Servo 4: Recht, achterwiel
#define RIGHT_REAR_WHEEL_ID 4
Servo 5: Klauw pols
#define CLAW_WRIST_ID 5
Standaard Servo links klauw
#define GRIPPER_SERVO_PIN 2
#define GRIPPER_MIN 0
#define GRIPPER_MAX 150
Servo gripperServo; een servo-object voor de 9g FT-FS90MG micro servo maken
#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; Beginnen bij 150 graden
int gripperAdd = 0; Beginnen bij 0 graden toevoegen
int currentLeftSpeed = 0; int currentRightSpeed = 0;
3. vervolgens definiëren de klassen DynamixSerial en commandant. De klasse DynamixSerial wordt ervan uitgegaan dat het gebruik van Serial1 voor mededelingen aan de Dynamixel servo's. Echter, als u in een hardware seriële poort en hier doorgeven kunt u het overschrijven. Dit is dus de software kan worden gebruikt op aangepaste projecten die geen van de DynamixShield gebruikmaken. Voor de commandant klasse moet u doorgeven in een seriële hardware-klasse. Ik heb ook een versie van de commandant-klasse die u toelaat om het gebruik software serie zo goed opgenomen. Wilt u gebruik deze in plaats daarvan dan ook de CommanderSS.
CommanderHS opdracht = CommanderHS (& Serial3);
DynamixelSerial Dynamixel(); of Dynamixel(&Serial1);
4. in het configuratiemenu beginnen we de debug seriële aansluiting als foutopsporing is ingeschakeld. Wij vervolgens opstarten de commandant op 38.400 baud, en dan beginnen de DynamixSerial klasse. Wordt standaard een 1 Mbaud communicatie tarief en de pin die het gebruikt om controle van de half-duplex van het Dynamixel-protocol is ingesteld op basis van het bord dat u gebruikt. Dus op een pin 22 is Due, maar op de nul is pin 2. U hebt echter ook de optie voor het opgeven van deze parameters in de aanroep van de methode van het begin te negeren wat wordt gebruikt voor aangepaste projecten. Nadat dat wordt gedaan roept het vervolgens een methode om de servo's en commandant verschuivingen te configureren. Ik zal niet ingaan op deze methoden in detail. Ze vooral gewoon instellen van de servo's in de juiste modus bevindt en reset alles naar een standaardpositie.
VOID Setup {}
#ifdef ENABLE_DEBUG
Serial.begin(57600);
terwijl (!. Serieel);
Serial.println ('setup starten');
#endif
Command.begin(38400);
Dynamixel.begin (); of Dynamixel.begin (1000000, 22);
configureServos();
configureCommanderOffsets();
}
5. de hoofdlus is zeer eenvoudig. Het roept enkel checkCommander en vervolgens vertragingen voor 10 milliseconden.
void loop {}
checkCommander();
delay(10);
}
6. de kern van de methode checkCommander wordt weergegeven. Het controleert eerst of de opdracht geen berichten heeft ontvangen. Als er vervolgens wordt gecontroleerd als de knoppen voor een snelle bocht zijn getroffen. Als dat niet het geval is, dan worden de knoppen voor het verplaatsen van de wielen verwerkt. Het verwerkt dan de knoppen voor de pols en de grijper.
ongeldig checkCommander() {}
if(Command. ReadMsgs() > 0) {}
Als we snel draaien dan gebruiken om te bepalen
de wielen. Anders gebruik gewone joystick.
if(!processFastTurns()) {}
processWheels();
}
processWrist();
processGripper();
7. u kunt de zes knoppen op de bovenkant van de commandant om de robot zet in plaats te maken. De drie knoppen aan de rechterkant maken het draaien goed op verschillende snelheden, terwijl de drie knoppen aan de linkerkant zal het draaien links op verschillende snelheden. Ook, de links en rechts knoppen aan de voorkant van de commandant zal maken het draai rechts en links op de hoogste snelheid. We kunnen dit zien hieronder waar wij Stel de linker- en wielen te draaien in tegengestelde richtingen met verschillende snelheden waarden afhankelijk van welke knop wordt geraakt.
BOOL processFastTurns() {}
Als (command.buttons & BUT_R1 ||
Command.Buttons & BUT_RT) {}
setLeftWheels(1023);
setRightWheels(-1023);
return true;
}
anders als (command.buttons & BUT_L6 ||
Command.Buttons & BUT_LT) {}
setLeftWheels(-1023);
setRightWheels(1023);
return true;
}
anders als (command.buttons & BUT_R2) {}
setLeftWheels(512);
setRightWheels(-512);
return true;
}
anders als (command.buttons & BUT_L5) {}
setLeftWheels(-512);
setRightWheels(512);
return true;
}
anders als (command.buttons & BUT_R3) {}
setLeftWheels(256);
setRightWheels(-256);
return true;
}
anders als (command.buttons & BUT_L4) {}
setLeftWheels(-256);
setRightWheels(256);
return true;
}
return false;
}
8. bent u niet in de snelle draai modus, en gaat dan wordt gecontroleerd als u de joystick gebruikt om de beweging van de robot te controleren. de methode processWheels doet dit door het vinden van de omvang en de hoek van de beweging van de joystick lopen. De wandeling-joystick is degene aan de linkerkant van de commandant. Het maakt gebruik van deze waarden om te bepalen van de links en rechts motor snelheden te gebruiken.
ongeldig processWheels() {}
Ten eerste kunt vinden van de totale lengte van de vector walkV
Dit zal de controle van de algehele snelheid
int speed = sqrt ((command.walkV*command.walkV) +
(command.walkH*command.walkH));
zweven speedNorm = (float) snelheid / (zweven) 144.0;
int leftSpeed = 0, rightSpeed = 0;
De hoek van verticale naar horizontale zal bepalen hoeveel beurt er is
Als (snelheid > 0) {}
float ratio = (float) (command.walkV)/ snelheid;
zweven leftRatio = 0, rightRatio = 0;
Als (command.walkH > 0) {}
leftRatio = sign(ratio) * speedNorm;
rightRatio = verhouding * speedNorm;
}
else {}
rightRatio = sign(ratio) * speedNorm;
leftRatio = verhouding * speedNorm;
}
De waarden van de commandant van de arbotix teruggegeven zijn niet circulaire
Ze zijn meer rechthoekig. Dus als u het vervolgens bij de max normaliseren
vooruit en achteruit instellingen er slechts bij ongeveer 70% sterkte. Dit
multiplier helpt max snelheid wanneer gaan vooruit of achteruit.
zweven multiplier = 1;
Als (((verhouding > = 0.90) & & (verhouding < = 1.0)) ||
((verhouding < =-0.90) & & (verhouding > = -1,0))) {
multiplier = 1.4141f;
}
leftSpeed = 1023 * leftRatio * multiplier;
rightSpeed = 1023 * rightRatio * multiplier;
setLeftWheels(leftSpeed);
setRightWheels(rightSpeed);
}
9. vervolgens controleren we de pols in de processWrist-methode. Duurt het blik joystick verticale signaal om te controleren de omhoog/omlaag-bewegingen van de grijper arm. De joystick voor kijken is de een aan de rechterkant van de commandant.
ongeldig processWrist() {}
int wristAdd = kaart (command.lookV,-102, 102, -10, 10);
Als ((wristPos + wristAdd > = WRIST_POS_MIN) & &
(wristPos + wristAdd < = WRIST_POS_MAX)) {
wristPos += wristAdd;
}
Als (wristAdd! = 0) {}
Dynamixel.moveSpeed (CLAW_WRIST_ID, wristPos, 700);
delay(10);
}
}
10. vervolgens controleren we of iemand probeert te sluiten de grijper door te kijken naar de horizontale blik signaal.
ongeldig processGripper() {}
int gripperAdd = kaart (command.lookH,-102, 102, -10, 10);
Als (gripperAdd! = 0) {}
gripperPos += gripperAdd;
if(gripperPos > GRIPPER_MAX) {}
gripperPos = GRIPPER_MAX;
}
anders if(gripperPos < GRIPPER_MIN) {}
gripperPos = GRIPPER_MIN;
}
gripperServo.write(gripperPos);
}
}
11. tot slot, hier is een beschrijving van de setRightWheels. Deze methode bepaalt de juiste wielen van de robot te laten verplaatsen. De setLeftWheels is vrij veel het zelfde, maar controleert de wielen aan de andere kant.
VOID setRightWheels (int snelheid) {}
Als (snelheid! = currentRightSpeed) {}
currentRightSpeed = snelheid;
Als (snelheid > 0) {}
Als (snelheid > 1023) {}
snelheid = 1023;
}
Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 1, snelheid);
delay(10);
Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 1, snelheid);
delay(10);
}
else {}
Als (snelheid <-1023) {}
snelheid =-1023;
}
Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 0,-snelheid);
delay(10);
Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 0,-snelheid);
delay(10);
}
}
}
En thats it. Er zijn een aantal andere dingen in de schets die ik deed hier niet bespreken, maar dat is vooral voor foutopsporing. U kunt inschakelen door de //#define ENABLE_DEBUG 1 lijn bij de bovenkant te debuggen. Dit zal printen wat als u de schets uitvoert gebeurt er. Gebruik van de Arduino IDE u de schets om in te programmeren uw microcontroller en vervolgens laat uitvoeren voor onze nieuwe robot!
