From 46e016fb47b3357d083eab88370de7a66fb1030e Mon Sep 17 00:00:00 2001 From: Quentin B Date: Sat, 30 Dec 2017 14:09:32 +0100 Subject: [PATCH] Sources --- source_robot_master.c | 272 +++++++++++++++++++++++++++++ source_robot_masterless.c | 354 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 626 insertions(+) create mode 100644 source_robot_master.c create mode 100644 source_robot_masterless.c diff --git a/source_robot_master.c b/source_robot_master.c new file mode 100644 index 0000000..bd14ee3 --- /dev/null +++ b/source_robot_master.c @@ -0,0 +1,272 @@ +// Bus pour le Dxl +#define DXL_BUS_SERIAL1 1 //Dynamixel on Serial1(USART1) <-OpenCM9.04 +#define DXL_BUS_SERIAL2 2 //Dynamixel on Serial2(USART2) <-LN101,BT210 +#define DXL_BUS_SERIAL3 3 //Dynamixel on Serial3(USART3) <-OpenCM 485EXP + +// Nos quatre pattes, avec leur deux servomoteurs + +// Arrière gauche +#define PATTE_1_SERVO1 7 // Proche corps +#define PATTE_1_SERVO2 9 // Proche sol + +// Arrière droit +#define PATTE_2_SERVO1 8 +#define PATTE_2_SERVO2 12 + +// Avant gauche +#define PATTE_3_SERVO1 16 +#define PATTE_3_SERVO2 11 + +// Avant droit +#define PATTE_4_SERVO1 14 +#define PATTE_4_SERVO2 18 + +// Positions initiales +#define POS_INITIALE_SERVO1_TYPE1 380 +#define POS_INITIALE_SERVO1_TYPE2 POS_INITIALE_SERVO1_TYPE1 + 256 + +#define POS_INITIALE_SERVO2_TYPE1 280 + +// Positions avancement +#define ROTATION_SERVO1 150 +#define ROTATION_SERVO2 120 + +// Vitesses servo +#define SPEED_SLOW 100 +#define SPEED_RAPID 200 + +// On initialise notre bus dynamixel +Dynamixel Dxl(DXL_BUS_SERIAL1); + +// Variables d'état de notre robot +int state_avg, state_avd, state_arg, state_ard; + +void setup() +{ + // Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps + Dxl.begin(3); + + // On passe nos 8 servo en mode position + Dxl.jointMode(PATTE_1_SERVO1); + Dxl.jointMode(PATTE_1_SERVO2); + + Dxl.jointMode(PATTE_2_SERVO1); + Dxl.jointMode(PATTE_2_SERVO2); + + Dxl.jointMode(PATTE_3_SERVO1); + Dxl.jointMode(PATTE_3_SERVO2); + + Dxl.jointMode(PATTE_4_SERVO1); + Dxl.jointMode(PATTE_4_SERVO2); + + // On passe dans le premier état (-1 = lancement des moteurs; puis 0 = initialisation) + state_avg = -1; + state_avd = -1; + state_arg = -1; + state_ard = -1; + + // On initialise notre communication série + SerialUSB.println("Hi"); +} + +void loop() +{ + // Gestion des différents états + + // Patte avant droite (patte maitre) + SerialUSB.print("Patte avant droite - état "); SerialUSB.println(state_avd); + patteManager(&state_avd, &state_arg, PATTE_4_SERVO1, PATTE_4_SERVO2, 1, 0, 1); + + // Patte arrière gauche + SerialUSB.print("Patte arrière gauche - état "); SerialUSB.println(state_arg); + patteManager(&state_arg, &state_avg, PATTE_1_SERVO1, PATTE_1_SERVO2, 0, 1, 0); + + // Patte avant gauche + SerialUSB.print("Patte avant gauche - état "); SerialUSB.println(state_avg); + patteManager(&state_avg, &state_ard, PATTE_3_SERVO1, PATTE_3_SERVO2, 0, 1, 1); + + // Patte arrière droite + SerialUSB.print("Patte arrière droite - état "); SerialUSB.println(state_ard); + patteManager(&state_ard, &state_avd, PATTE_2_SERVO1, PATTE_2_SERVO2, 0, 0, 0); + + SerialUSB.println("###################"); +} + +/******************************** +* FONCTIONS D'ANIMATION * +*********************************/ + +// Mouvement d'une patte +int patteManager(int *state, int *next_state, int patte_servo1, int patte_servo2, int is_master, int is_on_left, int is_on_front) +{ + // En fonction des paramètres, on défini les bonnes valeurs à partir des constantes (avg = ard et inversement) + int position_intiale_servo1 = (is_on_front == is_on_left ? POS_INITIALE_SERVO1_TYPE1 : POS_INITIALE_SERVO1_TYPE2); + int position_intiale_servo2 = (is_on_front == is_on_left ? POS_INITIALE_SERVO2_TYPE1 : POS_INITIALE_SERVO2_TYPE1); + + int rotation_move_servo1 = (is_on_left ? position_intiale_servo1 + ROTATION_SERVO1 : position_intiale_servo1 - ROTATION_SERVO1); + int rotation_move_servo2 = position_intiale_servo2 + ROTATION_SERVO2; + + // Switch d'état + switch (*state) + { + // Les différents états de notre patte + + case -1: // Etat spécial pour l'initialisation des positions au démarrage + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_RAPID); + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + *state = 0; // Pret à démarrer ! + + SerialUSB.println("Pret a démarrer"); + + break; + + case 0: // Initialisation, en attente - si la patte est maitre et que le robot est pret à bouger, elle passe dans l'état 1 + /*if(is_master && isReadyToBegin()) + { + *state = 1; + SerialUSB.println("Patte maitre, initialisation - Système pret"); + } + else + { + SerialUSB.println("En attente du maitre"); + }*/ + + if(isReadyToBegin()) + { + *state = 1; + } + + break; + + case 1: // La patte est posée à plat + SerialUSB.print("Etat 1, patte à plat..."); + + // On bouge la patte + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW); + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + // On change d'état + *state = 2; + + break; + + case 2: // Etat transitoire : on attend que la patte se pose + // La position est-elle la bonne ? + if( + (comparefive(Dxl.getPosition(patte_servo1), position_intiale_servo1)) && + (comparefive(Dxl.getPosition(patte_servo2), position_intiale_servo2)) + ) + { + *state = 3; + SerialUSB.println("OK"); + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo1)); + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 3: // La patte se lève et s'avance rapidement + SerialUSB.print("Etat 3, patte se lève et avance..."); + + // On bouge la patte + Dxl.setPosition(patte_servo1, rotation_move_servo1, SPEED_RAPID); + Dxl.setPosition(patte_servo2, rotation_move_servo2, SPEED_RAPID); + + // On change d'état + *state = 4; + + break; + + case 4: // Etat transitoire : on attend que la patte se soit avancée + // La position est-elle la bonne ? + if( + (comparefive(Dxl.getPosition(patte_servo1), rotation_move_servo1)) && + (comparefive(Dxl.getPosition(patte_servo2), rotation_move_servo2)) + ) + { + SerialUSB.println("OK"); + + // On change d'état + *state = 5; + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo1)); + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 5: // Etat où la patte se repose sur le sol + // On anime le servomoteur + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + // On change d'état + *state = 6; + + SerialUSB.print("La patte se repose sur le sol... "); + + break; + + case 6: // Etat transitoire où l'on attend que la patte se repose + // La position est-elle la bonne ? + if(comparefive(Dxl.getPosition(patte_servo2), position_intiale_servo2)) + { + *state = 7; + + // La patte reposée déclenche le mouvement de la suivante + *next_state = 1; + + SerialUSB.println("OK"); + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 7: // Le corps pivote + // Mouvement du servomoteur + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW); + + // Changement d'état + *state = 0; + + SerialUSB.println("Pivot délenché, retour au début"); + + break; + + default: + SerialUSB.println("Etat non supporté"); + break; + + } +} + +/***************** +* OUTILS * +******************/ + +// Vérifier si deux nombres correspondent, à 5 près +int comparefive(int number1, int number2) +{ + return (abs(number1 - number2) <= 5); +} + +// Vérifier que le système est dans un état d'interblocage pour démarrer : le maitre attend tous les esclaves, et les esclaves sont en attente du maitre +int isReadyToBegin() +{ + int statesInitCounter = 0; + + if(state_avg == 0) statesInitCounter++; + if(state_avd == 0) statesInitCounter++; + if(state_arg == 0) statesInitCounter++; + if(state_ard == 0) statesInitCounter++; + + return (statesInitCounter == 4); +} \ No newline at end of file diff --git a/source_robot_masterless.c b/source_robot_masterless.c new file mode 100644 index 0000000..9eb5ae1 --- /dev/null +++ b/source_robot_masterless.c @@ -0,0 +1,354 @@ +// Bus pour le Dxl +#define DXL_BUS_SERIAL1 1 //Dynamixel on Serial1(USART1) <-OpenCM9.04 +#define DXL_BUS_SERIAL2 2 //Dynamixel on Serial2(USART2) <-LN101,BT210 +#define DXL_BUS_SERIAL3 3 //Dynamixel on Serial3(USART3) <-OpenCM 485EXP + +// Nos quatre pattes, avec leur deux servomoteurs + +// Arrière gauche +#define PATTE_1_SERVO1 7 // Proche corps +#define PATTE_1_SERVO2 9 // Proche sol + +// Arrière droit +#define PATTE_2_SERVO1 8 +#define PATTE_2_SERVO2 12 + +// Avant gauche +#define PATTE_3_SERVO1 16 +#define PATTE_3_SERVO2 11 + +// Avant droit +#define PATTE_4_SERVO1 14 +#define PATTE_4_SERVO2 18 + +// Positions initiales +#define POS_INITIALE_SERVO1_TYPE1 380 +#define POS_INITIALE_SERVO1_TYPE2 POS_INITIALE_SERVO1_TYPE1 + 256 + +#define POS_INITIALE_SERVO2_TYPE1 280 + +// Positions avancement +#define ROTATION_SERVO1 150 +#define ROTATION_SERVO2 120 + +// Vitesses servo +#define SPEED_SLOW 150 +#define SPEED_RAPID 300 + +// Les 4 directions + le repos +#define DIRECTION_NONE 1 +#define DIRECTION_FRONT 2 +#define DIRECTION_BACK 3 +#define DIRECTION_LEFT 4 +#define DIRECTION_RIGHT 5 + +// On initialise notre bus dynamixel +Dynamixel Dxl(DXL_BUS_SERIAL1); + +// Variables d'état de notre robot +int state_avg, state_avd, state_arg, state_ard; +int direction, wanted_direction = NULL; + +void setup() +{ + // Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps + Dxl.begin(3); + + // On passe nos 8 servo en mode position + Dxl.jointMode(PATTE_1_SERVO1); + Dxl.jointMode(PATTE_1_SERVO2); + + Dxl.jointMode(PATTE_2_SERVO1); + Dxl.jointMode(PATTE_2_SERVO2); + + Dxl.jointMode(PATTE_3_SERVO1); + Dxl.jointMode(PATTE_3_SERVO2); + + Dxl.jointMode(PATTE_4_SERVO1); + Dxl.jointMode(PATTE_4_SERVO2); + + // On passe dans le premier état (-1 = lancement des moteurs; puis 0 = initialisation) + state_avg = -1; + state_avd = -1; + state_arg = -1; + state_ard = -1; + + // Direction par défaut + direction = DIRECTION_NONE; + + // On lie une interruption sur le port série USB + // Pour les commandes du robot + SerialUSB.attachInterrupt(USBInterrupt); +} + +void loop() +{ + // Gestion des différents états + + // Patte avant droite + SerialUSB.print("Patte avant droite - état "); SerialUSB.println(state_avd); + pawManager(&state_avd, &state_arg, PATTE_4_SERVO1, PATTE_4_SERVO2, 0, 1); + + // Patte arrière gauche + SerialUSB.print("Patte arrière gauche - état "); SerialUSB.println(state_arg); + pawManager(&state_arg, &state_avg, PATTE_1_SERVO1, PATTE_1_SERVO2, 1, 0); + + // Patte avant gauche + SerialUSB.print("Patte avant gauche - état "); SerialUSB.println(state_avg); + pawManager(&state_avg, &state_ard, PATTE_3_SERVO1, PATTE_3_SERVO2, 1, 1); + + // Patte arrière droite + SerialUSB.print("Patte arrière droite - état "); SerialUSB.println(state_ard); + pawManager(&state_ard, &state_avd, PATTE_2_SERVO1, PATTE_2_SERVO2, 0, 0); + + SerialUSB.println("###################"); +} + +/******************************** +* FONCTIONS D'ANIMATION * +*********************************/ + +// Mouvement d'une patte +int pawManager(int *state, int *next_paw_state, int patte_servo1, int patte_servo2, int is_on_left, int is_on_front) +{ + // En fonction des paramètres, on défini les bonnes valeurs à partir des constantes (avg = ard et inversement) + int position_intiale_servo1 = (is_on_front == is_on_left ? POS_INITIALE_SERVO1_TYPE1 : POS_INITIALE_SERVO1_TYPE2); + int position_intiale_servo2 = POS_INITIALE_SERVO2_TYPE1; + + int rotation_move_servo1 = position_intiale_servo1; + int rotation_move_servo2 = position_intiale_servo2 + ROTATION_SERVO2; + + // Le sens de déplacement influence les ordres donnés aux servomoteurs + switch (direction) + { + case DIRECTION_FRONT: + rotation_move_servo1 += (is_on_left ? ROTATION_SERVO1 : -1*ROTATION_SERVO1); + break; + + case DIRECTION_BACK: + rotation_move_servo1 += (is_on_left ? -1*ROTATION_SERVO1 : ROTATION_SERVO1); + break; + + case DIRECTION_LEFT: + rotation_move_servo1 -= ROTATION_SERVO1; + break; + + case DIRECTION_RIGHT: + rotation_move_servo1 += ROTATION_SERVO1; + break; + } + + // Switch d'état + switch (*state) + { + // Les différents états de notre patte + + case -1: // Etat spécial pour l'initialisation des positions au démarrage + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_RAPID); + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + *state = 0; // Pret à démarrer ! + + SerialUSB.println("Pret a démarrer"); + + break; + + case 0: // Initialisation, en attente - si le robot est pret à bouger, elle passe dans l'état 1 + if(isReadyToBegin()) + { + // Si la direction doit être changée, elle doit l'être à ce moment + if(direction != wanted_direction && wanted_direction != NULL) + { + direction = wanted_direction; + wanted_direction = NULL; + break; + } + + *state = 1; + SerialUSB.println("Système pret"); + } + else + { + SerialUSB.println("En attente des autres pattes"); + } + + break; + + case 1: // La patte est posée à plat + SerialUSB.print("Etat 1, patte à plat..."); + + // On bouge la patte + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW); + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + // On change d'état + *state = 2; + + break; + + case 2: // Etat transitoire : on attend que la patte se pose + // La position est-elle la bonne ? + if( + (compareFive(Dxl.getPosition(patte_servo1), position_intiale_servo1)) && + (compareFive(Dxl.getPosition(patte_servo2), position_intiale_servo2)) + ) + { + // L'état suivant dépend de si le robot doit avancer + if(direction == DIRECTION_NONE) + { + *state = 0; + } + else + { + *state = 3; + } + + SerialUSB.println("OK"); + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo1)); + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 3: // La patte se lève et s'avance rapidement + SerialUSB.print("Etat 3, patte se lève et avance..."); + + // On bouge la patte + Dxl.setPosition(patte_servo1, rotation_move_servo1, SPEED_RAPID); + Dxl.setPosition(patte_servo2, rotation_move_servo2, SPEED_RAPID); + + // On change d'état + *state = 4; + + break; + + case 4: // Etat transitoire : on attend que la patte se soit avancée + // La position est-elle la bonne ? + if( + (compareFive(Dxl.getPosition(patte_servo1), rotation_move_servo1)) && + (compareFive(Dxl.getPosition(patte_servo2), rotation_move_servo2)) + ) + { + SerialUSB.println("OK"); + + // On change d'état + *state = 5; + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo1)); + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 5: // Etat où la patte se repose sur le sol + // On anime le servomoteur + Dxl.setPosition(patte_servo2, position_intiale_servo2, SPEED_RAPID); + + // On change d'état + *state = 6; + + SerialUSB.print("La patte se repose sur le sol... "); + + break; + + case 6: // Etat transitoire où l'on attend que la patte se repose + // La position est-elle la bonne ? + if(compareFive(Dxl.getPosition(patte_servo2), position_intiale_servo2)) + { + *state = 7; + + // La patte reposée déclenche le mouvement de la suivante + // Sauf si un changement de direction est décidé : on bloque le mouvement + if(wanted_direction == NULL) + { + *next_paw_state = 1; + } + + SerialUSB.println("OK"); + } + else + { + SerialUSB.println(Dxl.getPosition(patte_servo2)); + } + + break; + + case 7: // Le corps pivote + // Mouvement du servomoteur + Dxl.setPosition(patte_servo1, position_intiale_servo1, SPEED_SLOW); + + // Changement d'état + *state = 0; + + SerialUSB.println("Pivot délenché, retour au début"); + + break; + + default: + SerialUSB.println("Etat non supporté"); + break; + + } +} + +// La direction est reçue par le port série USB +// Cette fonction gère l'interruption +void USBInterrupt(byte* buffer, byte nCount) +{ + // Ici, la valeur reçue est un nombre [0;5] + // Donc tenant sur 1 seul octet + // Pas besoin de parcourir au delà de l'index 0 du buffer + SerialUSB.print("Reçu (ascii ; int) : "); + SerialUSB.print(buffer[0]); + SerialUSB.print(" ; "); + + // On stocke la valeur dans une variable temporaire, en convertissant le char en int + // Sous condition que la valeur reçue soit un nombre ASCII + if(buffer[0] > 47 && buffer[0] < 58) + { + int tmp_direction = buffer[0] - '0'; + SerialUSB.print(tmp_direction); + + // La valeur est-elle cohérente ? + switch(tmp_direction) + { + case DIRECTION_NONE: + case DIRECTION_FRONT: + case DIRECTION_BACK: + case DIRECTION_LEFT: + case DIRECTION_RIGHT: + wanted_direction = tmp_direction; + break; + } + } + + SerialUSB.println(); +} + +/***************** +* OUTILS * +******************/ + +// Vérifier si deux nombres correspondent, à 5 près +int compareFive(int number1, int number2) +{ + return (abs(number1 - number2) <= 5); +} + +// Vérifier que toutes les pattes sont en attente de démarrage +int isReadyToBegin() +{ + int statesInitCounter = 0; + + if(state_avg == 0) statesInitCounter++; + if(state_avd == 0) statesInitCounter++; + if(state_arg == 0) statesInitCounter++; + if(state_ard == 0) statesInitCounter++; + + return (statesInitCounter == 4); +} \ No newline at end of file