/*****************************************************************************/ /**************** AUTOMATIC CONTROL - POLYTECH SORBONE - JAN 2025 ************/ /*********************** AUTHORS : L. BRIETZKE & G. MOREL ********************/ /************************* Sequence 1 - Problem 1 ****************************/ /**************************** YOU'RE IN CONTROL *******************************/ /*****************************************************************************/ /** * @file robot_control.ino * @brief Automatic robot control sequence for obstacle pickup and drop. */ // Libraries for CAN communications #include <can-serial.h> #include <mcp2515_can.h> #include <mcp2515_can_dfs.h> #include <mcp_can.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 = 10; const int CAN_INT_PIN = 2; #endif #include <Servo.h> Servo monservo; #include "mcp2515_can.h" mcp2515_can CAN(SPI_CS_PIN); // Set CS pin #define MAX_DATA_SIZE 8 #include <SPI.h> #include <math.h> #define MY_PI 3.14159265359 #define PERIOD_IN_MICROS 5000 // 5 ms #if !defined(FALSE) #define FALSE 0 #define TRUE 1 #endif #define MOTOR_MAX_VEL_CMD 300000 #define MOTOR_MAX_VOLTAGE_CMD 200 // Capteurs ultrasons (2: capteur avant) const int trig_pin = 6; const int echo_pin = 7; const int trig_pin_2 = 4; const int echo_pin_2= 5; float timingD = 0.0; float timingP = 0; // Enums de mouvements et d'étapes enum Mouvement { ARRETER, AvanceAvecUneDistanceCstAuMur, tournerG, RecupObs, PoseObs, Detectobs, reculer }; enum Etape { etap0, etap1, etap2, etap3, etap4, etap5, etap6, etap7, etap8, etap9, etap10, etap11, etap12, etap13, etap14, etap15, etap16, etap17, etap18, etap19, etap20, etap21, etap22, etap23, etap24, etap25, etap26 }; Mouvement Mouv = ARRETER; int MesureDistanceD = 20; // valeur du capteur droit int DistanceD = MesureDistanceD; int Condition = etap0; int MesureDistanceP = 0; /****************** GLOBAL VARIABLES *********************/ // Global motor state variables double currentMotorPosDeg[3]; double currentMotorVelDegPerSec[3]; double previousMotorPosDeg[3]; int currentMotorPosEncoder[3]; int offsetMotorPosEncoder[3]; int currentNumOfMotorRevol[3]; // Variables globales double motor1VelocityCommandInDegPerSec; // commande en vitesse moteur1 double motor2VelocityCommandInDegPerSec; // commande en vitesse moteur2 double Consignevitesse; // change la vitesse du ro sur tout le circuit // General purpose global variables int counterForPrinting; int printingPeriodicity; unsigned long current_time, old_time, initial_time; /** * @brief Allume le moteur. * @param motor_ID ID du moteur */ 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); } /** * @brief Éteint le moteur. * @param motor_ID ID du moteur */ void motorOFF(int motor_ID){ unsigned char msg[MAX_DATA_SIZE] = { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; Serial.print("Motor ID: "); Serial.println(motor_ID); CAN.sendMsgBuf(0x140+motor_ID, 0, 8, msg); } /** * @brief Envoie une commande de vitesse. * @param vel Vitesse en centièmes de degré/s * @param motor_ID ID du moteur */ void sendVelocityCommand(long int vel, int motor_ID) { 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); } /** * @brief Lit l'état du moteur et met à jour les variables globales. * @param motor_ID ID du moteur */ 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; int currentMotorVelRaw; int i; // wait for data while (CAN_MSGAVAIL != CAN.checkReceive()); // 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 = (int)data4_5; data6_7 = cdata[6] + pow(2, 8) * cdata[7]; currentMotorPosEncoder[i] = (int)data6_7; } // Conversion of the velocity and writing in the global cariable currentMotorVelDegPerSec[i] = ((double)(currentMotorVelRaw)); // Conversion of the position (with motor revolution counting) and wirting in the global variable int relativeEncoder = currentMotorPosEncoder[i] - offsetMotorPosEncoder[i]; currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])*360.0) + (((double)relativeEncoder) * 180.0 / 32768.0); // On convertit en degré if ((currentMotorPosDeg[i] - previousMotorPosDeg[i]) < -20.0) { currentNumOfMotorRevol[i]++; currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])) * 360.0 + ((double)relativeEncoder) * 180.0 / 32768.0; } if ((currentMotorPosDeg[i] - previousMotorPosDeg[i]) > 20.0) { currentNumOfMotorRevol[i]--; currentMotorPosDeg[i] = ((double)(currentNumOfMotorRevol[i])) * 360.0 + ((double)relativeEncoder) * 180.0 / 32768.0; } previousMotorPosDeg[i] = currentMotorPosDeg[i]; // writing in the global variable for next call } /** * @brief Fonction d'initialisation. */ void setup() { int i; char serialReceivedChar; int nothingReceived; // Initialization of the serial link Serial.begin(115200); // Initialization of the analog input pins // 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; offsetMotorPosEncoder[i] = 0; } // Send motot off then motor on command to reset motorOFF(1); delay(500); readMotorState(1); motorOFF(2); delay(500); readMotorState(2); motorOFF(3); delay(500); readMotorState(3); Serial.println(""); Serial.println("***********************************************************************"); Serial.println(" Turn the rotor in its ZERO position they type 'S'"); Serial.println("***********************************************************************"); nothingReceived = TRUE; while (nothingReceived==TRUE){ serialReceivedChar = Serial.read(); if(serialReceivedChar == 'S') { nothingReceived = FALSE; } } monservo.attach(3); motorON(1); readMotorState(1); delay(500); motorON(2); readMotorState(2); delay(500); motorON(3); readMotorState(3); delay(500); sendVelocityCommand((long int)(0),1); //delay(500); readMotorState(1); sendVelocityCommand((long int)(0),2); //delay(500); readMotorState(2); sendVelocityCommand((long int)(0),3); //delay(500); readMotorState(3); pinMode(echo_pin, INPUT); // Initialisation des capteurs Devant et Pince pinMode(trig_pin, OUTPUT); digitalWrite(trig_pin, LOW); pinMode(echo_pin_2, INPUT); pinMode(trig_pin_2, OUTPUT); digitalWrite(trig_pin_2, LOW); Serial.println("End of Initialization routine."); counterForPrinting = 0; printingPeriodicity = 10; // The variables will be sent to the serial link one out of printingPeriodicity loop runs. current_time = micros(); initial_time=current_time; } /** * @brief Boucle principale du robot. */ void loop() { delay(500); int i; //capteurs digitalWrite(trig_pin, LOW); delay(2); digitalWrite(trig_pin, HIGH); delay(10); digitalWrite(trig_pin, LOW); timingD = pulseIn(echo_pin, HIGH); digitalWrite(trig_pin_2, LOW); delay(2); digitalWrite(trig_pin_2, HIGH); delay(10); digitalWrite(trig_pin_2, LOW); timingP = pulseIn(echo_pin_2, HIGH); //CALCUL DISTANCES MesureDistanceD = (timingD * 0.034) / 2; MesureDistanceP = (timingP * 0.034) / 2; // Mesure distanceD if (MesureDistanceD > 300){ MesureDistanceD = 300; } if (MesureDistanceP > 300){ MesureDistanceP = 300; } Serial.print("MesureDistanceD "); Serial.println(MesureDistanceD); Serial.print("MesureDistanceP "); Serial.println(MesureDistanceP); Serial.print("DistanceD "); Serial.println(DistanceD); // Variable modifiable int ConsigneVitesse = 6000; //distance avec l'obstacle au moment se s'arreter pour le prendre à tester // Erreur float Erreur = DistanceD - MesureDistanceD ; /*if (Erreur > 10 || Erreur < -10 ){ Erreur = 1; Serial.println("ERREUR ABERRANTE"); }*/ switch (Mouv) { case AvanceAvecUneDistanceCstAuMur: motor1VelocityCommandInDegPerSec = ConsigneVitesse ; motor2VelocityCommandInDegPerSec = -ConsigneVitesse ; sendVelocityCommand(motor1VelocityCommandInDegPerSec, 1); readMotorState(1); sendVelocityCommand(motor2VelocityCommandInDegPerSec, 2); readMotorState(2); break; case reculer: motor1VelocityCommandInDegPerSec = -ConsigneVitesse ; motor2VelocityCommandInDegPerSec = ConsigneVitesse ; sendVelocityCommand(motor1VelocityCommandInDegPerSec, 1); readMotorState(1); sendVelocityCommand(motor2VelocityCommandInDegPerSec, 2); readMotorState(2); break; case tournerG: sendVelocityCommand(-3000,1); readMotorState(1); sendVelocityCommand(-3000,2); readMotorState(2); delay(12700); // temps necessaire pour tourner de 90 degrés break; case RecupObs: // permet de récuperer l'obstacle //attendre que la pince se place monservo.write(0); sendVelocityCommand(4000, 3); // placement a la bonne hauteur delay(200); readMotorState(3); delay(2000); // temps a travailler pour que le pince soit bien placer // Avancer sendVelocityCommand(2000, 1); delay(20); readMotorState(1); sendVelocityCommand(-2000, 2); delay(20); readMotorState(2); delay(7000); // attendre que le robot soit placer deva,t la pince monservo.write(84); // attraper l'objet en avancer doucement delay(100); sendVelocityCommand(-3000 , 3); delay(20); readMotorState(3); break; case Detectobs: sendVelocityCommand(-1000 , 3); // delay(200); readMotorState(3); break; case PoseObs: sendVelocityCommand(5000 , 3); // redescender la pince delay(20); readMotorState(3); delay(3000); // arret la pince en position basse monservo.write(0); // lacher l'objet en avancer doucement break; case ARRETER: sendVelocityCommand(0, 1); delay(200); readMotorState(1); sendVelocityCommand(0, 2); delay(200); readMotorState(2); break; } Serial.print("Condition : "); Serial.println(Condition); // Condition switch (Condition){ case etap0: // On attent un peu Mouv=ARRETER; Condition = etap1; break; case etap1: // On récupère la distance avec le mur DistanceD = MesureDistanceD; Condition = etap2; break; case etap2: // On avance avec un distance cst avec le mur jusqu'à se rapprocher du mur Mouv = AvanceAvecUneDistanceCstAuMur; if (MesureDistanceP < 15){ Condition = etap3; } break; case etap3: // On tourne à gauche delay(500); Mouv = tournerG; Condition = etap4; break; case etap4: // On attent un peu Mouv=ARRETER; Condition = etap5; break; case etap5: // On récupère la distance avec le mur DistanceD = MesureDistanceD; Condition = etap6; break; case etap6: // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle) Mouv = AvanceAvecUneDistanceCstAuMur; if (Erreur > 2 || Erreur < -2){ Condition = etap7; } break; case etap7: // On tourne à gauche delay(3300); Mouv = tournerG; Condition = etap8; break; case etap8: // On attent un peu Mouv=ARRETER; Condition = etap9; break; case etap9: // On récupère la distance avec le mur DistanceD = MesureDistanceD; Condition = etap10; break; case etap10: // On avance avec un distance cst avec le mur jusqu'à se rapprocher du mur Mouv = AvanceAvecUneDistanceCstAuMur; if (MesureDistanceP < 13 ){ Mouv = ARRETER; Condition = etap11; } break; case etap11: // on a est devant l'obj mtn, on le recup Mouv = Detectobs; if (MesureDistanceP > 18){ // ajuster la valeur de détection pour passer à l'étape de récuperation de l'obj Condition = etap12; } break; case etap12: // On récupère l'obj Mouv = RecupObs; Condition = etap13; break; case etap13: // On tourne à gauche Mouv = tournerG; Condition = etap14; break; case etap14: // On attent un peu Mouv=ARRETER; Condition = etap15; break; case etap15: // On récupère la distance avec le mur delay(1500); DistanceD = MesureDistanceD; Condition = etap16; break; case etap16: // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle) Mouv = AvanceAvecUneDistanceCstAuMur; if (Erreur > 1 || Erreur < -1 ){ Condition = etap17; } break; case etap17: // On tourne à gauche delay(2800); Mouv = tournerG; Condition = etap18; break; case etap18: // On attent un peu Mouv=ARRETER; Condition = etap19; break; case etap19: // On récupère la distance avec le mur DistanceD = MesureDistanceD; Condition = etap20; break; case etap20: // On avance avec un distance cst avec le mur jusqu'à voir une erreur (les 3cm de l'obstacle) Mouv = AvanceAvecUneDistanceCstAuMur; if (Erreur > 2 || Erreur < -2){ // On est au niveau de là où on doit poser l'obj Condition = etap21; } break; case etap21: // On attent un peu Mouv = ARRETER; Condition = etap22; break; case etap22: Mouv = reculer; Condition =etap23; break; case etap23: // On attent un peu delay(10000); Mouv = ARRETER; Condition = etap24; break; case etap24: // On pose l'obj Mouv = PoseObs; Condition = etap25; break; case etap25: Mouv = reculer; Condition = etap26; break; case etap26: // On attent un peu pour montrer qu'on est content delay(5000); Mouv = ARRETER; break; } }