picservo.cpp

This includes the project using a stereo vision to catch the ball shot from a high pressure air cann

SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
*kp = (p->gain).kp;
*kd = (p->gain).kd;
*ki = (p->gain).ki;
*il = (p->gain).il;
*ol = (p->gain).ol;
*cl = (p->gain).cl;
*el = (p->gain).el;
*sr = (p->gain).sr;
*dc = (p->gain).dc;
*sm = (p->gain).sm;
}
//---------------------------------------------------------------------------
BOOL ServoSetGain(byte addr, short int kp, short int kd, short int ki,
				  short int il, byte ol, byte cl, short int el,
                  byte sr, byte dc)
{
SERVOMOD * p;
char cmdstr[16];

p = (SERVOMOD *)(mod[addr].p);
(p->gain).kp = kp;
(p->gain).kd = kd;
(p->gain).ki = ki;
(p->gain).il = il;
(p->gain).ol = ol;
(p->gain).cl = cl;
(p->gain).el = el;
(p->gain).sr = sr;
(p->gain).dc = dc;

*( (short int *)(cmdstr) ) = kp;
*( (short int *)(cmdstr+2) ) = kd;
*( (short int *)(cmdstr+4) ) = ki;
*( (short int *)(cmdstr+6) ) = il;
*( (byte *)(cmdstr+8) ) = ol;
*( (byte *)(cmdstr+9) ) = cl;
*( (short int *)(cmdstr+10) ) = el;
*( (byte *)(cmdstr+12) ) = sr;
*( (byte *)(cmdstr+13) ) = dc;

return NmcSendCmd(addr, SET_GAIN, cmdstr, 14, addr);
}
//---------------------------------------------------------------------------
BOOL ServoSetGain2(byte addr, short int kp, short int kd, short int ki,
				  short int il, byte ol, byte cl, short int el,
                  byte sr, byte dc, byte sm)
{
SERVOMOD * p;
char cmdstr[16];

p = (SERVOMOD *)(mod[addr].p);
(p->gain).kp = kp;
(p->gain).kd = kd;
(p->gain).ki = ki;
(p->gain).il = il;
(p->gain).ol = ol;
(p->gain).cl = cl;
(p->gain).el = el;
(p->gain).sr = sr;
(p->gain).dc = dc;
(p->gain).sm = sm;

*( (short int *)(cmdstr) ) = kp;
*( (short int *)(cmdstr+2) ) = kd;
*( (short int *)(cmdstr+4) ) = ki;
*( (short int *)(cmdstr+6) ) = il;
*( (byte *)(cmdstr+8) ) = ol;
*( (byte *)(cmdstr+9) ) = cl;
*( (short int *)(cmdstr+10) ) = el;
*( (byte *)(cmdstr+12) ) = sr;
*( (byte *)(cmdstr+13) ) = dc;
*( (byte *)(cmdstr+14) ) = sm;

return NmcSendCmd(addr, SET_GAIN, cmdstr, 15, addr);
}
//---------------------------------------------------------------------------
BOOL ServoLoadTraj(byte addr, byte mode, long pos, long vel, long acc, byte pwm)
{
SERVOMOD * p;
char cmdstr[16];
int count;

p = (SERVOMOD *)(mod[addr].p);
p->movectrl = mode;
p->cmdpos = pos;
p->cmdvel = vel;
p->cmdacc = acc;
p->cmdpwm = pwm;

count = 0;
*( (byte *)(cmdstr + count) ) = mode;  count += 1;
if (mode & LOAD_POS) { *( (long *)(cmdstr + count) ) = pos; count += 4; }
if (mode & LOAD_VEL) { *( (long *)(cmdstr + count) ) = vel; count += 4; }
if (mode & LOAD_ACC) { *( (long *)(cmdstr + count) ) = acc; count += 4; }
if (mode & LOAD_PWM) { *( (byte *)(cmdstr + count) ) = pwm; count += 1; }

return NmcSendCmd(addr, LOAD_TRAJ, cmdstr, (byte)count, addr);
}
//---------------------------------------------------------------------------
void ServoInitPath(byte addr)
{
SERVOMOD * p;

NmcReadStatus(addr, SEND_POS | SEND_PERROR);

p = (SERVOMOD *)(mod[addr].p);
p->last_ppoint = p->pos + p->perror;
}
//---------------------------------------------------------------------------
   BOOL ServoAddPathpoints(byte addr, int npoints, long *path, int freq)
{
SERVOMOD * p;
char cmdstr[16];
long diff;
int rev;
int i;

p = (SERVOMOD *)(mod[addr].p);

for (i=0; i<npoints; i++)
  {
  diff = path[i] - p->last_ppoint;
  if (diff<0)
    {
    rev = 0x01;
    diff = -diff;
    }
  else rev = 0x00;

  //Scale the difference appropriately for path freq. used
  if (p->ioctrl & FAST_PATH)  //scale for 60/120 Hz fast path
    {
    if (freq == P_60HZ)
      {
      diff *= (256/32);
      diff |= 0x02;     //60 Hz -> set bit 1 = 1
      }
    else if (freq == P_120HZ) diff *= (256/16);
    else return(false);
    }
  else  //scale for 30/60 Hz slow path
    {
    if (freq == P_30HZ)
      {
      diff *= (256/64);
      diff |= 0x02;     //30 Hz -> set bit 1 = 1
      }
    else if (freq == P_60HZ) diff *= (256/32);
    else return(false);
    }

  diff |= rev;  //bit 0 = reverse bit

  *( (short int *)(cmdstr + 2*i) ) = (short int)diff;

  p->last_ppoint = path[i];
  }


return NmcSendCmd(addr, ADD_PATHPOINT, cmdstr, (byte)(npoints*2), addr);
}
//---------------------------------------------------------------------------
BOOL ServoStartPathMode(byte groupaddr, byte groupleader)
{
return NmcSendCmd(groupaddr, ADD_PATHPOINT, NULL, 0, groupleader);
}
//---------------------------------------------------------------------------
BOOL ServoStartMove(byte groupaddr, byte groupleader)
{
return NmcSendCmd(groupaddr, START_MOVE, NULL, 0, groupleader);
}
//---------------------------------------------------------------------------
BOOL ServoResetPos(byte addr)
{
return NmcSendCmd(addr, RESET_POS, NULL, 0, addr);
}
//---------------------------------------------------------------------------
BOOL ServoResetRelHome(byte addr)
{
byte mode;

mode = REL_HOME;
return NmcSendCmd(addr, RESET_POS, (char *)(&mode), 1, addr);
}
//---------------------------------------------------------------------------
BOOL ServoSetPos(byte addr, long pos)
{
char cmdstr[6];

cmdstr[0] = SET_POS;          //mode byte for reset pos
*( (long *)(cmdstr + 1) ) = pos;

return NmcSendCmd(addr, RESET_POS, cmdstr, 5, addr);
}
//---------------------------------------------------------------------------
BOOL ServoClearBits(byte addr)
{
return NmcSendCmd(addr, CLEAR_BITS, NULL, 0, addr);
}
//---------------------------------------------------------------------------
BOOL ServoStopMotor(byte addr, byte mode)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);

mode &= (byte)(~STOP_HERE);
p->stopctrl = mode;       //make sure STOP_HERE bit is cleared

return NmcSendCmd(addr, STOP_MOTOR, (char *)(&mode), 1, addr);
}
//---------------------------------------------------------------------------
BOOL ServoStopHere(byte addr, byte mode, long pos)
{
SERVOMOD * p;
char cmdstr[6];

p = (SERVOMOD *)(mod[addr].p);

p->stopctrl = mode;

cmdstr[0] = mode;
*( (long *)(cmdstr + 1) ) = pos;

return NmcSendCmd(addr, STOP_MOTOR, cmdstr, 5, addr);
}
//---------------------------------------------------------------------------
   BOOL ServoSetIoCtrl(byte addr, byte mode)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);

p->ioctrl = mode;

return NmcSendCmd(addr, IO_CTRL, (char *)(&mode), 1, addr);
}
//---------------------------------------------------------------------------
BOOL ServoSetHoming(byte addr, byte mode)
{
SERVOMOD * p;

p = (SERVOMOD *)(mod[addr].p);
p->homectrl = mode;

return NmcSendCmd(addr, SET_HOMING, (char *)(&mode), 1, addr);
}
//---------------------------------------------------------------------------
BOOL ServoHardReset(byte addr, byte mode)
{
return NmcSendCmd(addr, HARD_RESET, (char *)(&mode), 1, 0);
}
//---------------------------------------------------------------------------
  
BOOL ServoSetPhase(byte addr, int padvance, int poffset, int maxpwm)
{
int i;
char mode;
SERVOMOD * p;

if (maxpwm>255 || maxpwm<128) return(false);

p = (SERVOMOD *)(mod[addr].p);
p->ph_adv = (byte)padvance;
p->ph_off = (byte)poffset;


//First set PWM to 0:
ServoLoadTraj(addr, 0x88, 0, 0, 0, 0);  //set PWM to 0 now

//Set phase advance:
for (i=0; i<padvance; i++)
  {
  mode = 0xC0; 	//set dir bit hi
  NmcSendCmd(addr, 0x04, &mode, 1, addr);
  mode = 0x80; 	//set dir bit lo
  NmcSendCmd(addr, 0x04, &mode, 1, addr);
  }
//Toggle PWM high then low:
ServoLoadTraj(addr, 0x88, 0, 0, 0, 255);  //set PWM to 255 now
ServoLoadTraj(addr, 0x88, 0, 0, 0, 0);    //set PWM to 0 now

//Set index phasing count (icount):
for (i=0; i<poffset; i++)
  {
  mode = 0xC0; 	//set dir bit hi
  NmcSendCmd(addr, 0x04, &mode, 1, addr);
  mode = 0x80; 	//set dir bit lo
  NmcSendCmd(addr, 0x04, &mode, 1, addr);
  }

ServoClearBits(addr);  //Clear any position errors

for (i=127; i<=maxpwm; i++)    
  {
  ServoLoadTraj(addr, 0x88, 0, 0, 0, (byte)i);  //ramp up PWM to 255
  if ( (NmcGetStat(addr) & POS_ERR) )     //check if power still there
    {
    ServoLoadTraj(addr, 0x88, 0, 0, 0, 0);   // set PWM to zero on error
    break;
    }
  }

Sleep(500);
return( ServoLoadTraj(addr, 0x88, 0, 0, 0, 0) );    //set PWM to 0 now

}