1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155
| /////////////////////////////////////////
// CONSTANTE DEFINITION //
/////////////////////////////////////////
const int SM_SPEED=100 ; // SERVOMOTOR TIME IN MILISECONDS (100=HIGH , 1000=LOW)
const float L1=42.4; // "CUISSE" LENGTH
const float L2=53.3; // "MOLLET" LENGTH
const float L3=75.5; // FEET LENGTH
const float COORD_INIT [3] = {60,0,125}; // COORDINATE X,Y,Z OF INITIAL POSITION
// DEFINE PIN NUMBER FOR EACH DDL (GAMMA, ALPHA, BETA) AND EACH LEG.
const int PIN [6][3] = {
{29,30,31}, // LEG 1 (LEFT FRONT)
{25,26,27}, // LEG 2 (LEFT MIDDLE)
{18,17,16}, // LEG 3 (LEFT REAR)
{13,14,15}, // LEG 4 (RIGHT FRONT)
{9,10,11}, // LEG 5 (RIGHT MIDDLE)
{2,1,0}, // LEG 6 (RIGHT REAR)
};
// DEFINE POSITION 0° FOR EACH DDL (GAMMA, ALPHA, BETA) AND EACH LEG.
const int POSITION_0 [6][3] = {
{1700,1950,2500}, // LEG 1 (LEFT FRONT)
{1400,1400,2450}, // LEG 2 (LEFT MIDDLE)
{800,1900,2500}, // LEG 3 (LEFT REAR)
{800,1450,2500}, // LEG 4 (RIGHT FRONT)
{1250,1400,2200}, // LEG 5 (RIGHT MIDDLE)
{2000,1300,2400}, // LEG 6 (RIGHT REAR)
};
/////////////////////////////////////////
// VARIABLE DEFINITION //
/////////////////////////////////////////
float ANGLE [3]={0}; // VECTOR (GAMMA,ALPHA,BETA)
int POSITION [6][3]={{0}}; // POSITION MATRIX WITH SM COMMAND FOR EACH LEG AND DLL
// DEFINE ROTATION SIGN FOR POSITIVE ANGLE GAMMA, ALPHA, BETA.
const int SIGN [6][3] = {
{-1,-1,-1}, // LEG 1 (LEFT FRONT)
{-1,-1,-1}, // LEG 2 (LEFT MIDDLE)
{-1,-1,-1}, // LEG 3 (LEFT REAR)
{ 1,-1,-1}, // LEG 4 (RIGHT FRONT)
{ 1,-1,-1}, // LEG 5 (RIGHT MIDDLE)
{ 1,-1,-1}, // LEG 6 (RIGHT REAR)
};
void setup()
{
Serial.begin(9600);
}
void loop() {
COORD_CALCUL(COORD_INIT[0],COORD_INIT[1],COORD_INIT[2],ANGLE); // Calculate gamma, alpha and beta angles for initial position x,y and z
POSITION_MAT (ANGLE,SIGN,POSITION_0,POSITION[6][3]); // Calculate matrix of command servomotors for intial position from ANGLE
Serial.println("Sortie de la boucle");
stop();
}
void stop()
{
while(1);
}
////////////////////////////////////////////////////////////////////////
// FUNCTION : CALCULATE GAMMA,ALPHA AND BETA VERSUS COORDINATE X,Y,Z //
////////////////////////////////////////////////////////////////////////
void COORD_CALCUL (float x, float y, float z,float *ANGLE) {
float xx=pow((pow(x,2)+pow(y,2)),0.5);
float r= pow(pow((xx-L1),2)+pow(z,2),0.5);
float alpha3=(atan(z/(xx-L1)))*180/3.14116;
float alpha4=(acos((pow(r,2)+pow(L2,2)-pow(L3,2))/(2*r*L2)))*180/3.14116;
float alpha= alpha3 - alpha4;
float beta1=(acos((pow(L2,2)+pow(L3,2)-pow(r,2))/(2*L2*L3)))*180/3.14116;
float beta =180-beta1;
float gamma=(asin(y/(pow(pow(x,2)+pow(y,2),0.5))))*180/3.14116;
ANGLE [0]= gamma;
ANGLE [1]= alpha;
ANGLE [2]= beta;
}
///////////////////////////////////////////////////////////////////////////////////
// FUNCTION : CALCULATE POSITION MATRIX VERSUS ANGLE, SIGN AND INITIAL POSITION //
///////////////////////////////////////////////////////////////////////////////////
void POSITION_MAT (float ANGLE [3], int SIGN[6][3], int POSITION_0[6][3], int *POSITION[6][3]) {
for(int ii = 0 ; ii < 3 ; ii++)
{
for(int jj = 0 ; jj < 6 ; jj++)
{
int dd = int(( POSITION_0 [jj] [ii] + SIGN [jj] [ii] * (ANGLE [ii]) *2000/180));
Serial.println(dd);
POSITION [jj] [ii]= int(dd);
}
}
}
//////////////////////////////////////////////
// FUNCTION : MOVE EACH DLL IN 0° POSITION //
//////////////////////////////////////////////
void SM_POSITION(int PIN[6][3],int POSITION[6][3], int SM_SPEED ) {
for(int ii = 0 ; ii < 3 ; ii++)
{
for(int jj = 0 ; jj < 6 ; jj++)
{
SERVO((PIN [jj] [ii]),(POSITION [jj] [ii]),SM_SPEED);
}
}
}
//////////////////////////////////////////////
// FUNCTION : SEND A COMMAND TO MOVE SM //
//////////////////////////////////////////////
void SERVO(int servo, int position, int time) {
Serial.print("#");
Serial.print(servo);
Serial.print("P");
Serial.print(position);
Serial.print("T");
Serial.println(time);
delay(time);
} |
Partager