Voici le code. Merci de me dire si tout va bien ou s'il y a une erreur grossière. La compile est ok.
// FourBar525-Sandro-V2 n'utilise pas la bibli Servo.h - 07/12/2020
//équivalences pin/port
// 2 PD2
// 3 PD3
// 4 PD4
// 5 PD5
// 6 PD6
// 7 PD7
// 8 PB0
// 9 PB1
//octets avec un 0 à l'endroit correspondant à un pin dans les registres et 1 ailleurs
#define ZERO_DU_PIN_2 B11111011
#define ZERO_DU_PIN_3 B11110111
#define ZERO_DU_PIN_4 B11101111
#define ZERO_DU_PIN_5 B11011111
#define ZERO_DU_PIN_6 B10111111
#define ZERO_DU_PIN_7 B01111111
#define ZERO_DU_PIN_8 B11111110
#define ZERO_DU_PIN_9 B11111101
//nom du pot correspondant à chaque pin
#define PORT_DE_PIN_2 PORTD
#define PORT_DE_PIN_3 PORTD
#define PORT_DE_PIN_4 PORTD
#define PORT_DE_PIN_5 PORTD
#define PORT_DE_PIN_6 PORTD
#define PORT_DE_PIN_7 PORTD
#define PORT_DE_PIN_8 PORTB
#define PORT_DE_PIN_9 PORTB
#define PERIODE_US 20000 //période
unsigned long start_time;
uint16_t duree_pulse_us[10]={0,0,1700, 1700, 1700, 1700, 1700, 1700, 1700, 1700}; //position initiale
uint16_t duree_pulse_us_min[10]={0,0,1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};
uint16_t duree_pulse_us_max[10]={0,0,2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000};
int angle_min[10]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int angle_max[10]={0, 0, 180, 180, 180, 180, 180, 180, 180, 180};
#include <Servo.h>
unsigned long temp=0, start=0;
int Speed=500; // Speed determines le speed
const int nb=8; // Number of servos
int OnOff[nb]={1,1,1,1,1,1,1,1}; // Servos table on/off
//int OnOff[nb]={0,0,0,0,0,0,0,0}; // Servos table on/off
int FRLS=0, FRRS=1, FLLS=2, FLRS=3, BRLS=4, BRRS=5, BLLS=6, BLRS=7;
//***************** Correction for 90° LS and RS servos position **************************************
// servos FRLS FRRS FLLS FLRS BRLS BRRS BLLS BLRS modify these values according to your servos
int Err[nb]={ 0, 6, 0, 7, 0, 10, 12, 1 };
//*********************************************************************************************************
// 06/12/2020 - symetric Soucoupe volante V13-2 - Ay=145, a1=104, c1=88, Speed=500
int FRx[]= { 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10, 5, 0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80, -80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80};
int FLx[]= { 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10, 5, 0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80, -80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80};
int BRx[]= {-80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10, 5, 0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80};
int BLx[]= {-80,-75,-70,-65,-60,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 80, 75, 70, 65, 60, 55, 50, 45, 40, 35, 30, 25, 20, 15, 10, 5, 0, -5,-10,-15,-20,-25,-30,-35,-40,-45,-50,-55,-60,-65,-70,-75,-80};
int FRy[]= { 0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10, 0, 0, 4, 7, 9, 11, 13, 15, 16, 17, 18, 19, 20, 21, 21, 21, 21, 21, 21, 21, 21, 21, 20, 19, 18, 17, 16, 15, 13, 11, 9, 7, 4, 0};
int FLy[]= { 0, 4, 7, 9, 11, 13, 15, 16, 17, 18, 19, 20, 21, 21, 21, 21, 21, 21, 21, 21, 21, 20, 19, 18, 17, 16, 15, 13, 11, 9, 7, 4, 0, 0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10, 0};
int BRy[]= { 0, 4, 7, 9, 11, 13, 15, 16, 17, 18, 19, 20, 21, 21, 21, 21, 21, 21, 21, 21, 21, 20, 19, 18, 17, 16, 15, 13, 11, 9, 7, 4, 0, 0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10, 0};
int BLy[]= { 0,-10,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-15,-10, 0, 0, 4, 7, 9, 11, 13, 15, 16, 17, 18, 19, 20, 21, 21, 21, 21, 21, 21, 21, 21, 21, 20, 19, 18, 17, 16, 15, 13, 11, 9, 7, 4, 0};
int lgTab = sizeof(FRx)/sizeof(int);
void setup() {
pinMode(0,INPUT_PULLUP); // start/reset button attachment
Serial.begin(115200);
for(int i=2;i<=9;i++)
{
pinMode(i,OUTPUT);
}
Serial.print("\n\t To start, click on the Start button");
while( digitalRead(0) ); delay(400); // waiting for start button pressed
Serial.print("\n\t Started");
start=millis();
}
int i=0; // !!!!!!!!!!!!!!!!!!!!!!!!!!
void loop() {
unsigned long duration=micros()-start_time;
envoyer_pulse();
calculer_nouvelle_position(i);
i++;
//attendre la fin de la période
while(duration < PERIODE_US) {/*ne rien faire*/} // !!!!!!!!!!!!!!!!!!!!!!!!!
}
void calculer_nouvelle_position(int index)
{
int i=index % lgTab; //index croit indéfiniment, i correspond à l'index dans la période
InverseKinematics(FRx[i],FRy[i],FRLS,FRRS);
InverseKinematics(BLx[i],BLy[i],BLLS,BLRS);
InverseKinematics(FLx[i],FLy[i],FLLS,FLRS);
InverseKinematics(BRx[i],BRy[i],BRLS,BRRS);
}
void InverseKinematics(int Px, int Py, int LS, int RS){
const float A1x=0, A1y=145, A2x=0, A2y=145; // Values of servos positions
const float a1=104, c1=88, a2=104, c2=88; // Values of leg sizes lengths
float d=A1y-Py, e=Px; // Calculation of inverse kinematics
float b=sqrt((d*d)+(e*e)); // Calculation of inverse kinematics
float S=acos(d/b); if(e<0)S=(-S); // Calculation of inverse kinematics
float A12=acos(((b*b)+(c1*c1)-(a1*a1))/(2*b*c1)); // Calculation of inverse kinematics
float A22=acos(((b*b)+(c2*c2)-(a2*a2))/(2*b*c2)); // Calculation of inverse kinematics
float A11=(PI/2)-A12+S; // Calculation of inverse kinematics
float A21=(PI/2)-A22-S; // Calculation of inverse kinematics
int S1=round(A11*57.296); // left servo angle in degree
int S2=round(180-(A21*57.296)); // right servo angle in degree
// DEBUG
// Serial.print("\n\n\t Position to reach : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);
// Serial.print("\n\t d=");Serial.print(d);Serial.print("\t\t e=");Serial.print(e);
// Serial.print("\t\t b=");Serial.print(b);Serial.print("\t\t S=");Serial.print(S*57.296);
// Serial.print("\n\t A11=");Serial.print(A11*57.296);Serial.print("\t\t A12=");Serial.print(A12*57.296);
// Serial.print("\t\t A22=");Serial.print(A22*57.296);Serial.print("\t\t A21=");Serial.print(A21*57.296);
// Serial.print("\n\t Result of calculations, angles of the servos");
// Serial.print("\n\t S1=");Serial.print(S1);Serial.print("°\t\t\t S2=");Serial.print(S2);Serial.print("°");
if ( b>(a1+c1) ){
Serial.print("\n\t Target point Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t b=");Serial.print(b);Serial.print(" > ");
Serial.print(a1+c1);Serial.print(" is too long. Target impossible to reach !!!!!");
return;
}
if (S1+Err[LS]<0){
Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t angle S1<0° is not reachable !!!!!");
return;
}
if (S2+Err[RS]>180){
Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t angle S2<0° is not reachable !!!!!");
return;
}
if (S1+Err[LS]>120){
Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t angle S1>140° is not reachable !!!!!");
return;
}
if (S2+Err[RS]<60){
Serial.print("\n\t Position to reach : Px=");Serial.print(Px);Serial.print(" Py=");Serial.print(Py);Serial.print("\t angle S2<40° is not reachable !!!!!");
return;
}
// Serial.print("\t executed command");
if (OnOff[LS]) set_position_degrees(LS,S1+Err[LS]); // set target Left servo position if servo switch is On
if (OnOff[RS]) set_position_degrees(RS,S2+Err[RS]); // set target Right servo position if servo switch is On
}
void envoyer_pulse()
{
start_time=micros();
//mise à 1 de tous les pins
PORTD |= B11111100; //mise à 1 des pins 2 à 7 du port D (ie pins 2 à 7 de l'arduino)
PORTB |= B00000011; //mise à 1 des pins 0à 1 du port B (ie pins 8 et 9 de l'arduino)
while( (PORTD & 0B11111100) | (PORTB & 0B00000011) ) //tant qu'au moins un des pins est à 1
{
unsigned long duree_actuelle_du_pulse=micros()-start_time; //durée écoulée depuis le début du pulse
if(duree_actuelle_du_pulse>=duree_pulse_us[2])
PORT_DE_PIN_2 &= ZERO_DU_PIN_2;
if(duree_actuelle_du_pulse>=duree_pulse_us[3])
PORT_DE_PIN_3 &= ZERO_DU_PIN_3;
if(duree_actuelle_du_pulse>=duree_pulse_us[4])
PORT_DE_PIN_4 &= ZERO_DU_PIN_4;
if(duree_actuelle_du_pulse>=duree_pulse_us[5])
PORT_DE_PIN_5 &= ZERO_DU_PIN_5;
if(duree_actuelle_du_pulse>=duree_pulse_us[6])
PORT_DE_PIN_6 &= ZERO_DU_PIN_6;
if(duree_actuelle_du_pulse>=duree_pulse_us[7])
PORT_DE_PIN_7 &= ZERO_DU_PIN_7;
if(duree_actuelle_du_pulse>=duree_pulse_us[8])
PORT_DE_PIN_8 &= ZERO_DU_PIN_8;
if(duree_actuelle_du_pulse>=duree_pulse_us[9])
PORT_DE_PIN_8 &= ZERO_DU_PIN_9;
}
}
void set_position_degrees(byte pin, int angle_deg)
{
uint16_t duree_us=map(angle_deg, angle_min[pin], angle_max[pin], duree_pulse_us_min[pin], duree_pulse_us_max[pin]);
duree_pulse_us[pin]=duree_us;
}