Étape 14 : L’intégralité du Code pour le pilote de robot
/ * Bot Bluetooth Boom-pince. But dans la vie... Fait suite à vos commandes envoyées comme par magie dans l’air ! Ou à partir de USB:) Rédigé par Scott Beasley - 2015 gratuit pour utiliser ou modifier. Profitez. * / #include / / divers temps retards utilisés pour conduire et servo #define TURNDELAY 475 #define TURNDELAY45 235 / / contrôleur de servo Pololu commandes #define SETSPEED 0 x 01 #define SETPOS 0 x 04 / * zone Globals. * / / / Créer les objets de servo moteur, pour s’interfacer avec SoftwareSerial Serial_servo (10, 11) ; SoftwareSerial Serial_motors (10, 12) ; void configuration () {/ / changer les vitesses de transfert ici si différente de celle sous Serial.begin (9600); / / connexion Bluethooth Serial_servo.begin (9600) ; Serial_motors.Begin (19200) ; cesser (de) ; set_servo_speed (0, 10) ; La valeur de la set_servo_speed de vitesse de mouvement servo (1, 10) ; les faire bouger un peu plus lisse (de home_boom) ; home_gripper () ; retard (500) ; } void boucle () {commande octets = 0, val = 0; / / besoin ce qui suit à rester intact pendant la vie de la / / session alors qu’ils sont faits 'statiques'. Cela nous empêche de / / le devoir de les avoir comme globals statique octet dist = 1 ; Commencez par court déplace à vitesse statique octets = 1 ; Vitesse lente octet statique speed_offset = 40 ; turn_style statique octets = 1 ; et avec virages durs octet statique boom_val = 60 ; gripper_val statique octets = 60 ; Si (Serial.available () > 0) {/ / lire la commande la commande octets entrants = Serial.read ();} interrupteur (commande) {case "w": go_forward (speed_offset); //Serial.println (« Going Forward"); if (dist == 1) {retard (750); halt ();} ElseIf (dist == 2) {retard (1250); halt ();} break ; case"z": go_backward (speed_offset); //Serial.println (« va vers l’arrière"); if (dist == 1) {retarder (750); mettre fin ();} ElseIf (dist == 2) {retarder (1250); mettre fin ();} break ; case 'a' : go_left (speed_offset, turn_style) ; retard (TURNDELAY) ; cesser (de) ; Serial.println (« tournant gauche") ; rupture ; de cas ': go_right (speed_offset, turn_style) ; retard (TURNDELAY) ; cesser (de) ; Serial.println ("droit de tourner") ; rupture ; case « q »: go_left (speed_offset, turn_style) ; retard (TURNDELAY45) ; cesser (de) ; Serial.println (« tournant gauche") ; rupture ; Case « e »: go_right (speed_offset, turn_style) ; retard (TURNDELAY45) ; cesser (de) ; Serial.println ("droit de tourner") ; rupture ; case « h »: mettre un terme () ; Serial.println ("arrêter") ; rupture ; cas ' >': / / mouvement de pince X envoie servo la valeur val = Serial.read () ; gripper_val = val ; Nous limitons la valeur réelle movemovent limites de l’installation set_gripper (val) ; Serial.println ("GripperX") ; rupture ; cas ' ^': / / mouvement de pince Y envoie servo la valeur val = Serial.read () ; boom_val = val ; Nous limitons la valeur réelle movemovent limites de l’installation set_boom (val) ; Serial.println ("GripperY") ; rupture ; case « c »: gripper_val = 60 ; boom_val = 60 ; home_boom () ; home_gripper () ; Serial.println ("GripperHome") ; rupture ; case « v »: / / définir la spped le bot se déplace ; 1 = lent, 3 = le plus rapide = Serial.read () ; Si (vitesse == 1) {speed_offset = 40; / / Slow} ElseIf (vitesse == 2) {speed_offset = 20; / / Med} ElseIf (vitesse == 3) {speed_offset = 0; / / Fast!} //Serial.println ("Set speed") ; rupture ; affaire aurait ": / / définir combien de temps le bot se déplace ; 1 = court, moyen 2, / / 3 = dist continu = Serial.read () ; Serial.println ("set de distance de déplacement") ; rupture ; affaire ' t ': / / définit la façon dont le bot fait tour à tour, une roue (doux) ou deux turn_style (dur) = Serial.read () ; Serial.println ("style tour") ; rupture ; cas « + »: / / vitesse réglée sur téléphone Vitesse ++ ; Si (vitesse > = 4) Vitesse = 1 ; Si (vitesse == 1) {speed_offset = 40; / / Slow} ElseIf (vitesse == 2) {speed_offset = 20; / / Med} ElseIf (vitesse == 3) {speed_offset = 0; / / Fast! } break ; cas '-': / / Dist déplacé de téléphone dist ++ ; Si (dist > = 4) dist = 1 ; Serial.println ("set de distance de déplacement") ; rupture ; affaire «, »: / / pince ouverte de téléphone gripper_val += 10 ; Si (gripper_val > = 179) gripper_val = 179 ; set_gripper (gripper_val) ; Serial.println ("set de pinces") ; rupture ; cas '.': / / pince étroite de téléphone gripper_val-= 10 ; Si (gripper_val < = 0) gripper_val = 0 ; set_gripper (gripper_val) ; Serial.println ("set de pinces") ; rupture ; case « 0 »: / / basse-boom du téléphone boom_val += 10 ; Si (boom_val > = 179) boom_val = 179 ; set_boom (boom_val) ; Serial.println ("Boom set") ; rupture ; affaire « 1 »: / / Raise boom du téléphone boom_val-= 10 ; Si (boom_val < = 0) boom_val = 0 ; set_boom (boom_val) ; Serial.println ("Boom set") ; rupture ; 255-affaire: / / envoyé après pince toutes les commandes Serial.flush () ; rupture ; } Serial.flush () ; Delay(125) ; } void go_forward (byte speed_offset) {//Serial.println ("aller de l’avant...") ; Serial_motors.Write ((byte) 1 + speed_offset) ; Serial_motors.Write ((byte) 128 + speed_offset) ; } void go_backward (byte speed_offset) {//Serial.println ("en reculant...") ; Serial_motors.Write ((byte) 127 - speed_offset) ; Serial_motors.Write ((byte) 255 - speed_offset) ; } void go_left (speed_offset octets, turn_style octets) {//Serial.println ("Going gauche...") ; Si (turn_style == 1) Serial_motors.write ((byte) 1 + speed_offset) ; else Serial_motors.write ((byte) 64) ; Eteindre le moteur gauche (tour doux) Serial_motors.write ((byte) 255 - speed_offset) ; } void go_right (speed_offset octets, turn_style octets) {//Serial.println ("va droit...") ; Serial_motors.Write ((byte) 127 - speed_offset) ; Si (turn_style == 1) Serial_motors.write ((byte) 128 + speed_offset) ; else Serial_motors.write ((byte) 192) ; Éteignez le moteur de droite (virage souple)} void halt () {//Serial.println ("halte!") ; Serial_motors.Write ((byte) 0) ; } / / Définir une position servos / / servo_id est le nombre de servo (0-7) / / angle est la position de l’abs de 500 à 5500 void move_servo (int servo_id, int angle) {unsigned buff char [6] = {0 x 80, 0 x 01, SETPOS, 0 x 00, 0 x 00, 0 x 00}; unsigned int temp ; unsigned char data_hi, data_low temp = angle & 0x1f80 ; data_hi = temp >> 7; data_low = angle & 0x7f ; buff [3] = servo_id ; //servo numéro buff [4] = data_hi ; //data1 buff [5] = data_low ; //data2 () Serial_servo.write chamois, 6) ; Serial_servo.Flush () ; } void set_servo_speed (int servo_id, int vitesse) {unsigned buff char [5] = {0 x 80, 0 x 01, SETSPEED, 0 x 00, 0 x 00}; if (vitesse > 127) Vitesse = 127 ; buff [3] = octet (servo_id); buff [4] = octet (vitesse) ; Serial_servo.Write (chamois, 5) ; Serial_servo.Flush () ; } void set_boom (angle octets) {move_servo (0, carte (angle, 0, 179, 2500, 4000));} void set_gripper (angle octets) {move_servo (1, carte (angle, 0, 179, 2500, 4000));} void home_boom () {move_servo (0, 3000);} void home_gripper () {move_servo (1, 3000);}
