#include #include LiquidCrystal_I2C lcd(0x27, 16,2); #define X_pin A0 #define Y_pin A1 #define sw 10 #define Bb 2 //Bouton Bleu #define Bv 3 //Bouton Vert #define Br 4 //Bouton Rouge #define Bg 7 //Bouton Gris //Etat des boutons int bouttonb_stat = 0; int bouttonv_stat = 0; int bouttonr_stat = 0; int bouttong_stat = 0; float Rx;//Cordonnée Actuel selon x float Ry;//Cordonnée Actuel selon y float Rxx;//Cordonnée a atteindre selon x float Ryy;//Cordonnée a atteindre selon y int angle_effecteur=0; //Definition des servos Servo droit; Servo gauche; Servo haut; //Valeur de sauvgarde d'états int haut_s=50; int bas_s=43; int etat=bas_s; float cpt=0; //Parametres du modele geometrique inverse float l1=7; float l4=8; float l2=7; float l3=8; float l5=3.5; float a=5; int cpts=0; int x_val; int y_val; int sw_val; //Modele geometrique inverse float theta1(float xp,float yp){ float O1P=sqrt((-a+xp)*(-a+xp)+(yp)*(yp)); float b=(((l1*l1)-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0) )+(O1P*O1P))/(2*l1*O1P)); float alpha=acos(b)*180/M_PI; float beta=atan(((yp)/(a-xp)))*180/M_PI; float theta=180-(beta+alpha); return theta; } float theta2(float xp,float yp){ float O1P=sqrt((-a+xp)*(-a+xp)+(yp)*(yp)); float omega=(acos(((O1P*O1P)-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0))-(l1*l1))/(-2*(sqrt(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0)))*l1))*180.0/M_PI)+acos((-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0))+(l5*l5)-(l4*l4))/(-2*l4*(sqrt(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0)))))*180.0/M_PI; float omega_bis=omega-theta1(xp,yp)-angle_effecteur; float O2C=sqrt((a+(xp+l5*(cos(omega_bis*M_PI/180.0))))*(a+(xp+l5*cos(-omega_bis*M_PI/180.0)))+(yp+l5*sin(-omega_bis*M_PI/180.0))*(yp+l5*sin(-omega_bis*M_PI/180.0))); float alpha2=acos((((l2*l2)-(l3*l3)+(O2C*O2C))/(2*l2*O2C)))*180/M_PI; float beta2=atan(((yp+l5*sin(-omega_bis*M_PI/180.0))/(a+xp+l5*cos(-omega_bis*M_PI/180.0))))*180.0/M_PI; float theta2=beta2+alpha2; return theta2; } //DEbut du programme void setup() { Serial.begin(9600); droit.attach(5); gauche.attach(6); haut.attach(9); pinMode(X_pin,INPUT); pinMode(Y_pin,INPUT); pinMode(sw,INPUT_PULLUP); pinMode (Bb,INPUT_PULLUP); pinMode (Bv,INPUT_PULLUP); pinMode (Br,INPUT_PULLUP); pinMode (Bg,INPUT_PULLUP); droit.write(90); gauche.write(120); haut.write(60); lcd.init();//initialisation de l'afficheur Lcd lcd.backlight();//Allumer le retro eclairage } void loop() { //initialisation des bouttons bouttong_stat = digitalRead(Bg); bouttonb_stat = digitalRead(Bb); bouttonr_stat = digitalRead(Br); bouttonv_stat = digitalRead(Bv); //Ligne continue if(bouttonb_stat==LOW){ haut.write(60); lcd.setCursor(1,0);//Position d'écriture dans l'afficheur lcd.print("Mode : Ligne ");//Ecriture dans l'afficheur haut.write(45); //Dessin d'une ligne de 5cm for(float i=-2.5;i<=2.5;i=i+0.0015){ droit.write(180-theta1(i,12)); gauche.write(180-theta2(i,12)); } delay(50); haut.write(60); droit.write(180-theta1(-2.5,12)); gauche.write(180-theta2(-2.5,12)); delay(1000); lcd.clear();//Effacer } //Ligne discontinue if(bouttonv_stat==LOW){ lcd.setCursor(1,0);//Position d'écriture dans l'afficheur lcd.print("Mode : Ligne ");//Ecriture dans l'afficheur lcd.setCursor(1,1);//Position d'écriture dans l'afficheur lcd.print("Discontinue ");//Ecriture dans l'afficheur //Dessin d'une ligne de 5cm for(float i=-2.5;i<=2.5;i=i+0.1){ droit.write(180-theta1(i,12)); gauche.write(180-theta2(i,12)); cpt=cpt+0.1; //Sauvegarde de l'état if(cpt>=0.1 && etat==bas_s ){ etat=haut_s; haut.write(haut_s); delay(700); } else if(cpt>=0.1 && etat==haut_s) { etat=bas_s; haut.write(bas_s); delay(700); } } delay(50); droit.write(180-theta1(-2.5,12)); gauche.write(180-theta2(-2.5,12)); delay(1000); lcd.clear();//Effacer } //Cercle if(bouttonr_stat==LOW){ lcd.setCursor(1,0);//Position d'écriture dans l'afficheur lcd.print("Mode : Cercle ");//Ecriture dans l'afficheur haut.write(40); //Dessin du demi cercle superieur for(float x=-2;x<=2;x=x+0.009){ gauche.write(180-theta2(x,12+sqrt((2*2)-(x*x)))); droit.write(180-theta1(x,12+sqrt((2*2)-(x*x)))); delay(10); } //Dessin du demi cercle inferieur for(float x=2;x>=-2;x=x-0.009){ gauche.write(180-theta2(x,12-sqrt((2*2)-(x*x)))); droit.write(180-theta1(x,12-sqrt((2*2)-(x*x)))); delay(10); } delay(2000); lcd.clear();//Effacer } //Cercle discontinue if(bouttong_stat==LOW){ lcd.setCursor(1,0);//Position d'écriture dans l'afficheur lcd.print("Mode : Cercle ");//Ecriture dans l'afficheur lcd.setCursor(1,1);//Position d'écriture dans l'afficheur lcd.print("Discontinue ");//Ecriture dans l'afficheur cpt=0; etat=haut_s; //Dessin du demi cercle superieur for(float x=-2.5;x<=2.5;x=x+0.2){ gauche.write(180-theta2(x,12+sqrt((2.5*2.5)-(x*x)))); droit.write(180-theta1(x,12+sqrt((2.5*2.5)-(x*x)))); delay(10); cpt=cpt+0.1; //Sauvgarde de l'etat actuel if(cpt>=0.1 && etat==bas_s ){ etat=haut_s; cpt=0; haut.write(haut_s); delay(700); } else if(cpt>=0.1 && etat==haut_s) { etat=bas_s; cpt=0; haut.write(bas_s); delay(700); } } //Dessin du demi cercle inferieur for(float x=2.5;x>=-2.5;x=x-0.2){ gauche.write(180-theta2(x,12-sqrt((2.5*2.5)-(x*x)))); droit.write(180-theta1(x,12-sqrt((2.5*2.5)-(x*x)))); delay(10); cpt=cpt+0.1; //Sauvgarde de l'etat actuele if(cpt>=0.1 && etat==bas_s ){ etat=haut_s; cpt=0; haut.write(haut_s); delay(700); } else if(cpt>=0.1 && etat==haut_s) { etat=bas_s; haut.write(bas_s); delay(700); } } delay(2000); lcd.clear();//Effacer } //Mode manuel if (bouttonr_stat == HIGH && bouttonb_stat == HIGH && bouttonv_stat == HIGH && bouttong_stat == HIGH) { lcd.setCursor(1,0);//Position d'écriture dans l'afficheur lcd.print("Mode : Manuel ");//Ecriture dans l'afficheur // Lecture des valeurs du joystick sw_val=digitalRead(sw); Serial.println(sw_val); x_val = analogRead(X_pin); y_val = analogRead(Y_pin); //conversion Rx=map(x_val,0,1023,3,-3); Ry=map(y_val,0,1023,13.8,7); //Commande en vitesse if (Rxx < Rx) { Rxx += 0.01; if (Rxx > Rx) { Rxx = Rx; } } else if (Rxx > Rx) { Rxx -= 0.01; if (Rxx < Rx) { Rxx = Rx; } } if (Ryy < Ry) { Ryy += 0.01; if (Ryy > Ry) { Ryy = Ry; } } else if (Ryy > Ry) { Ryy -= 0.01; if (Ryy < Ry) { Ryy = Ry; } } droit.write(180-theta1(Rxx,Ryy)); gauche.write(180-theta2(Rxx,Ryy)); //Lever le stylo if(!digitalRead(sw) && etat==haut_s){ haut.write(45); etat=bas_s; delay(300); } else if (!digitalRead(sw) && etat==bas_s){ haut.write(60); etat=haut_s; delay(300); } /*if(cpts==1 && sw_val==0 ){ haut.write(haut_s); } else{ haut.write(bas_s); }*/ delay(10); } }