// Libraries for CAN communications #include <can-serial.h> #include <mcp2515_can.h> #include <mcp2515_can_dfs.h> #include <mcp_can.h> #include <Servo.h> #include <math.h> #if defined(SEEED_WIO_TERMINAL) && defined(CAN_2518FD) const int SPI_CS_PIN = BCM8; const int CAN_INT_PIN = BCM25; #else const int SPI_CS_PIN = 9; const int CAN_INT_PIN = 2; #endif #include "mcp2515_can.h" mcp2515_can CAN(SPI_CS_PIN); // Set CS pin #define MAX_DATA_SIZE 8 #include <SPI.h> #define MY_PI 3.14 #define PERIOD_IN_MICROS 10000 #define VEL_MAX 20000 #if !defined(FALSE) #define FALSE 0 #define TRUE 1 #endif // Here change with the motor ID if it has been changed with the firmware. #define MOTOR_MAX_VEL_CMD 300000 #define MOTOR_MAX_VOLTAGE_CMD 200 #define MOTOR_MAX_POS_DEG 120.0 #define MOTOR_MIN_POS_DEG -120.0 Servo myservo; /****************** GLOBAL VARIABLES *********************/ // Global motor state variables int trigPin_f = 4; int echoPin_f = 3; int trigPin_r = 6; int echoPin_r = 5; int broche_pince = 13; int begin_push = A0; int g_int = A1; int in_pince = A3; long duration; double distance_f; double distance_r; double last_distance; double currentMotorPosDeg[3]; double currentMotorVelDegPerSec[3]; double last_currentMotorPosDeg[3]; double previousMotorPosDeg[3]; double currentMotorPosEncoder[3]; double offsetMotorPosEnconder[3]; double currentNumOfMotorRevol[3]; double x = 0; double x_arrivee=0; double y = 0; double zone_a[2] = {-1,-1}; double obj[2] = {-1, -1}; double teta = 0; double d_teta = 0; double rayon = 2.480; double dist_roue = 9.0; double erreur_teta = 0; double teta_cible = 0.0; double vitesse = 0; double v_omega = 0; int GI = 0; int pos = 0; int cas = 1; int test = 0; int test2= 0; int test3 = 0; //ces variables test nous servent à ne passer qu'un nombre choisi de fois dans les if de chaque étape/cas int test4 = 0; int test5 = 0; int test6 = 0; int compteur_balise = 0; //ce compteur sert à éviter le bruit lié au bugg de transmission moteur-CAN à partir de la fin de la 3eme rotation : il faut que le capteur détecte plusieurs fois la variation de distance pour qu'on considère détecter la balise int compteur_recul = 0; //ce compteur nous sert à reculer suffisemment longtemps après avoir reposé le totem pour nous arreter ensuite : pas très propre mais rapide à coder double dist_rep = 0; double dist_rep_f = 0; //distances repères f pour front int x_rep = 0; int y_rep = 0; int cpt = 0; double alpha_prim = 0; double h = 9.1; double dist_roue_pince = 20.36; double alpha0 = 75; double consigne = 8.0; // consigne c'est ce qu'on donne au moteur qui gère la rotation du bras double x0 = 0; // General purpose global variables int counterForPrinting; int printingPeriodicity; unsigned long current_time, old_time, initial_time; void motorON(int motor_ID){ unsigned char msg[MAX_DATA_SIZE] = { 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); } void motorOFF(int motor_ID){ unsigned char msg[MAX_DATA_SIZE] = { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); } /*** This function sends a velocity command. Unit = hundredth of degree per second *****/ void sendVelocityCommand(long int vel, int motor_ID) { if(vel > VEL_MAX) vel = VEL_MAX; if(vel < -VEL_MAX){ vel = -VEL_MAX; } long int local_velocity; local_velocity = vel; unsigned char *adresse_low = (unsigned char *)(&local_velocity); unsigned char msg[MAX_DATA_SIZE] = { 0xA2, 0x00, 0x00, 0x00, *(adresse_low), *(adresse_low + 1), *(adresse_low + 2), *(adresse_low + 3) }; CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); } /*** This function reads the motor states and affects the following global variables *****/ /* previousMotorPosDeg : previous position in Deg (set to the new value, will be "previous" for next call); /* currentNumOfMotorRevol = number of revoutions (old value +/- 1 if one jump has been observed ont the encoder read) /* currentEncodPos = current Encoder Pos value (read) /* currentMotorPosDeg = Motor position in degrees /* currentMotorVelDegPerSec = Motor position velocity in degrees per s ***********************************************************************/ void readMotorState(int motor_ID) { uint32_t id; uint8_t type; uint8_t len; byte cdata[MAX_DATA_SIZE] = { 0 }; int data2_3, data4_5, data6_7; double currentMotorVelRaw[3]; // wait for data //while (CAN_MSGAVAIL != CAN.checkReceive()); unsigned long start = millis(); while (CAN_MSGAVAIL != CAN.checkReceive()) { if (millis() - start > 50) { Serial.print("Timeout: Pas de message CAN reçu sur moteur "); Serial.println(motor_ID); return; } } // read data, len: data length, buf: data buf CAN.readMsgBuf(&len, cdata); id = CAN.getCanId(); type = (CAN.isExtendedFrame() << 0) | (CAN.isRemoteRequest() << 1); // Check if the received ID matches the motor ID if ((id - 0x140) == motor_ID) { data4_5 = cdata[4] + pow(2, 8) * cdata[5]; currentMotorVelRaw[motor_ID - 1] = (double)data4_5; data6_7 = cdata[6] + pow(2, 8) * cdata[7]; currentMotorPosEncoder[motor_ID - 1] = (double)data6_7; } // Conversion of the velocity and writing in the global cariable currentMotorVelDegPerSec[motor_ID - 1] = ((double)(currentMotorVelRaw[motor_ID - 1])); // Conversion of the position (with motor revolution counting) and wirting in the global variable currentMotorPosEncoder[motor_ID - 1] -= offsetMotorPosEnconder[motor_ID - 1]; currentMotorPosDeg[motor_ID - 1] = ((double)(currentNumOfMotorRevol[motor_ID - 1]) * 360.0) + (((double)currentMotorPosEncoder[motor_ID - 1]) * 180.0 / 32768.0); // On convertit en degré if ((currentMotorPosDeg[motor_ID - 1] - previousMotorPosDeg[motor_ID - 1]) < -20.0) { currentNumOfMotorRevol[motor_ID - 1]++; currentMotorPosDeg[motor_ID - 1] = ((double)(currentNumOfMotorRevol[motor_ID - 1])) * 360.0 + ((double)currentMotorPosEncoder[motor_ID - 1]) * 180.0 / 32768.0; } if ((currentMotorPosDeg[motor_ID - 1] - previousMotorPosDeg[motor_ID - 1]) > 20.0) { currentNumOfMotorRevol[motor_ID - 1]--; currentMotorPosDeg[motor_ID - 1] = ((double)(currentNumOfMotorRevol[motor_ID - 1])) * 360.0 + ((double)currentMotorPosEncoder[motor_ID - 1]) * 180.0 / 32768.0; } previousMotorPosDeg[motor_ID - 1] = currentMotorPosDeg[motor_ID - 1]; // writing in the global variable for next call } /********************* THIS FUNCTION IS EXECUTED FIRST AND CONTAINS INITIALIZATION ***********/ void setup() { myservo.attach(8); int i; char serialReceivedChar; int nothingReceived; pinMode(trigPin_f, OUTPUT); // Sets the trigPin as an Output pinMode(echoPin_f, INPUT); pinMode(trigPin_r, OUTPUT); // Sets the trigPin as an Output pinMode(echoPin_r, INPUT); pinMode(begin_push, INPUT); pinMode(g_int, INPUT); // Initialization of the serial link Serial.begin(115200); // Initialization of the CAN communication. THis will wait until the motor is powered on while (CAN_OK != CAN.begin(CAN_500KBPS)) { Serial.println("CAN init fail, retry ..."); delay(500); } Serial.println(""); Serial.println(""); Serial.println(""); Serial.println(""); Serial.println(""); Serial.println("***********************************************************************"); Serial.println("***********************************************************************"); Serial.println("***********************************************************************"); Serial.println(" Serial link and CAN initialization went ok! Power ON the motor"); Serial.println("***********************************************************************"); Serial.println("***********************************************************************"); Serial.println("***********************************************************************"); Serial.println("***********************************************************************"); Serial.println(""); // Initialization of the motor command and the position measurement variables for(int i = 0 ; i < 3; i++){ previousMotorPosDeg[i] = 0.0; currentNumOfMotorRevol[i] = 0; offsetMotorPosEnconder[i] = 0; last_currentMotorPosDeg[i] = 0; } // Send motot off then motor on command to reset motorOFF(1); readMotorState(1); motorOFF(2); readMotorState(2); Serial.println("ok"); motorOFF(3); readMotorState(3); Serial.println("ok"); int begin = 0; while(begin < 900){ begin = analogRead(begin_push); } delay(1000); counterForPrinting = 0; printingPeriodicity = 30; // The variables will be sent to the serial link one out of printingPeriodicity loop runs. current_time = micros(); initial_time=current_time; motorON(1); readMotorState(1); motorON(2); readMotorState(2); motorON(3); readMotorState(3); sendVelocityCommand((long int)(0),1); readMotorState(1); sendVelocityCommand((long int)(0),2); readMotorState(2); sendVelocityCommand((long int)(0),3); readMotorState(3); delay(500); for(int i = 0; i < 3; i++){ offsetMotorPosEnconder[i] = currentMotorPosEncoder[i]; currentNumOfMotorRevol[i] = 0; previousMotorPosDeg[i] = 0.0; } sendVelocityCommand((long int)(0),1); readMotorState(1); sendVelocityCommand((long int)(0),2); readMotorState(2); sendVelocityCommand((long int)(0),3); readMotorState(3); delay(500); Serial.println("End of Initialization routine."); counterForPrinting = 0; printingPeriodicity = 5; // The variables will be sent to the serial link one out of printingPeriodicity loop runs. current_time = micros(); initial_time=current_time; } void pince(){ if(pos == 0){ for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees // in steps of 1 degree myservo.write(pos); // tell servo to go to position in variable 'pos' delay(15); // waits 15 ms for the servo to reach the position } pos = 180; } else{ for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees myservo.write(pos); // tell servo to go to position in variable 'pos' delay(15); // waits 15 ms for the servo to reach the position } pos = 0; } } void loop() { int i; unsigned int sleep_time; double elapsed_time_in_s; int d_capt_roue = 0; old_time=current_time; current_time=micros(); elapsed_time_in_s = (double)(current_time-initial_time); elapsed_time_in_s *= 0.000001; if(v_omega > MY_PI/120){ v_omega = MY_PI/120; } if(v_omega < -MY_PI/120){ v_omega = -MY_PI/120; } sendVelocityCommand(-((1/rayon) * vitesse - (dist_roue/2*rayon) * v_omega) * (180/MY_PI) * 100, 1); readMotorState(1); sendVelocityCommand(((1/rayon) * vitesse + (dist_roue/2*rayon) * v_omega) * (180/MY_PI) * 100, 2); readMotorState(2); double d_d = (-(currentMotorPosDeg[0] - last_currentMotorPosDeg[0])) * (MY_PI / 180) * rayon; //delta de distance avancée par la roue droite entre deux tours de loop double d_g = (currentMotorPosDeg[1] - last_currentMotorPosDeg[1]) * (MY_PI / 180) * rayon; d_teta = (d_d - d_g) / dist_roue; //delta d'angle de l'axe "devant" par rapport aux distances parcouru par les roues en 1 tour de loop y -= ((d_d + d_g) * cos((MY_PI - d_teta) / 2 - teta)) / 2; //coordonnées sur la map (on commence par avancer selon x voir video) x += ((d_d + d_g) * sin((MY_PI - d_teta) / 2 - teta)) / 2; teta += d_teta; erreur_teta = teta - teta_cible; v_omega = 1.0 *erreur_teta; int general_interupt = analogRead(g_int); if(general_interupt > 10000) GI = 1; if(GI == 0){ digitalWrite(trigPin_f, LOW); delayMicroseconds(2); digitalWrite(trigPin_f, HIGH); delayMicroseconds(10); digitalWrite(trigPin_f, LOW); duration = pulseIn(echoPin_f, HIGH); distance_f = (duration * 0.034 / 2) + dist_roue_pince; digitalWrite(trigPin_r, LOW); delayMicroseconds(2); digitalWrite(trigPin_r, HIGH); delayMicroseconds(10); digitalWrite(trigPin_r, LOW); duration = pulseIn(echoPin_r, HIGH); distance_r = (duration * 0.034 / 2) + 10.079; h =(sin((alpha0 - currentMotorPosDeg[2])*MY_PI/180)*15) -5.39; dist_roue_pince = (cos((alpha0 - currentMotorPosDeg[2])*MY_PI/180)*15) + 14.12; sendVelocityCommand(-1000 * (consigne - h), 3); readMotorState(3); for(int i = 0; i < 3; i ++){ last_currentMotorPosDeg[i] = currentMotorPosDeg[i]; } if(cpt == 0){ dist_rep = distance_r; cpt = 300; } //================================================================Serial.println(cas); switch(cas){ case(1): if(test == 0){ for (pos = 180; pos >= 0; pos -= 1) { myservo.write(pos); delay(5); } pos = 0; x = 150 - (distance_f - d_capt_roue); test = 1; } vitesse = 5; //on avance /*Serial.print(" x = "); // verifications des variations de coordonnées Serial.println(x); Serial.print(" y = "); Serial.println(y);*/ if(test2 == 1 && abs(dist_rep - distance_r) >= 2){ // à lire après le if suivant ! test2 = 0; test = 0; cas = 2; zone_a[0] = (zone_a[0] + x) / 2; dist_rep = distance_r; Serial.print("zone_a = "); Serial.println(x); } if(abs(dist_rep - distance_r) > 2){ //detection de balise, dist_rep c'est la valeur de y qu'on a prit au tout début de la ligne droite puis on compare avec les nouvelles test2 = 1; dist_rep = distance_r; zone_a[0] = x; // c'est la valeur de x qu'on va utiliser pour poser le totem } break; case(2): if((teta + MY_PI/2) > 0.01){ //on tourne tant qu'on est pas au teta voulu (cible) vitesse = 0; teta_cible = -MY_PI/2; } else{ if(test == 0){ y = 150 - distance_f; dist_rep = distance_r; //on prend la distance repere au tout début de la ligne droite test = 1; } vitesse = 4; //on avance /*Serial.print(" x = "); Serial.println(x); Serial.print(" y = "); Serial.println(y);*/ if(test2 == 1 && abs(y - y_rep - 1) < 0.01){ //je ne remets pas les commentaires pour les boucles identiques aux cas précédents vitesse = 0; test2 = 0; test = 0; test3 = 0; test4 = 0; test5 = 0; cas = 3; x_rep = x; dist_rep_f = distance_f; obj[1] = (obj[1] + y) / 2; } if(abs(dist_rep - distance_r) > 2){ obj[1] = y; test2 = 1; y_rep = y; } } break; case(3): if (test5==0) { teta=-MY_PI/2; test5=1; //Serial.println("init teta cas 3 ok"); } if((teta + MY_PI) > 0.01){ //tourner de PI/2 teta_cible = -MY_PI; vitesse = 0; } else{ //si c'est tourné if(test == 0){ consigne = 7.0; // on baisse un peu la pince 8->7 obj[0] = x - distance_f; // on note la coordonnee en x du totem test = 1; dist_rep = distance_r; // 1er tour de boucle on prend une dist referente } vitesse = 4; /*Serial.print(" x = "); Serial.println(x); Serial.print(" y = "); Serial.println(y);*/ if((abs(x - obj[0]) < 5 || distance_f < 5 + dist_roue_pince) && test3 == 0){ //si quasi arrivé en coordonnée ou si le capteur détecte limite proche x_rep = x; test3 = 1; } if(test3 == 1 && test4 == 0){ //detecté pour la 1ere fois? alpha_prim = asin((7.0 + 5.39)/15); x0 = (cos(alpha_prim)*15) + 14.12; // dist d'avance horizontale pour descendre au bon endroit vitesse = -4; if(abs(x - x0 - dist_roue_pince) < 0.01){ //si assez reculé consigne = 7.0; //descend la pince vitesse = 0; } if(abs(h -consigne) < 0.1){ // descent la pince vitesse = 0; test2 = 1; test4 = 1; } } else if(test2 == 1 && test4 == 1){ //il avance doucement (à la bonne hauteur) vitesse = 0.5; if(x_rep - x + 3 < 0.1 || distance_f > 200){ // coord ou physique vitesse = 0; pince(); //saisit l'objet consigne = 8.0; //remonte le bras ( en cm) test=0; test2 = 0; test3 = 0; test4 = 0; //zréinitialisation des test pour la boucle d'après test5=0; cas = 4; } } } break; case(4): if (test4==0) { teta=-MY_PI; test4=1; //Serial.println("init teta ok"); } if((teta + (1.5*MY_PI)) < -0.01){ //il tourne à 3PI/2 vitesse = 0; teta_cible = -1.5*MY_PI; //Serial.println(teta+(1.5*MY_PI)); //Serial.println("trop tourné"); test=0; } if((teta + (1.5*MY_PI)) > 0.01){ // tourne vitesse = 0; teta_cible = -1.5*MY_PI; //Serial.println(teta+(1.5*MY_PI)); test=0; } else { //si t'as tourné if(test == 0){ //Serial.println("j'avance"); dist_rep = distance_r; // dist fixe pour comparaison balise test = 1; //test pour pas retomber dans la boucle } vitesse = 4; if((test2 == 1)&&(compteur_balise==50)){ //déja expliqué dans les initialisations vitesse = 0; test = 0; cas = 5; compteur_balise = 0; } if(abs(dist_rep - distance_r) > 2){ //Serial.println("balise detectée"); compteur_balise = compteur_balise + 1; zone_a[1] = y; test2 = 1; y_rep = y; } } break; case(5): if(test==0) { teta = -1.5*MY_PI; test2 = 0; test3 = 0; test4 = 0; test5 = 0; test = 1; //Serial.println("init cas5 ok"); } if((test5==1) && (abs(teta) > 0.01)) { //tourne, vitesse = 0; teta_cible = 0; //Serial.print("correction delta teta = "); //Serial.println(teta); } if((test5==0) && (teta > -2*MY_PI)) { //on veut qu'il tourne à teta=-2PI pour que ça refasse teta=0 vitesse = 0; teta_cible = -2*MY_PI; //Serial.print("virage cas5, delta teta = "); //Serial.println(teta); } else{ test5=1; if (test6 ==0) { vitesse = 3; /*Serial.println("j'avance cas5"); Serial.print(" x = "); Serial.println(x); Serial.print(" y = "); Serial.println(y);*/ } //Serial.print("delta x = "); //Serial.println(abs(x - zone_a[0])); if((abs(zone_a[0] - x) < 0.1) && test4 == 0 && test3==0){ //arrivé en x vitesse = 0; test4 = 1; test2 = 1; } if(test2 == 1 && test3 == 0){ //au bon endroit + premiere fois alpha_prim = asin((6.5 + 5.39)/15); x0 = (cos(alpha_prim)*15) + 14.12; //calcul du x0 pour la descente vitesse = -4; if(x_rep - x - 3 < 0.1){ //descend le bras consigne = 7; vitesse = 0; } if((abs(h -consigne) < 0.1)&& (test3==0)) { vitesse = 0; test3 = 1; pince(); //on lache le totem vitesse = -0.5; test6=1; } if(test3 == 1){ vitesse = 0; consigne = 7; cas = 6; } } } break; case(6): if (compteur_recul<500) { compteur_recul = compteur_recul + 1; //il recule pour ne pas s'arreter avec le totem entre la pince écartée vitesse = -0.5; } else { vitesse = 0; //il s'arrete v_omega = 0; } } } else{ vitesse = 0; v_omega = 0; motorOFF(1); readMotorState(1); motorOFF(2); readMotorState(2); motorOFF(3); readMotorState(3); } if(teta > 2*MY_PI){ teta -= 2* M_PI; } if(teta < -2*MY_PI) { teta += 2* M_PI; } sleep_time = PERIOD_IN_MICROS-((micros()-current_time)); if ( (sleep_time >0) && (sleep_time < PERIOD_IN_MICROS) ) { delayMicroseconds(sleep_time); } cpt--; }