Aller au contenu


Marmont

Inscrit(e) (le) 04 déc. 2019
Déconnecté Dernière activité déc. 05 2019 10:06
-----

Messages que j'ai postés

Dans le sujet : Vigibot Pi arduino Serial Communication Example

04 décembre 2019 - 03:02

Hello here,

I have tested the code with a pololu zumo, work like a charm!

 

The advantage here is if you have a robot wich is already driven by arduino, it can be easily modified to work with vigibot:

 

1) Connect the UART :

  • Ardu.pin0 RX <-> PI GPIO14 TX
  • Ardu.pin1 TX <-> PI GPIO15 RX
  • GND <-> GND

2) Modify the code above to work with your bot, in the main loop you can get the most importants variables:

trameRx.vitesses.y -> Speed in main axis of the robot send by the server

trameRx.vitesses.z - Rotation around the vertical axis send by the server

 

With theses moves requests you can program your motors to feet the particularity of the robot. Here the exemple with a classical differential driven robot, the Zumo.

    // mixage pour obtenir les vitesses moteurs
    left_speed = (trameRx.vitesses.y * MAX_SPEED / SPEED_RANGE) - (trameRx.vitesses.z * MAX_SPEED / SPEED_RANGE);
    right_speed = (trameRx.vitesses.y * MAX_SPEED / SPEED_RANGE) + (trameRx.vitesses.z * MAX_SPEED / SPEED_RANGE);

    // Saturation des vitesses
    left_speed = min(max(left_speed, -MAX_SPEED), MAX_SPEED);
    right_speed = min(max(right_speed, -MAX_SPEED), MAX_SPEED);

Then you can send the right speed to your motors, in my case: (I use a library to drive the zumo)

    ZumoMotors::setSpeeds(left_speed, right_speed);

Here the complete code :

/*
 * Vigibot Pi to Arduino Uart default remote configuration example by Mike118
 * Completed by Marmont to work with a Zumo
 */

#include <ZumoShield.h>

// Meta Type :

typedef struct {
 union {
  struct {
   uint16_t x;
   uint16_t y;
  };
  uint16_t coordonnees[2];
  uint8_t bytes[4];
 };
} Point;

typedef struct {
 union {
  struct {
   int8_t x;
   int8_t y;
   int8_t z;
  };
  uint8_t bytes[3];
 };
} Vitesses;


// CONFIG

#define PISERIAL Serial
#define NBPOSITIONS 2
#define FAILSAFE 250 // ms

#define MAX_SPEED              400 // max motor speed
#define SPEED_RANGE            128 //

// TTS

#define TTSBUFFERSIZE 255  
uint8_t ttsBuffer[TTSBUFFERSIZE];
uint8_t ttsCurseur = 0;


// TX

#define TXFRAMESIZE (NBPOSITIONS * 4 + 17)

typedef struct {
 union {
  struct {
   uint8_t sync[4];               // 4
   Point positions[NBPOSITIONS];  // NBPOSITIONS * 4
   uint16_t val16[2];             // 2 * 2
   uint8_t choixCameras;          // 1
   Vitesses vitesses;             // 3
   uint8_t interrupteurs;         // 1
   uint8_t val8[4];               // 4
  };

  uint8_t bytes[TXFRAMESIZE];
 };
} TrameTx;


// RX

#define RXFRAMESIZE (NBPOSITIONS * 4 + 9)

typedef struct {
 union {
  struct {                       // Sizes
   uint8_t sync[4];              // 4
   Point positions[NBPOSITION];  // NBPOSITIONS * 4
   uint8_t choixCameras;         // 1
   Vitesses vitesses;            // 3
   uint8_t interrupteurs;        // 1
  };

  uint8_t bytes[RXFRAMESIZE];
 };
} TrameRx;


TrameTx trameTx;
TrameRx trameRx;

uint32_t lastTrameTimestamp = millis();

void setup() {
  PISERIAL.begin(115200);
 
 
  // add all your init here
}


void loop() {
 
  if(readPiSerial()) {
    // each time we receive a full trame run repeatedly:
    // use values inside TrameRx to tell your robot how to move ...
    // trameRx.vitesses.x , trameRx.vitesses.y, trameRx.vitesses.z
    // trameRx.positions[i].x trameRx.positions[i].y  etc....
    
    writePiSerial();
    lastTrameTimestamp = millis();
  }

  if( millis() - lastTrameTimestamp > FAILSAFE ) {
    // Stop the robot in case the robot lost connection with the Pi
  } else {

    int left_speed, right_speed;
    
    // mixage pour obtenir les vitesses moteurs
    left_speed = (trameRx.vitesses.y * MAX_SPEED / SPEED_RANGE) - (trameRx.vitesses.z * MAX_SPEED / SPEED_RANGE);
    right_speed = (trameRx.vitesses.y * MAX_SPEED / SPEED_RANGE) + (trameRx.vitesses.z * MAX_SPEED / SPEED_RANGE);

    // Saturation des vitesse
    left_speed = min(max(left_speed, -MAX_SPEED), MAX_SPEED);
    right_speed = min(max(right_speed, -MAX_SPEED), MAX_SPEED);

    ZumoMotors::setSpeeds(left_speed, right_speed);
  }
}

bool readPiSerial() {
 uint8_t current;
 static uint8_t lastType = 0;
 static uint8_t n = 0;
 static uint8_t frame[RXFRAMESIZE];

 while(PISERIAL.available()) {
  current = PISERIAL.read();

  switch(n) {

   case 0:
    if(current == '$')
     n = 1;
    break;

   case 1:
    if(current != 'T' && lastType == 'T')
     writeTtsBuffer('\0');

    if(current == 'S' || current == 'T') {
     lastType = current;
     n = 2;
    } else
     n = 0;
    break;

    default:
    frame[n++] = current;

    if(n == RXFRAMESIZE) {

     if(lastType == 'T') {

      for(uint8_t i = 4; i < RXFRAMESIZE; i++) // 4 to not send the sync data into tts
       writeTtsBuffer(frame[i]);

     } else if(lastType == 'S') {

      for(uint8_t p = 0; p < RXFRAMESIZE; p++)
       trameRx.bytes[p] = frame[p];

     }
     n = 0;
     return true;
    }

  }
 }

 return false;
}

void writePiSerial() {
 // Header, do not modify
 trameTx.sync[0] = '$';
 trameTx.sync[1] = 'R';  
 trameTx.sync[2] = ' ';
 trameTx.sync[3] = ' ';

 // modify the feedback according your need. By default we copy the trameRx content ...
 for(uint8_t i = 0; i < NBPOSITIONS; i++) {
  trameTx.positions[i].x = trameRx.positions[i].x;
  trameTx.positions[i].y = trameRx.positions[i].y;
 }
 trameTx.val16[0] = 0;   // Voltage
 trameTx.val16[1] = 0;   // Percent
 trameTx.choixCameras = trameRx.choixCameras;
 trameTx.vitesses.x = trameRx.vitesses.x;
 trameTx.vitesses.y = trameRx.vitesses.y;
 trameTx.vitesses.z = trameRx.vitesses.z;
 trameTx.interrupteurs = trameRx.interrupteurs;
 trameTx.val8[0] = 0;   // CPU load
 trameTx.val8[1] = 0;   // Soc temp 
 trameTx.val8[2] = 0;   // link
 trameTx.val8[3] = 0;   // RSSI

 for( uint8_t i = 0; i < TXFRAMESIZE; i++)
  PISERIAL.write(trameTx.bytes[i]);
}

void displayTtsBuffer (uint8_t * ttsBuffer, uint8_t bufferSize) {
  // you should modify this function to display text on a screen depending on your hardware...
  /* if using an arduino with a second Serial different from the PISERIAL example SERIAL1 you can do this :
  for( uint8_t i = 0; i < bufferSize; i++)
    Serial1.write(ttsBuffer[i]);
  Serial1.println("");
  */
}

void writeTtsBuffer( uint8_t ttsChar) {
  static uint8_t ttsCurseur = 0;
  if( ttsCurseur < TTSBUFFERSIZE && ttsChar != '\0') {
    ttsBuffer[ttsCurseur] = ttsChar;
    ttsCurseur ++;
  }
  if( ttsCurseur == TTSBUFFERSIZE || ttsChar == '\0') {
    displayTtsBuffer (ttsBuffer, ttsCurseur);
    ttsCurseur = 0;
  }
}

Very easy, very fonctionnal, a lot more functionnality to investigate!

Thk to mikes118 for the great work.

Marmont