#include #include #include #include //----------------------INITIALISATION----------------------------------------------------------- //declaration pins servomoteur/boutons #define servo1Pin 3 #define servo2Pin 5 #define servo3Pin 9 #define bouton1 2 #define bouton2 4 //declaration des servomoteurs Servo servo1 ; Servo servo2 ; Servo servo3 ; //declaration des variables int etatBouton1,etatBouton2 ; float x , y ; // variable position (x,y) float l1 = 70 ; // longueur première bielle float l2 = 103.87 ; // longueur deuxième bielle const int xJoy = A0 ; // pins qui recoivent les données du joystick const int yJoy = A1 ; void setup() // fonction lue en premier { Serial.begin(9600) ; // lance le moniteur servo1.attach(3) ; //attache les servomoteurs à leur pins servo2.attach(5) ; servo3.attach(9) ; pinMode(2,INPUT) ; // met les boutons en mode lecture pinMode(4,INPUT) ; pos_ini() ; // va à la position initiale delay(1000) ; } //----------------------ECRITURE FONCTIONS--------------------------------------------------------------- void positionBasse() { servo3.write(63) ; delay(10) ; } void positionHaute() { servo3.write(77) ; delay(10) ; } void pos_ini(){ //Les angles 93 et 90 correspondent aux angles 0,0 de notre modèle géométrique servo1.write(93) ; servo2.write(90) ; delay(150) ; positionHaute() ; } int al(float x, float y){ // calcul l'angle alpha en fonction de x,y float a = -atan(x/y)-acos((l1*l1 - l2*l2+ x*x+ y*y)/(2*l1*sqrt(x*x + y*y))) ; a = a / 2 / M_PI * 360 ; int alpha = round(a) ; return alpha ; } int be(float x, float y){ // calcul l'angle beta en fonction de x,y float k2=0.344 ; float b = M_PI - acos((l1*l1 + l2*l2 -x*x - y*y)/(2*l1*l2)) ; b = b / 2 / M_PI * 360 ; b = b * k2 ; int beta = round(b) ; return beta ; } int secu(float x, float y){ // fonction qui retourne 1 si tout va bien 0 si les angles sont hors format float alpha = al(x,y) ; float beta = be(x,y) ; if(beta<=124.5 && alpha>=-60){ // angles à ne pas franchir return 1 ; } else{ Serial.print("erreur") ; return 0 ; } } //--------------- DROITE ----------------------------------------- void droite(){ //------ Point de depart ----------------- delay(50) ; y= 75 ; x = -100 ; servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(250) ; positionBasse() ; delay(50) ; //------ Tracé de la droite ---------------- for(float y=75; y<=125; y+= 0.3) { servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(54); } //----- Retour Position intiale ------------ delay(150); positionHaute(); delay(200); pos_ini(); } //--------------- DROITE POINTILLÉE --------------------------------------- void ligneP(){ //------ Point de depart --------------------- delay(100) ; y= 75 ; x = -100 ; // x fixé servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(500) ; //------ Tracé droite point. ---------------- for(float y=75 ; y<=125 ; y+= 4) { //------calcul angle----------- servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; //------montée/descente--------- delay(225) ; positionBasse() ; delay(235) ; positionHaute() ; delay(225) ; } pos_ini() ; } //--------------- ROND ----------------------------------------- void rond(){ //------ Point de depart ----------------- delay(50) ; x = -75 ; y = 100 ; servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(450) ; positionBasse() ; delay(150) ; //centre du rond int x0 = -100 ; int y0 = 100 ; //------ Tracé du rond ---------------------- for(float i=0.01 ; i<2*M_PI + 0.2 ; i+= 0.01){ //calculs de position x = x0 + 25*cos(i) ; y = y0 + 25*sin(i) ; //calculs d'angles servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(12) ; } delay(300); positionHaute(); delay(300) ; pos_ini() ; } //--------------- ROND POINTILLÉ --------------------------------------- void rondP(){ //------ Point de depart -------------------- x = -75 ; y = 100 ; servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; delay(150) ; positionBasse() ; delay(100) ; int x0 = -100 ; // position du centre du rond int y0 = 100 ; //------ Tracé du rond point. ---------------- for(float i = 0.01 ; i<2*M_PI ; i += 0.39){ //------calcul position/angle--------- x = x0 + 25*cos(i) ; y = y0 + 25*sin(i) ; servo1.write(93+al(x,y)) ; servo2.write(90-be(x,y)) ; //------montée/descente stylo--------- delay(150) ; positionBasse() ; delay(230) ; positionHaute() ; delay(150) ; } //----- Retour Position intiale ------------ delay(150); pos_ini(); } //--------------- JOYSTICK ----------------------------------------- void Joy(){ //------ Point de depart ----------------- x = -100; y = 100; servo1.write(93+al(x,y)); servo2.write(90-be(x,y)); //------ Boucle d'arret ------------------ while(etatBouton1 == LOW){ //----descente/montée------------ etatBouton2 = digitalRead(4) ; // lecture bouton de descente/montée if(etatBouton2 == HIGH){ delay(150) ; if (servo3.read()<69){ positionHaute() ; delay(150) ; } else{ positionBasse(); delay(150) ; } } //---Recupération x y Joystick--- int xVal = analogRead(xJoy)-500 ; int yVal = analogRead(yJoy)-500 ; float a = float(xVal)/2500 ; float b = float(yVal)/2500 ; //-----Ecriture des angles------- if(secu(x,y)!=0){ // securité x += a; y += b; servo1.write(93+al(x,y)); servo2.write(90-be(x,y)); } delay(20); etatBouton1 = digitalRead(2); // bouton d'arrêt du while } //----- Retour Position intiale ------------ delay(300) ; positionHaute() ; delay(300) ; pos_ini() ; } //-----------------------BOUCLE PRINCIPALE---------------------------------------------------------------- void loop() { // recupération de x du Joystick, qui va définir le mode de fonctionnement int xChoice = analogRead(xJoy) ; //--------Joystick au milieu--------------------- if(xChoice < 900 && xChoice > 150){ // lecture états boutons etatBouton1 = digitalRead(2) ; etatBouton2 = digitalRead(4) ; if (etatBouton1 == HIGH){ //ROND EN 10s rond() ; } if(etatBouton2 == HIGH){ // ligne droite en 10s droite() ; } } //--------Joystick à gauche --------------------- else if(xChoice < 100){ // JOYSTICK Joy() ; } //--------Joystick à droite --------------------- else if(xChoice >= 900){ //boucle pour ne pas sortir du mode tant que //une des deux fonctions n'a pas été exécutée while(etatBouton1 != HIGH && etatBouton2 != HIGH){ // lecture états boutons etatBouton1 = digitalRead(2) ; etatBouton2 = digitalRead(4) ; if (etatBouton1 == HIGH){ //ROND POINTILLÉ EN 10s rondP() ; } if(etatBouton2 == HIGH){ //LIGNE POINTILLÉE EN 10s ligneP() ; } } } }