DarylRobotProject Serial arduino java

#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;
  }

}