Commit 19170fb7 authored by behrajor's avatar behrajor

ajout du code arduino commenté

parent 346c5957
#include <Servo.h>
#include <math.h>
//Déclaration des objets Servo
Servo base;
Servo epaule;
Servo coude;
Servo poigne;
Servo pince;
String cmd = "";//La chaine de caractère qui va stocker la commande
int angle[5]; //Angle de Chaque moteur, dans l'ordre : B, E, C, P et G
int prevAngle[5];
void setup()
{
//On relie les objets aux pins respectives
base.attach(5);
epaule.attach(6);
coude.attach(9);
poigne.attach(10);
pince.attach(11);
//Pour initialiser la communication série
Serial.begin(9600);
//Angles initiaux
angle[0] = prevAngle[0] = 90;
angle[1] = prevAngle[1] = 120;
angle[2] = prevAngle[2] = 120;
angle[3] = prevAngle[3] = 90;
angle[4] = prevAngle[4] = 40;
base.write(angle[0]);
epaule.write(angle[1]);
coude.write(angle[2]);
poigne.write(angle[3]);
pince.write(angle[4]);
}
void loop()
{
//Boucle pour lire la commande
while(Serial.available()>0)
{
cmd+=(char)Serial.read();
delay(10);
}
//Si on a reçu une commande...
if(cmd != "")
{
parseCmd();
//Si l'angle à été modifié, on déplace le moteur
if(angle[0] != prevAngle[0])
{
base.write(angle[0]);
prevAngle[0] = angle[0];
}
if(angle[1] != prevAngle[1])
{
epaule.write(angle[1]);
prevAngle[1] = angle[1];
}
if(angle[2] != prevAngle[2])
{
coude.write(angle[2]);
prevAngle[2] = angle[2];
}
if(angle[3] != prevAngle[3])
{
poigne.write(angle[3]);
prevAngle[3] = angle[3];
}
if(angle[4] != prevAngle[4])
{
pince.write(angle[4]);
prevAngle[4] = angle[4];
}
}
}
void parseCmd()
{
char servo; //Lettre du moteur
int value, i = 0;
//boucle pour lire la commande
while(cmd != "")
{
//On élimine la commande si elle n'est pas au bon format (par sécurité)
while((cmd.charAt(0) != 'B' && cmd.charAt(0) != 'E' && cmd.charAt(0) != 'C' &&
cmd.charAt(0) != 'P' && cmd.charAt(0) != 'G') && cmd != "")
cmd.remove(0, 1);
//On prend la première lettre de la commande (qui correspond au moteur)
servo = cmd.charAt(0);
cmd.remove(0, 1);//On la supprime après l'avoir lu
value = cmd.toInt();//On récupère la valeur de l'angle
//On attribue la bonne valeur au bon moteur
switch(servo)
{
case 'B':
angle[0] = value;
break;
case 'E':
angle[1] = value;
break;
case 'C':
angle[2] = value;
break;
case 'P':
angle[3] = value;
break;
case 'G':
angle[4] = value;
break;
}
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment