Hi,
this is a code for use stepper motor on vigibot using an Arduino and AccelStepper library.
I use a A4988 stepper motor driver., two wire is needed between driver and Arduino ( Step & Dir).
/*
* Vigibot Pi to Arduino Uart default remote configuration example by Mike118
*/
#include <AccelStepper.h>
// Meta Type :
typedef struct {
union {
struct {
int16_t x;
int16_t y;
};
int16_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
// 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[NBPOSITIONS]; // 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();
AccelStepper Xaxis(1, 2, 6);
AccelStepper Yaxis(1, 4, 7);
void setup() {
PISERIAL.begin(115200);
// add all your init here
Xaxis.setMaxSpeed(400);
Yaxis.setMaxSpeed(400);
Xaxis.setSpeed(0);
Yaxis.setSpeed(0);
}
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();
updateSteppers();
}
if( millis() - lastTrameTimestamp > FAILSAFE ) {
// Stop the robot in case the robot lost connection with the Pi
stopSteppers();
} else {
// put your main code here, to run repeatedly:
// avoid abstacle, run speed ...
runSteppers();
}
}
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++) // Do not send the 4 sync data in 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 (will be updated by Raspberry pi)
trameTx.val16[1] = 0; // Percent (will be updated by Raspberry pi)
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 (will be updated by Raspberry pi)
trameTx.val8[1] = 0; // Soc temp (will be updated by Raspberry pi)
trameTx.val8[2] = 0; // link (will be updated by Raspberry pi)
trameTx.val8[3] = 0; // RSSI (will be updated by Raspberry pi)
for( uint8_t i = 0; i < TXFRAMESIZE; i++)
PISERIAL.write(trameTx.bytes[i]);
}
void displayTtsBuffer (uint8_t * ttsBuffer, uint8_t bufferSize) {
// you can modify this function to display text on a screen depending on your hardware...
for( uint8_t i = 0; i < bufferSize; i++)
Serial.write(ttsBuffer[i]);
Serial.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;
}
}
void stopSteppers() {
Xaxis.setSpeed(0);
Yaxis.setSpeed(0);
runSteppers();
}
void runSteppers() {
Xaxis.runSpeed();
Yaxis.runSpeed();
}
void updateSteppers() {
Xaxis.setSpeed(trameTx.positions[0].x);
Yaxis.setSpeed(trameTx.positions[0].y);
}
