#include <Servo.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
//----------------------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() ;
}
}
}
}