Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <SoftwareSerial.h>
- // Constantes
- #define SERIAL_START_FLAG 0x7E
- #define SERIAL_END_FLAG 0x24
- #define MAX_SERIAL_MESSAGE_SIZE 10
- #define SERIAL_SENDING_INTERVAL 50
- #define UPDATE_INTERVAL 50
- #define DISTANCE_SECURITE 100
- #define NB_LECTURE 12
- #define NB_MEDIAN 2
- // Serial pin pour le controleur moteur
- #define TXPIN_POLOLU 4
- #define RXPIN_POLOLU 5
- // Serial pin pour le controleur servo
- #define TXPIN_MAESTRO 6
- #define RXPIN_MAESTRO 7
- // Canal des servos
- #define HONRIZONTAL 1
- #define VERTICAL 0
- #define pingSensor A3 //Ping sensor
- int pingSensorCm;
- #define leftSensorIr A1 //IR sensor gauche
- float leftSensorValue;
- int leftSensorCm;
- #define centerSensorIr A0 //IR sensor milieu
- float centerSensorValue;
- int centerSensorCm;
- #define rightSensorIr A2 //IR sensor droit
- float rightSensorValue;
- int rightSensorCm;
- // Créé une instance du software serial
- // Interface avec le controleur moteur Pololu TReX
- SoftwareSerial pololu(RXPIN_POLOLU, TXPIN_POLOLU);
- // Créé une instance du software serial
- // Interface avec le controleur servo Pololu Micro maestro
- SoftwareSerial maestro(RXPIN_MAESTRO, TXPIN_MAESTRO);
- /**********************
- * VARIABLES GLOBALES *
- **********************/
- // Lettres pour la communication avec JAVA: O / F / B / L / R / A / M / W / V
- unsigned long previousMillis = 0; //Permet de stocker le moment du précédent update
- int frameInnerCounter=0; //Permet de connaitre le nombre de vraies frames dans un message
- boolean readFrame=false; //Flag indiquant la lecture des vraies frames
- int sizeOfData=0; //Nombre de vraies frames dans le message
- int serialData[MAX_SERIAL_MESSAGE_SIZE]; //Stockage des données
- int serialFlag=0; //Flag permettant de savoir s'il y a un message complet à traiter
- unsigned long previousMillisSerial = 0; //To store last time data was sent
- char motorDirection = 'O'; //Direction des moteurs
- char motorDirectionPrec = 'O'; //Direction des moteurs précédent
- int moteurValue = 0; //Valeur des moteurs
- int robotMode = 0; //Mode du robot: 0 pour manuel, 1 pour automatique
- /***************************
- * DECLARATION DES FONCTIONS
- ***************************/
- int tenpow(int x);
- int getSmoothIRSensor(int sensorpin, char character);
- int irValueToCm(float x);
- int getPingSensor();
- //Fonction permettant de catcher un message reçu via le port série
- void handleMessage(int serialData[]);
- void setup() {
- // On lance la communication avec l'interface JAVA
- Serial.begin(115200);
- // Input-output pin's pour le TReX
- pinMode(RXPIN_POLOLU, INPUT);
- pinMode(TXPIN_POLOLU, OUTPUT);
- // On lance la communication avec l'interface pololu TreX
- pololu.begin(19200);
- // Input-output pin's pour le Micro maestro
- pinMode(RXPIN_MAESTRO, INPUT);
- pinMode(TXPIN_MAESTRO, OUTPUT);
- // On lance la communication avec l'interface pololu Micro Maestro
- maestro.begin(115200);
- delay(1000);
- // On met la tourelle dans sa position initiale
- settarget(VERTICAL, 60);
- settarget(HONRIZONTAL, 90);
- delay(100);
- goHome();
- pinMode(leftSensorIr, INPUT);
- digitalWrite(leftSensorIr, LOW);
- pinMode(centerSensorIr, INPUT);
- digitalWrite(centerSensorIr, LOW);
- pinMode(rightSensorIr, INPUT);
- digitalWrite(rightSensorIr, LOW);
- // prescale clock à 16 pour les pins analogiques
- bitClear(ADCSRA,ADPS0);
- bitClear(ADCSRA,ADPS1);
- bitSet(ADCSRA,ADPS2);
- }
- void loop()
- {
- unsigned long currentMillis;
- unsigned long currentMillisSerial;
- //On récupère les frames d'un message tant que la var serialFlag n'est pas à 1
- if(Serial.available()>0){
- getSerialMessage();
- }
- //Si il y a un message complet à traiter, on le traite
- if(serialFlag==1){
- handleMessage(serialData);
- }
- //******************************************
- // Algorithme pour l'évitement d'obstacles *
- //******************************************
- rightSensorCm = getSmoothIRSensor(rightSensorIr, 'Z');
- centerSensorCm = getSmoothIRSensor(centerSensorIr, 'Y');
- leftSensorCm = getSmoothIRSensor(leftSensorIr, 'X');
- // Si robotMode est en manuel, on met à jour la valeur des moteurs avec la valeur recue via serial
- if (robotMode == 0) {
- goMotors(motorDirection,moteurValue);
- motorDirectionPrec = motorDirection;
- //Si mode automatique
- }
- else if (robotMode == 1) {
- // Tant qu'on est sup à la distance de sécurité
- if (rightSensorCm>DISTANCE_SECURITE && centerSensorCm>DISTANCE_SECURITE && leftSensorCm>DISTANCE_SECURITE) {
- // On avance
- goMotors('F',0127);
- motorDirectionPrec = 'F';
- }
- else {
- // Sinon on arrête le robot
- goMotors('O',0000);
- motorDirectionPrec = 'O';
- // On recule
- goMotors('B',127);
- motorDirectionPrec = 'B';
- delay(400);
- // On arrête
- goMotors('O',0000);
- motorDirectionPrec = 'O';
- // Si obstacle à gauche, on tourne à droite
- if (leftSensorCm<centerSensorCm && leftSensorCm<rightSensorCm){
- sendValue('V',0001);
- goMotors('R',0127);
- motorDirectionPrec = 'R';
- delay(600);
- }
- // Si obstacle au milieu, on fait demi tour
- if (centerSensorCm<leftSensorCm && centerSensorCm<rightSensorCm){
- sendValue('V',0002);
- goMotors('L',0127);
- motorDirectionPrec = 'L';
- delay(1200);
- }
- // Si obstacle à droite, on tourne à gauche
- if (rightSensorCm<centerSensorCm && rightSensorCm<leftSensorCm){
- sendValue('V',0003);
- goMotors('L',0127);
- motorDirectionPrec = 'L';
- delay(600);
- }
- // On arrête
- goMotors('O',0000);
- motorDirectionPrec = 'O';
- delay(2500);
- // On prépare et commence le scan
- sendValue('V',0000);
- settarget(VERTICAL, 60);
- settarget(HONRIZONTAL, 40);
- delay(100);
- int i=40;
- // On scanne
- while(i<=140)
- {
- settarget(HONRIZONTAL, i);
- pingSensorCm = getPingSensor();
- delay(30);
- i++;
- }
- // On repositionne la tourelle
- settarget(VERTICAL, 60);
- settarget(HONRIZONTAL, 90);
- delay(100);
- goHome();
- }
- }
- //*****************************************
- // Si on veut faire un update des données selon un interval donné
- currentMillis = millis();
- if(currentMillis - previousMillis > UPDATE_INTERVAL) {
- previousMillis = currentMillis;
- }
- // On peut renvoyer des données selon un interval
- if(currentMillis - previousMillisSerial > SERIAL_SENDING_INTERVAL) {
- previousMillisSerial = currentMillis;
- sendValue(motorDirection,moteurValue);
- sendValue('Z',rightSensorCm);
- sendValue('Y',centerSensorCm);
- sendValue('X',leftSensorCm);
- }
- }/*---- FIN DE LA BOUCLE -----*/
- /**
- * sendValue : Fonction permettant d'envoyer une valeur avec un flag donné vers le port série
- */
- void sendValue(char command,int value){
- uint8_t* message;
- message=(uint8_t*)malloc(10*sizeof(uint8_t));
- message[0]=SERIAL_START_FLAG;
- int milA=value/1000;
- int centA=(value-milA*1000)/100;
- int dizA=(value-milA*1000-centA*100)/10;
- int unitA=value-milA*1000-centA*100-dizA*10;
- message[1]=command;
- message[2]=milA+'0';
- message[3]=centA+'0';
- message[4]=dizA+'0';
- message[5]=unitA+'0';
- message[6]=SERIAL_END_FLAG;
- message[7]='\n';
- Serial.write(message,8);
- free(message);
- }
- /**
- * tenpow: Fonction pemrmettant de recomposer un int à partir du message sérialisé
- */
- int tenpow(int x) {
- switch(x){
- case 3:
- return 1;
- break;
- case 2 :
- return 10;
- break;
- case 1 :
- return 100;
- break;
- case 0 :
- return 1000;
- }
- }
- /**
- *Fontion permettant de traiter un message reçu via le port série.
- *La fontion prend en entrée la var serialData
- */
- void handleMessage(int serialData[]){
- serialFlag=0;
- //On récupére le caractère indiquant l'élement à évaluer
- char command=serialData[1];
- int value=0;
- //On traduit la valeur depuis ascii little endian vers un simple int
- for(int j=2;j<sizeOfData-1;j++){
- value=value + (serialData[j]-'0')*tenpow(j-2);
- }
- // On réalise l'opération en fonction du caractère
- switch (command){
- case 'L':
- motorDirection = 'L';
- moteurValue=value;
- break;
- case 'R':
- motorDirection = 'R';
- moteurValue=value;
- break;
- case 'F':
- motorDirection = 'F';
- moteurValue=value;
- break;
- case 'B':
- motorDirection = 'B';
- moteurValue=value;
- break;
- case 'O':
- motorDirection = 'O';
- moteurValue=0;
- break;
- case 'A':
- robotMode = 1;
- break;
- case 'M':
- robotMode = 0;
- break;
- }
- }
- /**
- * Fonction permettant de récuperer un message depuis le port série
- */
- void getSerialMessage(void){
- while(Serial.available()>0){
- int reading=Serial.read();
- /* S'il s'agit de la 1ére frame 0x7E (frame de début), on réinitialise à 0 la var frameInnerCounter
- * On positionne la var readFrame à true
- */
- if(reading==SERIAL_START_FLAG){
- frameInnerCounter=0;
- serialData[frameInnerCounter]=reading;
- readFrame=true;
- }
- /* Si on a déjà passé la frame de début on
- * incrémente la var frameInnerCounter et on sauvegarde les données
- */
- else if(readFrame==true){
- frameInnerCounter++;
- serialData[frameInnerCounter]=reading;
- /* S'il s'agit de la dernière frame 0x24(frame de fin),
- * On positionne la var readFrame à false
- * On positionne la var serialFlag à 1 pour indiquer qu'il y a un message à traiter
- * On stock le nombre de frame réelle à traiter dans la var sizeOfData
- */
- if(reading==SERIAL_END_FLAG) {
- readFrame=false;
- serialFlag=1;
- sizeOfData=frameInnerCounter+1;
- }
- }
- }
- }
- /**
- * Permet de convertir la valeur des sensors IR en CM
- */
- int irValueToCm(int sensorValue)
- {
- int cm;
- cm = 10650.08 * pow(sensorValue,-0.935) - 10;
- if (cm>200||cm==0) {
- cm = 200;
- }
- return cm;
- }
- /**
- * Permet de convertir une durée en cm (pour capteur ping)
- */
- unsigned long Convert_Time_Space(const unsigned long fnDuration ) {
- // 29 microseconds par cm.
- return fnDuration / 29 / 2 ;
- }
- /**
- * Récupère une valeur lissé d'un capteur IR en effectuant plusieurs tirs
- */
- int getSmoothIRSensor(int sensorpin, char character){
- switch (character){
- case 'X':
- digitalWrite(leftSensorIr, HIGH);
- break;
- case 'Y':
- digitalWrite(centerSensorIr, HIGH);
- break;
- case 'Z':
- digitalWrite(rightSensorIr, HIGH);
- break;
- }
- delay(5);
- int returnval = 0;
- returnval = irValueToCm(digitalRead(sensorpin));
- digitalWrite(leftSensorIr, LOW);
- digitalWrite(centerSensorIr, LOW);
- digitalWrite(rightSensorIr, LOW);
- int sortedValues[NB_LECTURE];
- for(int i=0;i<NB_LECTURE;i++){
- int value = irValueToCm(analogRead(sensorpin));
- int j;
- if(value<sortedValues[0] || i==0){
- j=0; //insert at first position
- }else{
- for(j=1;j<i;j++){
- if(sortedValues[j-1]<=value && sortedValues[j]>=value){
- // j is insert position
- break;
- }
- }
- }
- for(int k=i;k>j;k--){
- // move all values higher than current reading up one position
- sortedValues[k]=sortedValues[k-1];
- }
- sortedValues[j]=value; //insert current reading
- }
- //return scaled mode of NB_MEDIAN*2 values
- for(int i=NB_LECTURE/2-NB_MEDIAN;i<(NB_LECTURE/2+NB_MEDIAN);i++){
- returnval +=sortedValues[i];
- }
- returnval = returnval/(NB_MEDIAN*2);
- return returnval;
- }
- /**
- * Récupération de la valeur du capteur ping
- */
- int getPingSensor()
- {
- int pingSensorValue;
- pinMode(pingSensor, OUTPUT);
- digitalWrite(pingSensor, LOW); // init sensor to ensure clean HIGH pulse
- delayMicroseconds(2);
- digitalWrite(pingSensor, HIGH); // make the sensor send a pulse
- delayMicroseconds(5);
- digitalWrite(pingSensor, LOW); // Set LOW again
- pinMode(pingSensor, INPUT); // Get ready to capture the duration of the resulting pulse
- // Capture how long the pin stays in HIGH state.
- unsigned long duration = pulseIn(pingSensor, HIGH);
- pingSensorValue = Convert_Time_Space(duration);
- sendValue('W',pingSensorValue);
- return pingSensorValue;
- }
- /**
- * Envoi une commande Set Target au Maestro.
- * La target est en quart de microseconds.
- */
- void settarget(unsigned char servo, unsigned int target)
- {
- target = map(target, 0, 180, 880*4, 2320*4);
- maestro.write(0xAA); //start byte
- maestro.write(0x0C) ; //device id
- maestro.write(0x04); //command number
- maestro.write(servo); //servo number
- maestro.write(target & 0x7F);
- maestro.write((target >> 7) & 0x7F);
- }
- /**
- * Envoi une commande goHome au Maestro.
- * Permet de ne plus envoyer de commande au maestro
- * et donc d'éteindre tous les servos
- */
- void goHome()
- {
- maestro.write(0xAA); //start byte
- maestro.write(0x0C) ; //device id
- maestro.write(0x22); //go home command number
- }
- // Set the motor index, direction, and speed
- // Motor index should either be a 0 or 1
- // Direction should be either true for forward or false for backwards
- // Speed should range between 0 and 127 (inclusivly)
- void SetSpeed(int MotorIndex, boolean Forward, int Speed)
- {
- // Validate motor index
- if(MotorIndex < 0 || MotorIndex > 1)
- return;
- // Validate speed
- if(Speed < 0)
- Speed = 0;
- else if(Speed > 127)
- Speed = 127;
- // Send the "set" command based on the motor
- // Note that we do not accelerate to the
- // speed, we just instantly set it
- unsigned char SendByte = 0;
- if(MotorIndex == 0)
- SendByte = 0xC2;
- else if(MotorIndex == 1)
- SendByte = 0xCA;
- // If we go backwards, the commands are the same
- // but minus one
- if(!Forward)
- SendByte--;
- // Send the set speed command byte
- pololu.write(SendByte);
- // Send the speed data byte
- pololu.write(Speed);
- }
- //On indique au robot d'avancer
- void goForward(int Speed)
- {
- SetSpeed(0, false, Speed);
- SetSpeed(1, false, Speed);
- }
- //On indique au robot de reculer
- void goBackward(int Speed)
- {
- SetSpeed(0, true, Speed);
- SetSpeed(1, true, Speed);
- }
- //On indique au robot d'aller à gauche
- void goGauche(int Speed)
- {
- SetSpeed(0, false, Speed);
- SetSpeed(1, true, Speed);
- }
- //On indique au robot d'aller à droite
- void goDroite(int Speed)
- {
- SetSpeed(0, true, Speed);
- SetSpeed(1, false, Speed);
- }
- //Gestion des moteurs
- void goMotors(char Direction, int Speed)
- {
- switch (Direction){
- case 'O':
- switch (motorDirectionPrec){
- case 'F':
- goBackward(Speed);
- break;
- case 'B':
- goForward(Speed);
- break;
- case 'L':
- goDroite(Speed);
- break;
- case 'R':
- goGauche(Speed);
- break;
- }
- break;
- case 'L':
- goGauche(Speed);
- break;
- case 'R':
- goDroite(Speed);
- break;
- case 'F':
- goForward(Speed);
- break;
- case 'B':
- goBackward(Speed);
- break;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement