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