Étape 20 : Programmer le robot.
Vous aurez besoin de télécharger le code source de ce robot de mon dépôt github. Puisque vous avez déjà attribué les ID de servo, vous devriez avoir les bibliothèques installées au bon endroit. Nous allons utiliser la classe DynamixSerial et la classe CommanderHS. DynamixSerial est utilisé pour contrôler les servomoteurs Dynamixel servos, tandis que la classe commandant est utilisée pour traiter les commandes de la manette commandant Arbotix qui sont en provenance du XBee. L’esquisse que vous voulez charger se trouve à DynamixShield/sketches/ProbingRobot/ProbingRobot.ino. Je vais aller sur les parties principales de cette esquisse ici d’expliquer comment il fonctionne.
1. nous partons en incluant les en-têtes pour les bibliothèques, que nous aurons besoin.
#include //include la bibliothèque de servo pour commander les Servos RobotGeek
#include « CommanderHS.h »
#include
2. ensuite, nous déclarons certaines constantes, que nous aurons besoin qui définissent les ID pour les servos et certaines valeurs min et max pour les mouvements.
Servo 1: Gauche, roue arrière
#define LEFT_REAR_WHEEL_ID 1
Servo 2: Gauche, roue avant
#define LEFT_FRONT_WHEEL_ID 2
Servo 3: Droite, roue avant
#define RIGHT_FRONT_WHEEL_ID 3
Servo 4: Droite, roue arrière
#define RIGHT_REAR_WHEEL_ID 4
Servo 5: Poignet de griffe
#define CLAW_WRIST_ID 5
Griffe de Servo gauche standard
#define GRIPPER_SERVO_PIN 2
#define GRIPPER_MIN 0
#define GRIPPER_MAX 150
Servo gripperServo ; créer un objet de servo pour les micro servos de 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 ; Commencent à 150 degrés
int gripperAdd = 0 ; Ajouter commencent à 0 degrés
int currentLeftSpeed = 0 ; int currentRightSpeed = 0 ;
3. puis définir les classes DynamixSerial et commandant. La classe de DynamixSerial suppose qu’il utilise pour les communications pour les servos de servomoteurs Dynamixel Serial1. Toutefois, si vous passez d’un port série hardware ici vous pouvez l’ignorer. C’est donc le logiciel peut être utilisé sur des projets personnalisés qui n’utilisent pas le DynamixShield. Pour la classe de commandant, vous devez passer dans une classe de série de matériel. J’ai également inclus une version de la classe commandant qui vous permet d’utiliser le logiciel série ainsi. Si vous souhaitez utiliser, puis incluent le CommanderSS.
Commande de CommanderHS = CommanderHS (& Serial3) ;
DynamixelSerial Dynamixel() ; ou Dynamixel(&Serial1) ;
4. dans la configuration, nous commençons tout d’abord la connexion série de débogage si le débogage est activé. Nous puis démarrez le commandant à 38 400 bauds et puis commencer la classe DynamixSerial. Il est par défaut à une fréquence de communication 1 Mbaud et la broche qu’elle utilise pour contrôler l’half duplex du protocole servomoteurs Dynamixel est définie en fonction sur la carte que vous utilisez. Ainsi de suite a Due c’est cheville 22, mais sur le zéro c’est la broche 2. Cependant, vous avez également la possibilité de spécifier ces paramètres dans l’appel à la méthode begin pour remplacer ce qui est utilisé pour des projets personnalisés. Après que c’est fait, alors il appelle une méthode pour configurer les servos et les décalages de commandant. Je n’entrerai pas dans ces méthodes en détail. Ils ont principalement juste les servos dans le bon mode d’installation et tout réinitialiser une position par défaut.
void setup() {}
#ifdef ENABLE_DEBUG
Serial.Begin(57600) ;
while (!. Série) ;
Serial.println (« configuration de départ ») ;
#endif
Command.Begin(38400) ;
Dynamixel.Begin () ; ou Dynamixel.begin (1000000, 22) ;
configureServos() ;
configureCommanderOffsets() ;
}
5. la boucle principale est très simple. Il appelle simplement checkCommander et puis des retards de 10 millisecondes.
void loop() {}
checkCommander() ;
Delay(10) ;
}
6. la base de la méthode checkCommander est montré. Il vérifie d’abord si la commande a reçu tous les messages. Si elle a il vérifie ensuite si les boutons pour un tour rapide ont été touchés. Si ce n’est pas le cas, elle traite ensuite les boutons pour déplacer les roues. Il traite ensuite les boutons pour le poignet et la pince.
void checkCommander() {}
if(Command. ReadMsgs() > 0) {}
Si nous tournons vite puis utilisez-le pour contrôle
les roues. Sinon, utilisez manette de jeu régulière.
{if(!processFastTurns())}
processWheels() ;
}
processWrist() ;
processGripper() ;
7. vous pouvez utiliser les six boutons sur le dessus du commandant pour faire le robot tourner en place. Les trois boutons sur le côté droit il fera tourner à droite à des vitesses différentes, alors que les trois boutons sur la gauche il fera tourner à gauche à des vitesses différentes. En outre, les touches droite et gauche à l’avant du commandant rendra rotation droite et gauche à la vitesse la plus rapide. Nous pouvons le voir ci-dessous où nous avons mis les roues gauche et droite pour tourner dans des directions opposées avec les valeurs de vitesse différente selon laquelle le bouton est frappé.
bool processFastTurns() {}
Si (command.buttons & BUT_R1 ||
Command.Buttons & BUT_RT) {}
setLeftWheels(1023) ;
setRightWheels(-1023) ;
retourne la valeur true ;
}
ElseIf (command.buttons & BUT_L6 ||
Command.Buttons & BUT_LT) {}
setLeftWheels(-1023) ;
setRightWheels(1023) ;
retourne la valeur true ;
}
ElseIf (command.buttons & BUT_R2) {}
setLeftWheels(512) ;
setRightWheels(-512) ;
retourne la valeur true ;
}
ElseIf (command.buttons & BUT_L5) {}
setLeftWheels(-512) ;
setRightWheels(512) ;
retourne la valeur true ;
}
ElseIf (command.buttons & BUT_R3) {}
setLeftWheels(256) ;
setRightWheels(-256) ;
retourne la valeur true ;
}
ElseIf (command.buttons & BUT_L4) {}
setLeftWheels(-256) ;
setRightWheels(256) ;
retourne la valeur true ;
}
retourne la valeur false ;
}
8. Si vous n’êtes pas dans le rapide activer le mode, puis il vérifie si vous utilisez le joystick pour contrôler le mouvement du robot. la méthode processWheels pour cela trouver la grandeur et l’angle du mouvement du levier de commande à pied. Le levier de commande de marche est celui sur la gauche du commandant. Il utilise ces valeurs pour déterminer les vitesses gauche et à droite moteurs à utiliser.
void processWheels() {}
Tout d’abord permet de déterminer la longueur totale du vecteur walkV
Il contrôlera la vitesse globale
int vitesse = sqrt ((command.walkV*command.walkV) +
(command.walkH*command.walkH)) ;
float speedNorm = (float) Vitesse / (float) 144,0 ;
int leftSpeed = 0, rightSpeed = 0 ;
L’angle de la verticale à l’horizontale va contrôler combien de tour il est
Si (vitesse > 0) {}
ratio de flotteur = (float) (vitesse de command.walkV)/ ;
float leftRatio = 0, rightRatio = 0 ;
Si (command.walkH > 0) {}
leftRatio = sign(ratio) * speedNorm ;
rightRatio = rapport * speedNorm ;
}
else {}
rightRatio = sign(ratio) * speedNorm ;
leftRatio = rapport * speedNorm ;
}
Les valeurs du retour du commandant arbotix ne sont pas circulaires
Ils sont plus rectangulaires. Donc, si vous normalisez il puis au max
avance et retour paramètres c’est seulement à environ 70 % force. Ceci
multiplicateur permet d’obtenir la vitesse max en allant vers l’avant ou vers l’arrière.
flotteur multiplicateur = 1 ;
Si (((ratio > = 0,90) & & (ratio < = 1,0)) ||
((ratio < = -0,90) & & (ratio > = -1,0))) {
coefficient multiplicateur = 1.4141f ;
}
leftSpeed = 1023 * leftRatio * multiplicateur ;
rightSpeed = 1023 * rightRatio * multiplicateur ;
setLeftWheels(leftSpeed) ;
setRightWheels(rightSpeed) ;
}
9. ensuite, nous vérifions le poignet dans la méthode processWrist. Il prend le signal vertical de regard joystick pour contrôler les mouvements de montée/descente du bras de la pince. Le joystick du regard est celle sur la droite du commandant.
void processWrist() {}
int wristAdd = carte (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. ensuite, nous vérifions si quelqu'un tente de fermer la pince en regardant le signal du regard horizontal.
void processGripper() {}
int gripperAdd = carte (command.lookH, -102, 102, -10, 10) ;
Si (gripperAdd! = 0) {}
gripperPos += gripperAdd ;
if(gripperPos > GRIPPER_MAX) {}
gripperPos = GRIPPER_MAX ;
}
d’autre if(gripperPos < GRIPPER_MIN) {}
gripperPos = GRIPPER_MIN ;
}
gripperServo.write(gripperPos) ;
}
}
11. Enfin, voici une description de le setRightWheels. Cette méthode contrôle les roues droite du robot pour les faire bouger. Le setLeftWheels est à peu près la même chose, mais il contrôle les roues de l’autre côté.
void setRightWheels (int vitesse) {}
Si (vitesse! = currentRightSpeed) {}
currentRightSpeed = Vitesse ;
Si (vitesse > 0) {}
Si {(vitesse > 1023)
Vitesse = 1023 ;
}
Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 1, vitesse) ;
Delay(10) ;
Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 1, vitesse) ;
Delay(10) ;
}
else {}
Si (vitesse < -1023) {}
Vitesse = -1023 ;
}
Dynamixel.Turn (RIGHT_REAR_WHEEL_ID, 0,-vitesse) ;
Delay(10) ;
Dynamixel.Turn (RIGHT_FRONT_WHEEL_ID, 0,-vitesse) ;
Delay(10) ;
}
}
}
Et c’est tout. Il y a d’autres choses dans le sketch que j’ai n’ont pas discuté ici, mais c’est principalement pour des fins de débogage. Vous pouvez activer le débogage de décommenter la //#define ENABLE_DEBUG 1 ligne en haut. Cet exemple affiche ce qui se passe lorsque vous exécutez l’esquisse. Utiliser l’IDE Arduino pour vous programmer le croquis dans votre microcontrôleur, puis permet d’exécuter notre nouveau robot !
