Hi - This is probably a long shot... but I have some code here from Windows-based PIC that I am trying to convert to Arduino. Apparently an int in Windows is 32 bit so I am running into problems as a result. I am trying to convert the code to use longs instead but it's choking (on the CheckSum routine I think).
Anyway... if someone has the patience of Job and wants to take a look, the unaltered code is below. This is motor controller code and what is happening is that if the motor position > the max value of an int, it rolls over to be a negative number. I've tried various thing but without success.
tx,
Roy
#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 gxMode2 45
#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
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 setMode2(int boardID, int newMode){
/* set drive mode of Gamoto */
return WriteReg(boardID, gxMode2,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
}
long getPositionError(int boardID){
return ReadReg(boardID, 97, 3);
}
/**************************************************************/
/* 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
delay(100);
ReadBytes(rBuffer,&bytes_read,2);
if (rBuffer[0]==ACK && rBuffer[1]==ACK)
return 1;
else
Serial.print("error ");
Serial.println(boardID);
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(200);
// 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();
}
}
}