Hi - I've reached the boundaries of my C knowledge so if anyone has the time to look through the code below it would be a big help.
I think it must be a simple issue. I've adapted this Gamoto motor controller code to work with 2 motors. The boards support serial addressing so that multiple boards can be daisy chained.
What I did was to take the routines like this
int setmPosition(int mp){
return WriteReg(gxmPosition,3,mp);
}
and add a boardID param:
int setmPosition(int boardID, int mp){
return WriteReg(boardID, gxmPosition,3,mp);
}
It sort of works (i.e. both motors run) but they don't follow the position commands accurately. I was thinking that it might be because the original code used just 2 read/write buffers:
unsigned char wBuffer[20]; //write buffer used to construct messages
unsigned char rBuffer[20]; //read buffer used to parse messages
I tried creating W/R buffers for each motor and put in a switch statement in the WriteBytes/ReadBytes methods depending on which boardID was being passed as a param. That didn't work. So I'm a bit lost.
--Is it because Serial stuff is asynchronous so trying to read/write to both motors in loop() without doing some sort of blocking is causing the data to get munged?
Anyway - the Serial code is at the end of the routiine below - maybe someone can x-ray it and see if there is a simple fix.
thanks!
--Roy
///gamoto2Boards_01
#define ACK 0x41
#define gxWRITE 0x82
#define POSITION_MODE 1
#define TRAJECTORY_MODE 3
#define VELOCITY_MODE 5
#define POWER_MODE 17
#define gxFactoryReset 0
#define gxSaveParams 1
#define gxReset 2
#define gxSetHome 3
#define gxKp 34
#define gxKi 36
#define gxKd 38
#define gxiL 40
#define gxdS 42
#define gxMode 43
#define gxPositionMode 1
#define gxTrajMode 3
#define gxVelocityMode 5
#define gxPowerMode 17
#define gxpwrLimit 44
#define gxdummy1 45
#define gxsetPosition 47
#define gxmPosition 51
#define gxsetVelocity 54
#define gxmVelocity 57
#define gxTrajectory 59
#define gxmPower 60
#define dBug1 122
#define dBug2 123
#define gxAnalog0 160
#define gxAnalog1 162
#define gxAnalog2 164
#define gxAnalog3 166
#define gxAnalog4 168
#define gxProfile0 180
#define gxVersion 178
#define gxTrajx 180
#define gxTrajv 183
#define gxTraja 185
unsigned char wBuffer[20]; //write buffer used to construct messages
unsigned char rBuffer[20]; //read buffer used to parse messages
//=============================================================================
byte val; // variable to receive data from the serial port
int ledPin = 48;
int xpos = 5000;
int xcurrentPos = 0;
int ypos = 5000;
int ycurrentPos = 0;
int xAxis = 0xAA;
int yAxis = 0xAB;
void setup()
{
Serial.begin(19200); // serial to Director
Serial1.begin(19200); // serial to gamoto
pinMode(ledPin, OUTPUT);
int i;
for (i =0; i < 3; i++){
digitalWrite(ledPin, HIGH);
delay(100);
digitalWrite(ledPin, LOW);
delay(100);
}
SetHome(xAxis);
setsetPosition(xAxis, xpos);
SetHome(yAxis);
setsetPosition(yAxis, ypos);
}
void loop() {
digitalWrite(ledPin, HIGH);
// Serial.println(currentPos);
ycurrentPos = getmPosition(yAxis);
if (ypos != ycurrentPos ){
Serial.print("y ");
Serial.println(ycurrentPos);
}
else {
Serial.println("y done");
if (ypos == 5000){
ypos = 500;
}
else {
ypos = 5000;
}
int y = setsetPosition(yAxis, ypos);
// Serial.println(y);
}
////
xcurrentPos = getmPosition(xAxis);
if (xpos != xcurrentPos ){
Serial.print("x ");
Serial.println(xcurrentPos);
}
else {
Serial.println("x done");
if (xpos == 5000){
xpos = 500;
}
else {
xpos = 5000;
;
}
int x = setsetPosition(xAxis, xpos);
// Serial.println(x);
}
delay(200);
digitalWrite(ledPin, LOW);
delay(200);
}
//// GAMOTO ROUTINES==============================================
int Reset(int boardID){
return WriteReg(boardID, gxReset,1,0);
}
int FactoryReset(int boardID){
return WriteReg(boardID, gxFactoryReset,1,0);
}
int SaveParams(int boardID){
return WriteReg(boardID, gxSaveParams,1,0);
}
int SetHome(int boardID){
return WriteReg(boardID, gxSetHome,1,0);
}
int setKp(int boardID, int kp){
return WriteReg(boardID, gxKp,2,kp);
}
int setKi(int boardID, int ki){
return WriteReg(boardID, gxKi,2,ki);
}
int setKd(int boardID, int kd){
return WriteReg(boardID, gxKd,2,kd);
}
int setdS(int boardID, int ds){
return WriteReg(boardID, gxdS,2,ds);
}
int setiL(int boardID, int il){
return WriteReg(boardID, gxiL,2,il);
}
int setMode(int boardID, int newMode){
/* set drive mode of Gamoto */
return WriteReg(boardID, gxMode,1,newMode);
}
int setsetPosition(int boardID, int pos){
/* Writes to the set position of the motor. This value is used by the PID in absolute
mode to drive the motor to a particular absolute position. The set position is also updated
by some internal functions of the Gamoto board, see Gamoto documentation for full details. */
return WriteReg(boardID, gxsetPosition,3,pos);
}
int setTrajectory(int boardID, int traj){
return WriteReg(boardID, gxTrajectory,1,traj);
}
int setpwrLimit(int boardID, int pl){
return WriteReg(boardID, gxpwrLimit,1,pl);
}
int setmPosition(int boardID, int mp){
return WriteReg(boardID, gxmPosition,3,mp);
}
int setsetVelocity(int boardID, int sv){
return WriteReg(boardID, gxsetVelocity,2,sv);
}
int setmPower(int boardID, int mp){
return WriteReg(boardID, gxmPower,1,mp);
}
int setTrajx(int boardID, int tx,int profile){
return WriteReg(boardID, gxTrajx+7*profile,3,tx);
}
int setTrajv(int boardID, int tv,int profile){
return WriteReg(boardID, gxTrajv+7*profile,2,tv);
}
int setTraja(int boardID, int ta,int profile){
return WriteReg(boardID, gxTraja+7*profile,2,ta);
}
int getKp(int boardID){
return ReadReg(boardID, gxKp,2);
}
int getKi(int boardID){
return ReadReg(boardID, gxKi,2);
}
int getKd(int boardID){
return ReadReg(boardID, gxKd,2);
}
int getiL(int boardID){
return ReadReg(boardID, gxiL,2);
}
int getdS(int boardID){
return ReadReg(boardID, gxdS,1);
}
int getMode(int boardID){
return ReadReg(boardID, gxMode,1);
}
int getpwrLimit(int boardID){
return ReadReg(boardID, gxpwrLimit,1);
}
int getsetPosition(int boardID){
/* Reads the current value of the setPosition register in the Gamoto */
return ReadReg(boardID, gxsetPosition,3);
}
int getmPosition(int boardID){
return ReadReg(boardID, gxmPosition,3);
}
int getsetVelocity(int boardID){
return ReadReg(boardID, gxsetVelocity,2);
}
int getmVelocity(int boardID){
return ReadReg(boardID, gxmVelocity,2);
}
int getTrajectory(int boardID){
return ReadReg(boardID, gxTrajectory,1);
}
int getmPower(int boardID){
return ReadReg(boardID, gxmPower,1);
}
int getVersion(int boardID){
return ReadReg(boardID, gxVersion,1);
}
int getmCurrent(int boardID){
return ReadReg(boardID, gxAnalog0,2);
}
int getAnalog0(int boardID){
return ReadReg(boardID, gxAnalog0,2);
}
int getAnalog1(int boardID){
return ReadReg(boardID, gxAnalog1,2);
}
int getAnalog2(int boardID){
return ReadReg(boardID, gxAnalog2,2);
}
int getAnalog3(int boardID){
return ReadReg(boardID, gxAnalog3,2);
}
int getAnalog4(int boardID){
return ReadReg(boardID, gxAnalog4,2);
}
/**************************************************************/
/* Lower level register access routines */
int WriteReg(int boardID, int RegAdr,int RegLen,int Regvalue)
{
// Write to a Gamoto Register, given the register, length, and
// data to write. Check for an ACK response when done.
// Returns 1 if ACK received,
// Returns 0 if no ACK (timeout) or invalid ACK
long bytes_read;
// Build the message
wBuffer[0]= boardID;
wBuffer[1]= RegLen+1;
wBuffer[2]= RegAdr;
for (int i=0;i<RegLen;i++) {
wBuffer[3+i] = Regvalue % 256;
Regvalue = Regvalue >> 8;
}
// Add the checksum as the last byte
wBuffer[3+RegLen]= CheckSum(wBuffer,3+RegLen);
// Send the command
WriteBytes(wBuffer,RegLen+4);
// Receive the ACK response
ReadBytes(rBuffer,&bytes_read,2);
if (rBuffer[0]==ACK && rBuffer[1]==ACK)
return 1;
else
return 0;
}
int ReadReg(int boardID, int RegAdr, int RegLen) {
// Reads a value from a Gamoto Register
int Regvalue=0;
long bytes_read;
// Build the command
wBuffer[0]= boardID;
wBuffer[1]= gxWRITE;
wBuffer[2]= RegAdr;
wBuffer[3]= RegLen;
wBuffer[4]= CheckSum(wBuffer,4);
// Send the command
WriteBytes(wBuffer,5);
//delay(5);
// Get the response
ReadBytes(rBuffer,&bytes_read,RegLen+2);
if (bytes_read>0){
// We got a response - now parse the result
for (int i=0;i<RegLen;i++) {
Regvalue += rBuffer[i+1] * (1<<(8*i));
}
// Handle negative case
if (Regvalue >= (1<<(8*RegLen))/2)
Regvalue -= 1<<(8*RegLen);
}
return Regvalue;
}
byte CheckSum(byte* Buffer,int Len) {
// Calculate the checksum of a read or write buffer
byte sum=0;
for (int i=1;i<Len;i++)
sum += Buffer[i];
return sum;
}
void WriteBytes(unsigned char* lpBuf, int dwToWrite){
int i;
for(i = 0; i < dwToWrite; i++){
Serial1.print(lpBuf[i]);
}
}
void ReadBytes(unsigned char* buffer, long* bytesRead, int len){
while(Serial1.available()){
int i;
for(i = 0; i < len; i++){
bytesRead++;
buffer[i] = Serial1.read();
}
}
}