daily pastebin goal
53%
SHARE
TWEET

DarylRobotProject Serial arduino java

a guest Sep 26th, 2014 596 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <SoftwareSerial.h>
  2.  
  3. // Constantes
  4. #define SERIAL_START_FLAG 0x7E
  5. #define SERIAL_END_FLAG 0x24
  6. #define MAX_SERIAL_MESSAGE_SIZE 10
  7. #define SERIAL_SENDING_INTERVAL 50
  8. #define UPDATE_INTERVAL 50
  9. #define DISTANCE_SECURITE 100
  10. #define NB_LECTURE 12
  11. #define NB_MEDIAN 2
  12.  
  13. // Serial pin pour le controleur moteur
  14. #define TXPIN_POLOLU 4
  15. #define RXPIN_POLOLU 5
  16.  
  17. // Serial pin pour le controleur servo
  18. #define TXPIN_MAESTRO 6
  19. #define RXPIN_MAESTRO 7
  20.  
  21. // Canal des servos
  22. #define HONRIZONTAL 1
  23. #define VERTICAL 0
  24.  
  25. #define pingSensor A3 //Ping sensor
  26. int pingSensorCm;
  27.  
  28. #define leftSensorIr A1 //IR sensor gauche
  29. float leftSensorValue;
  30. int leftSensorCm;
  31.  
  32. #define centerSensorIr A0 //IR sensor milieu
  33. float centerSensorValue;
  34. int centerSensorCm;
  35.  
  36. #define rightSensorIr A2 //IR sensor droit
  37. float rightSensorValue;
  38. int rightSensorCm;
  39.  
  40. // Créé une instance du software serial
  41. // Interface avec le controleur moteur Pololu TReX
  42. SoftwareSerial pololu(RXPIN_POLOLU, TXPIN_POLOLU);
  43.  
  44. // Créé une instance du software serial
  45. // Interface avec le controleur servo Pololu Micro maestro
  46. SoftwareSerial maestro(RXPIN_MAESTRO, TXPIN_MAESTRO);
  47.  
  48. /**********************
  49.  * VARIABLES GLOBALES *
  50.  **********************/
  51. // Lettres pour la communication avec JAVA: O / F / B / L / R / A / M / W / V
  52.  
  53. unsigned long previousMillis = 0; //Permet de stocker le moment du précédent update
  54. int frameInnerCounter=0; //Permet de connaitre le nombre de vraies frames dans un message
  55. boolean readFrame=false; //Flag indiquant la lecture des vraies frames
  56. int sizeOfData=0; //Nombre de vraies frames dans le message
  57. int serialData[MAX_SERIAL_MESSAGE_SIZE]; //Stockage des données
  58. int serialFlag=0; //Flag permettant de savoir s'il y a un message complet à traiter
  59. unsigned long previousMillisSerial = 0; //To store last time data was sent
  60. char motorDirection = 'O'; //Direction des moteurs
  61. char motorDirectionPrec = 'O'; //Direction des moteurs précédent
  62. int moteurValue = 0; //Valeur des moteurs
  63. int robotMode = 0; //Mode du robot: 0 pour manuel, 1 pour automatique
  64.  
  65. /***************************
  66.  * DECLARATION DES FONCTIONS
  67.  ***************************/
  68.  
  69. int tenpow(int x);
  70. int getSmoothIRSensor(int sensorpin, char character);
  71. int irValueToCm(float x);
  72. int getPingSensor();
  73.  
  74. //Fonction permettant de catcher un message reçu via le port série
  75. void handleMessage(int serialData[]);
  76.  
  77. void setup() {
  78.  
  79.   // On lance la communication avec l'interface JAVA
  80.   Serial.begin(115200);
  81.  
  82.   // Input-output pin's pour le TReX
  83.   pinMode(RXPIN_POLOLU, INPUT);
  84.   pinMode(TXPIN_POLOLU, OUTPUT);
  85.  
  86.   // On lance la communication avec l'interface pololu TreX
  87.   pololu.begin(19200);
  88.  
  89.   // Input-output pin's pour le Micro maestro
  90.   pinMode(RXPIN_MAESTRO, INPUT);
  91.   pinMode(TXPIN_MAESTRO, OUTPUT);
  92.  
  93.   // On lance la communication avec l'interface pololu Micro Maestro
  94.   maestro.begin(115200);
  95.   delay(1000);
  96.  
  97.   // On met la tourelle dans sa position initiale
  98.   settarget(VERTICAL, 60);
  99.   settarget(HONRIZONTAL, 90);
  100.   delay(100);
  101.   goHome();
  102.  
  103.   pinMode(leftSensorIr, INPUT);
  104.   digitalWrite(leftSensorIr, LOW);
  105.   pinMode(centerSensorIr, INPUT);
  106.   digitalWrite(centerSensorIr, LOW);
  107.   pinMode(rightSensorIr, INPUT);
  108.   digitalWrite(rightSensorIr, LOW);
  109.  
  110.   // prescale clock à 16 pour les pins analogiques
  111.   bitClear(ADCSRA,ADPS0);
  112.   bitClear(ADCSRA,ADPS1);
  113.   bitSet(ADCSRA,ADPS2);
  114.  
  115. }
  116.  
  117. void loop()
  118. {
  119.   unsigned long currentMillis;
  120.   unsigned long currentMillisSerial;
  121.  
  122.   //On récupère les frames d'un message tant que la var serialFlag n'est pas à 1
  123.   if(Serial.available()>0){
  124.     getSerialMessage();
  125.   }
  126.  
  127.   //Si il y a un message complet à traiter, on le traite
  128.   if(serialFlag==1){  
  129.     handleMessage(serialData);    
  130.   }
  131.  
  132.   //******************************************
  133.   // Algorithme pour l'évitement d'obstacles *
  134.   //******************************************
  135.  
  136.   rightSensorCm = getSmoothIRSensor(rightSensorIr, 'Z');
  137.   centerSensorCm = getSmoothIRSensor(centerSensorIr, 'Y');
  138.   leftSensorCm = getSmoothIRSensor(leftSensorIr, 'X');
  139.  
  140.   // Si robotMode est en manuel, on met à jour la valeur des moteurs avec la valeur recue via serial
  141.   if (robotMode == 0) {
  142.  
  143.     goMotors(motorDirection,moteurValue);
  144.     motorDirectionPrec = motorDirection;
  145.  
  146.     //Si mode automatique
  147.   }
  148.   else if (robotMode == 1) {
  149.  
  150.     // Tant qu'on est sup à la distance de sécurité
  151.     if (rightSensorCm>DISTANCE_SECURITE && centerSensorCm>DISTANCE_SECURITE && leftSensorCm>DISTANCE_SECURITE) {
  152.  
  153.       // On avance
  154.       goMotors('F',0127);
  155.       motorDirectionPrec = 'F';
  156.  
  157.     }
  158.     else {
  159.  
  160.       // Sinon on arrête le robot
  161.       goMotors('O',0000);
  162.       motorDirectionPrec = 'O';
  163.  
  164.       // On recule
  165.       goMotors('B',127);
  166.       motorDirectionPrec = 'B';
  167.       delay(400);
  168.  
  169.       // On arrête
  170.       goMotors('O',0000);
  171.       motorDirectionPrec = 'O';
  172.  
  173.       // Si obstacle à gauche, on tourne à droite
  174.       if (leftSensorCm<centerSensorCm && leftSensorCm<rightSensorCm){
  175.         sendValue('V',0001);
  176.         goMotors('R',0127);
  177.         motorDirectionPrec = 'R';
  178.         delay(600);
  179.       }
  180.  
  181.       // Si obstacle au milieu, on fait demi tour
  182.       if (centerSensorCm<leftSensorCm && centerSensorCm<rightSensorCm){
  183.         sendValue('V',0002);
  184.         goMotors('L',0127);
  185.         motorDirectionPrec = 'L';
  186.         delay(1200);
  187.       }
  188.  
  189.       // Si obstacle à droite, on tourne à gauche
  190.       if (rightSensorCm<centerSensorCm && rightSensorCm<leftSensorCm){
  191.         sendValue('V',0003);
  192.         goMotors('L',0127);
  193.         motorDirectionPrec = 'L';
  194.         delay(600);
  195.       }
  196.  
  197.       // On arrête
  198.       goMotors('O',0000);
  199.       motorDirectionPrec = 'O';
  200.       delay(2500);
  201.  
  202.       // On prépare et commence le scan
  203.       sendValue('V',0000);
  204.       settarget(VERTICAL, 60);
  205.       settarget(HONRIZONTAL, 40);
  206.       delay(100);
  207.  
  208.       int i=40;
  209.  
  210.       // On scanne
  211.       while(i<=140)
  212.       {
  213.         settarget(HONRIZONTAL, i);
  214.         pingSensorCm = getPingSensor();
  215.         delay(30);
  216.         i++;
  217.       }
  218.  
  219.       // On repositionne la tourelle
  220.       settarget(VERTICAL, 60);
  221.       settarget(HONRIZONTAL, 90);
  222.       delay(100);
  223.       goHome();
  224.  
  225.     }
  226.  
  227.   }
  228.  
  229.   //*****************************************
  230.    
  231.   // Si on veut faire un update des données selon un interval donné
  232.   currentMillis = millis();
  233.  
  234.   if(currentMillis - previousMillis > UPDATE_INTERVAL) {  
  235.     previousMillis = currentMillis;
  236.   }
  237.  
  238.   // On peut renvoyer des données selon un interval
  239.   if(currentMillis - previousMillisSerial > SERIAL_SENDING_INTERVAL) {  
  240.     previousMillisSerial = currentMillis;
  241.     sendValue(motorDirection,moteurValue);
  242.     sendValue('Z',rightSensorCm);
  243.     sendValue('Y',centerSensorCm);
  244.     sendValue('X',leftSensorCm);
  245.   }
  246.  
  247. }/*---- FIN DE LA BOUCLE -----*/
  248.  
  249. /**
  250.  * sendValue : Fonction permettant d'envoyer une valeur avec un flag donné vers le port série
  251.  */
  252. void sendValue(char command,int value){
  253.  
  254.   uint8_t* message;
  255.   message=(uint8_t*)malloc(10*sizeof(uint8_t));
  256.   message[0]=SERIAL_START_FLAG;
  257.   int milA=value/1000;
  258.   int centA=(value-milA*1000)/100;
  259.   int  dizA=(value-milA*1000-centA*100)/10;
  260.   int  unitA=value-milA*1000-centA*100-dizA*10;
  261.  
  262.   message[1]=command;
  263.   message[2]=milA+'0';
  264.   message[3]=centA+'0';
  265.   message[4]=dizA+'0';
  266.   message[5]=unitA+'0';
  267.   message[6]=SERIAL_END_FLAG;
  268.   message[7]='\n';
  269.  
  270.   Serial.write(message,8);
  271.   free(message);
  272. }
  273.  
  274.  
  275. /**
  276.  * tenpow: Fonction pemrmettant de recomposer un int à partir du message sérialisé
  277.  */
  278. int tenpow(int x) {
  279.   switch(x){
  280.   case 3:
  281.     return 1;
  282.     break;
  283.   case 2 :
  284.     return 10;
  285.     break;
  286.   case 1 :
  287.     return 100;
  288.     break;
  289.   case 0 :
  290.     return 1000;
  291.   }
  292. }
  293.  
  294. /**
  295.  *Fontion permettant de traiter un message reçu via le port série.
  296.  *La fontion prend en entrée la var serialData
  297.  */
  298. void handleMessage(int serialData[]){  
  299.   serialFlag=0;
  300.  
  301.   //On récupére le caractère indiquant l'élement à évaluer
  302.   char command=serialData[1];
  303.   int value=0;    
  304.   //On traduit la valeur depuis ascii little endian vers un simple int
  305.   for(int j=2;j<sizeOfData-1;j++){
  306.     value=value + (serialData[j]-'0')*tenpow(j-2);
  307.   }
  308.  
  309.   // On réalise l'opération en fonction du caractère
  310.   switch (command){
  311.  
  312.   case 'L':
  313.     motorDirection = 'L';
  314.     moteurValue=value;
  315.     break;
  316.  
  317.   case 'R':
  318.     motorDirection = 'R';
  319.     moteurValue=value;
  320.     break;
  321.  
  322.   case 'F':
  323.     motorDirection = 'F';
  324.     moteurValue=value;
  325.     break;
  326.  
  327.   case 'B':
  328.     motorDirection = 'B';
  329.     moteurValue=value;
  330.     break;
  331.  
  332.   case 'O':
  333.     motorDirection = 'O';
  334.     moteurValue=0;
  335.     break;
  336.  
  337.   case 'A':
  338.     robotMode = 1;
  339.     break;
  340.  
  341.   case 'M':
  342.     robotMode = 0;
  343.     break;
  344.   }
  345.  
  346. }
  347.  
  348.  
  349. /**
  350.  * Fonction permettant de récuperer un message depuis le port série
  351.  */
  352. void getSerialMessage(void){
  353.  
  354.   while(Serial.available()>0){
  355.     int reading=Serial.read();
  356.  
  357.     /* S'il s'agit de la 1ére frame 0x7E (frame de début), on réinitialise à 0 la var frameInnerCounter
  358.      * On positionne la var readFrame à true
  359.      */
  360.     if(reading==SERIAL_START_FLAG){        
  361.       frameInnerCounter=0;
  362.       serialData[frameInnerCounter]=reading;
  363.       readFrame=true;
  364.     }
  365.  
  366.     /* Si on a déjà passé la frame de début on
  367.      * incrémente la var frameInnerCounter et on sauvegarde les données
  368.      */
  369.     else if(readFrame==true){
  370.       frameInnerCounter++;
  371.       serialData[frameInnerCounter]=reading;
  372.  
  373.       /* S'il s'agit de la dernière frame 0x24(frame de fin),
  374.        * On positionne la var readFrame à false
  375.        * On positionne la var serialFlag à 1 pour indiquer qu'il y a un message à traiter
  376.            * On stock le nombre de frame réelle à traiter dans la var sizeOfData
  377.        */
  378.       if(reading==SERIAL_END_FLAG) {
  379.         readFrame=false;
  380.         serialFlag=1;
  381.         sizeOfData=frameInnerCounter+1;
  382.       }
  383.     }        
  384.   }
  385. }
  386.  
  387. /**
  388.  * Permet de convertir la valeur des sensors IR en CM
  389.  */
  390. int irValueToCm(int sensorValue)
  391. {
  392.   int cm;
  393.   cm = 10650.08 * pow(sensorValue,-0.935) - 10;
  394.   if (cm>200||cm==0) {
  395.     cm = 200;
  396.   }
  397.   return cm;
  398. }
  399.  
  400. /**
  401.  * Permet de convertir une durée en cm (pour capteur ping)
  402.  */
  403. unsigned long Convert_Time_Space(const unsigned long fnDuration ) {    
  404.   // 29 microseconds par cm.    
  405.   return fnDuration / 29 / 2 ;
  406. }
  407.  
  408. /**
  409.  * Récupère une valeur lissé d'un capteur IR en effectuant plusieurs tirs
  410.  */
  411. int getSmoothIRSensor(int sensorpin, char character){
  412.    
  413.    switch (character){
  414.     case 'X':
  415.       digitalWrite(leftSensorIr, HIGH);
  416.       break;
  417.     case 'Y':
  418.       digitalWrite(centerSensorIr, HIGH);
  419.       break;
  420.     case 'Z':
  421.       digitalWrite(rightSensorIr, HIGH);
  422.       break;
  423.     }
  424.  
  425.    delay(5);  
  426.    int returnval = 0;
  427.    returnval = irValueToCm(digitalRead(sensorpin));
  428.    
  429.    digitalWrite(leftSensorIr, LOW);
  430.    digitalWrite(centerSensorIr, LOW);
  431.    digitalWrite(rightSensorIr, LOW);
  432.    
  433.    int sortedValues[NB_LECTURE];
  434.    
  435.    for(int i=0;i<NB_LECTURE;i++){
  436.      
  437.      int value = irValueToCm(analogRead(sensorpin));
  438.      int j;
  439.      if(value<sortedValues[0] || i==0){
  440.        
  441.         j=0; //insert at first position
  442.        
  443.      }else{
  444.        
  445.        for(j=1;j<i;j++){
  446.          
  447.           if(sortedValues[j-1]<=value && sortedValues[j]>=value){
  448.             // j is insert position
  449.             break;
  450.           }
  451.        }
  452.      }
  453.      
  454.      for(int k=i;k>j;k--){
  455.        // move all values higher than current reading up one position
  456.        sortedValues[k]=sortedValues[k-1];
  457.      }
  458.      
  459.      sortedValues[j]=value; //insert current reading
  460.      
  461.    }
  462.    
  463.    //return scaled mode of NB_MEDIAN*2 values
  464.    for(int i=NB_LECTURE/2-NB_MEDIAN;i<(NB_LECTURE/2+NB_MEDIAN);i++){
  465.      
  466.      returnval +=sortedValues[i];
  467.      
  468.    }
  469.    returnval = returnval/(NB_MEDIAN*2);
  470.    return returnval;
  471. }
  472.  
  473. /**
  474.  * Récupération de la valeur du capteur ping
  475.  */
  476. int getPingSensor()
  477. {
  478.   int pingSensorValue;
  479.   pinMode(pingSensor, OUTPUT);  
  480.   digitalWrite(pingSensor, LOW); // init sensor to ensure clean HIGH pulse  
  481.   delayMicroseconds(2);    
  482.   digitalWrite(pingSensor, HIGH);  // make the sensor send a pulse    
  483.   delayMicroseconds(5);    
  484.   digitalWrite(pingSensor, LOW);  // Set LOW again  
  485.   pinMode(pingSensor, INPUT);  // Get ready to capture the duration of the resulting pulse  
  486.   // Capture how long the pin stays in HIGH state.  
  487.   unsigned long duration = pulseIn(pingSensor, HIGH);
  488.   pingSensorValue = Convert_Time_Space(duration);
  489.   sendValue('W',pingSensorValue);  
  490.   return pingSensorValue;
  491. }
  492.  
  493. /**
  494.  * Envoi une commande Set Target au Maestro.
  495.  * La target est en quart de microseconds.
  496.  */
  497. void settarget(unsigned char servo, unsigned int target)
  498. {
  499.   target = map(target, 0, 180, 880*4, 2320*4);
  500.   maestro.write(0xAA); //start byte
  501.   maestro.write(0x0C) ; //device id
  502.   maestro.write(0x04); //command number
  503.   maestro.write(servo); //servo number
  504.   maestro.write(target & 0x7F);
  505.   maestro.write((target >> 7) & 0x7F);
  506. }
  507.  
  508. /**
  509.  * Envoi une commande goHome au Maestro.
  510.  * Permet de ne plus envoyer de commande au maestro
  511.  * et donc d'éteindre tous les servos
  512.  */
  513. void goHome()
  514. {
  515.   maestro.write(0xAA); //start byte
  516.   maestro.write(0x0C) ; //device id
  517.   maestro.write(0x22); //go home command number
  518. }
  519.  
  520. // Set the motor index, direction, and speed
  521. // Motor index should either be a 0 or 1
  522. // Direction should be either true for forward or false for backwards
  523. // Speed should range between 0 and 127 (inclusivly)
  524. void SetSpeed(int MotorIndex, boolean Forward, int Speed)
  525. {
  526.   // Validate motor index
  527.   if(MotorIndex < 0 || MotorIndex > 1)
  528.     return;
  529.  
  530.   // Validate speed
  531.   if(Speed < 0)
  532.     Speed = 0;
  533.   else if(Speed > 127)
  534.     Speed = 127;
  535.  
  536.   // Send the "set" command based on the motor
  537.   // Note that we do not accelerate to the
  538.   // speed, we just instantly set it
  539.   unsigned char SendByte = 0;
  540.   if(MotorIndex == 0)
  541.     SendByte = 0xC2;
  542.   else if(MotorIndex == 1)
  543.     SendByte = 0xCA;
  544.  
  545.   // If we go backwards, the commands are the same
  546.   // but minus one
  547.   if(!Forward)
  548.     SendByte--;
  549.  
  550.   // Send the set speed command byte
  551.   pololu.write(SendByte);
  552.  
  553.   // Send the speed data byte
  554.   pololu.write(Speed);
  555. }
  556.  
  557. //On indique au robot d'avancer
  558. void goForward(int Speed)
  559. {
  560.   SetSpeed(0, false, Speed);
  561.   SetSpeed(1, false, Speed);
  562. }
  563.  
  564. //On indique au robot de reculer
  565. void goBackward(int Speed)
  566. {
  567.   SetSpeed(0, true, Speed);
  568.   SetSpeed(1, true, Speed);
  569. }
  570.  
  571. //On indique au robot d'aller à gauche
  572. void goGauche(int Speed)
  573. {
  574.   SetSpeed(0, false, Speed);
  575.   SetSpeed(1, true, Speed);
  576. }
  577.  
  578. //On indique au robot d'aller à droite
  579. void goDroite(int Speed)
  580. {
  581.   SetSpeed(0, true, Speed);
  582.   SetSpeed(1, false, Speed);  
  583. }
  584.  
  585. //Gestion des moteurs
  586. void goMotors(char Direction, int Speed)
  587. {
  588.   switch (Direction){
  589.  
  590.   case 'O':
  591.  
  592.     switch (motorDirectionPrec){
  593.     case 'F':
  594.       goBackward(Speed);
  595.       break;
  596.     case 'B':
  597.       goForward(Speed);
  598.       break;
  599.     case 'L':
  600.       goDroite(Speed);
  601.       break;
  602.     case 'R':
  603.       goGauche(Speed);
  604.       break;
  605.     }
  606.     break;
  607.  
  608.   case 'L':
  609.     goGauche(Speed);
  610.     break;
  611.   case 'R':
  612.     goDroite(Speed);
  613.     break;
  614.   case 'F':
  615.     goForward(Speed);
  616.     break;
  617.   case 'B':
  618.     goBackward(Speed);
  619.     break;
  620.   }
  621.  
  622. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top