Problem adapting some Gamoto motor controller code

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();
    }
  }
 
}