Sources
This commit is contained in:
272
source_robot_master.c
Normal file
272
source_robot_master.c
Normal 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
354
source_robot_masterless.c
Normal 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);
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user