quent1/OpenCM-Araignee
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Sources

Browse Source
This commit is contained in:
Quentin Bouteiller
2017-12-30 14:09:32 +01:00
commit 46e016fb47
2 changed files with 626 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

272
source_robot_master.c Normal file
View File

@@ -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);
}

354
source_robot_masterless.c Normal file
View File

@@ -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);
}