DMPAPI(unsigned int) usb_Read(void *vusb)
{
BYTE val;
DWORD pretime;
USB_Device *usb = (USB_Device *)vusb;
if (usb == NULL) { err_print((char*)"%s: USB device is null.\n", __FUNCTION__); return 0xffff; }
if (usb->TimeOut != USB_NO_TIMEOUT) {
pretime = timer_nowtime();
while (usb->rcvd->count <= 0 && (timer_nowtime() - pretime) < usb->TimeOut);
if (usb->rcvd->count <= 0) {
if (USB_TIMEOUT_DEBUG)
err_print((char*)"%s: USB device receive timeout.\n", __FUNCTION__);
return (unsigned int)0xffff;
}
}
else while (usb->rcvd->count <= 0);
io_DisableINT();
{
val = (BYTE)PopQueue(usb->rcvd);
}
io_RestoreINT();
if (!(io_inpdw(usb->EP[2].OutDLR) & 0x80000000L) && usb->rcvd->count < (usb->rcvd->size - NEAR_FULL_SIZE))
SetEPnDLR(usb, EP2, OUT, ENABLE | EP2_MAX_PACKET_SIZE_OUT);
return (unsigned int)val;
}
RBAPI(bool) i2cslave_Write(int dev, unsigned char val) {
unsigned long nowtime;
if (I2C_action[dev] != I2CACT_SLAVEWRITEREQ)
{
err_SetMsg(ERROR_I2CWRONGUSAGE, "slave must write data only when being requested");
return false;
}
i2cslave_ClearSlaveWREQ(dev);
i2c_WriteDataREG(dev, val);
for (nowtime=0; nowtime<100000L; nowtime++) //trick for performance; timer_nowtime() is time-consuming in some OS
if (i2c_CheckTXDone(dev) == true) goto TX_SUCCESS;
nowtime = timer_nowtime();
while ((i2c_CheckTXDone(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));
if (i2c_CheckTXDone(dev) == false)
{
err_SetMsg(ERROR_I2CFAIL, "the I2C module doesn't respond");
I2C_action[dev] = I2CACT_IDLE;
return false;
}
TX_SUCCESS:
i2c_ClearTXDone(dev);
I2C_action[dev] = I2CACT_SLAVE;
return true;
}
_RB_INLINE bool check_RX_done(int dev) {
unsigned long nowtime;
unsigned char statreg;
for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
if (i2c_CheckRXRdy(dev) == true) goto RX_SUCCESS;
nowtime = timer_nowtime();
while ((i2c_CheckRXRdy(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));
RX_SUCCESS:
statreg = i2c_ReadStatREG(dev); //ugly code for speed:p
i2c_ClearRXRdy(dev);
if ((statreg & 0x08) != 0) //if (i2cmaster_CheckARLoss(dev) == true)
{
err_SetMsg(ERROR_I2CARLOSS, "arbitration loss for I2C bus");
i2cmaster_ClearARLoss(dev);
return false;
}
if ((statreg & 0x01) == 0) //if (i2c_IsMaster(dev) == false)
{
err_SetMsg(ERROR_I2CFAIL, "I2C%d module isn't in Master Mode", dev);
return false;
}
if ((statreg & 0x40) == 0) //if (i2c_CheckRXRdy(dev) == false)
{
err_SetMsg(ERROR_I2CFAIL, "I2C%d module doesn't respond", dev);
return false;
}
return true;
}
int analogRead(uint8_t pin) {
unsigned int d;
unsigned long time;
int i;
if(pin > A6) return 0xffff;
while((io_inpb(BaseAddress + 2) & 0x01) != 0)
io_inpb(BaseAddress + 4);
io_outpb(BaseAddress + 1, 0x08); // enable ADC_PD & ADC_ST
io_outpb(BaseAddress + 0, (0x01<<pin)); // enable channel 0
io_outpb(BaseAddress + 1, 0x01); // enable ADC_PD & ADC_ST
time = timer_nowtime();
while(1)
{
if(timer_nowtime() - time > TimeOut)
{
printf("timeout!!\n");
return 0xffff;
}
if((io_inpb(BaseAddress + 2) & 0x01) != 0) //data_Rdy
{
d = io_inpw(BaseAddress + 4);// read AUX_NO & ADC_Data
break;
}
}
d = (d&0x07ff)>>1;
return d;
}
RBAPI(bool) com_Send(int com, unsigned char* buf, int bsize) {
unsigned long nowtime;
int numbytes = 0, bsize2;
if (COM_duplex[com] == COM_HDUPLEX_RTS)
{
MPOS_Start();
set_rts(com);
}
while (bsize > 0)
{
bsize2 = (bsize <= MAXRWSIZE)? bsize : MAXRWSIZE;
for (nowtime = timer_nowtime(); bsize2 > 0; buf += numbytes, bsize2 -= numbytes, bsize -= numbytes)
{
#if defined(RB_MSVC_WIN32) || defined(RB_MSVC_WINCE)
if (WriteFile(COM_info[com].fp, buf, bsize2, (LPDWORD)&numbytes, NULL) == FALSE)
{
err_SetMsg(ERROR_COM_SENDFAIL, "WriteFile() fails");
goto SEND_FAIL;
}
#elif defined(RB_LINUX)
if ((numbytes = write(COM_info[com].fp, buf, bsize2)) < 0)
{
err_SetMsg(ERROR_COM_SENDFAIL, "write() fails");
goto SEND_FAIL;
}
#else
// TODO ...
err_SetMsg(ERROR_COM_INVALID, "unsupported platform");
goto SEND_FAIL;
#endif
if ((timer_nowtime() - nowtime) > COM_TIMEOUT)
{
err_SetMsg(ERROR_COM_SENDFAIL, "time-out to write bytes");
goto SEND_FAIL;
}
} // for (nowtime...
} // end while (bsize...
if (COM_duplex[com] == COM_HDUPLEX_RTS)
{
com_FlushWFIFO(com);
clear_rts(com);
MPOS_End();
}
return true;
SEND_FAIL:
if (COM_duplex[com] == COM_HDUPLEX_RTS)
{
clear_rts(com);
MPOS_End();
}
return false;
}
void Prepare(void)
{
int i;
GetLegOrigin(LEGORIGIN);
Update();
switch(STATE)
{
case IDLE:
for(i = 0; i < 6; i++)
{
LEGGOAL[i*3] = LEGORIGIN[i*3];
LEGGOAL[i*3+1] = LEGORIGIN[i*3+1];
LEGGOAL[i*3 + 2] = LEGORIGIN[i*3 + 2] + GOAL_HIGHT;
}
break;
case BALANCE:
InitSensor();
for(i = 0; i < 6; i++)
{
LEGGOAL[i*3] = LEGORIGIN[i*3];
LEGGOAL[i*3+1] = LEGORIGIN[i*3+1];
LEGGOAL[i*3 + 2] = LEGORIGIN[i*3 + 2];
}
SetOrigin(NORMAL, POSITION);
STABLETIMER = timer_nowtime();
break;
default:
if(SWING == 0)
SetOrigin(NORMAL, POSITION);
for(i = 0; i < 6; i++)
{
LEGFRAME[i] = -1;
GetLegGoal(GOALPOS, i, LEGGOAL);
GetLegOpposite(GOALPOS, i, STATE,LEGOPPOSITE);
if(SWING == 1)
{
LEGGOAL[i*3 + 2] = LEGGOAL[i*3 + 2] + 10.0;
LEGOPPOSITE[i*3 + 2] = LEGOPPOSITE[i*3 + 2] + GOAL_HIGHT;
}
if(STATE == RCIRCLE || STATE == LCIRCLE)
{
Transform(ROTATE, GOAL_PITCH, GOAL_ROLL, LEGGOAL+3*i);
Transform(-ROTATE, GOAL_PITCH, GOAL_ROLL, LEGOPPOSITE+3*i);
}
PLAYTIMER[i] = timer_nowtime();
}
break;
}
}
RBAPI(bool) com_Receive(int com, unsigned char* buf, int bsize) {
int numbytes, bsize2;
unsigned long nowtime;
for (; bsize > 0; bsize -= bsize2, buf += bsize2)
{
bsize2 = (bsize <= MAXRWSIZE)? bsize : MAXRWSIZE;
// wait enough bytes to read
for (nowtime = timer_nowtime(); (numbytes = check_rfifo(com)) < bsize2; )
{
if (numbytes < 0)
{
err_SetMsg(ERROR_COM_READFAIL, "fail to check read FIFO");
return false;
}
if ((timer_nowtime() - nowtime) > COM_TIMEOUT)
{
err_SetMsg(ERROR_COM_READFAIL, "time-out to read bytes");
return false;
}
} // end for (nowtime...
#if defined(RB_MSVC_WIN32) || defined(RB_MSVC_WINCE)
if (ReadFile(COM_info[com].fp, buf, bsize2, (LPDWORD)&numbytes, NULL) == FALSE)
{
err_SetMsg(ERROR_COM_READFAIL, "ReadFile() fails");
return false;
}
#elif defined(RB_LINUX)
if ((numbytes = read(COM_info[com].fp, buf, bsize2)) < 0)
{
err_SetMsg(ERROR_COM_READFAIL, "read() fails");
return false;
}
#else
// TODO ...
err_SetMsg(ERROR_COM_INVALID, "unsupported platform");
return false;
#endif
if (numbytes != bsize2)
{
err_SetMsg(ERROR_COM_READFAIL, "cannot read enough bytes");
return false;
}
} // end for (; bsize...
return true;
}
void Stable(void)
{
static double gval[3];
static double pre_pitch, pitch, pre_roll, roll;
int i;
if(timer_nowtime() - STABLETIMER > FRESHTIME)
{
ReadGSensor();
gval[0] = (double)(G_AXIS_VALUE[0]);
gval[1] = (double)(G_AXIS_VALUE[1]);
gval[2] = (double)(G_AXIS_VALUE[2]);
pitch = (GetPitch(gval)- 5.0*DEG_TO_RAD)*0.5 + pre_pitch*0.5;
roll = (GetRoll(gval) - 4.5*DEG_TO_RAD)*0.5 + pre_roll*0.5;
BODY_PITCH = BODY_PITCH + STABLE_P*pitch + (pitch - pre_pitch)*STABLE_D/FRESHTIME;
BODY_ROLL = BODY_ROLL + STABLE_P*roll + (roll - pre_roll)*STABLE_D/FRESHTIME;
if(BODY_PITCH > 60.0*DEG_TO_RAD)
BODY_PITCH = 60.0*DEG_TO_RAD;
else if(BODY_PITCH < -60.0*DEG_TO_RAD)
BODY_PITCH = -60.0*DEG_TO_RAD;
if(BODY_ROLL > 60.0*DEG_TO_RAD)
BODY_ROLL = 60.0*DEG_TO_RAD;
else if(BODY_ROLL < -60.0*DEG_TO_RAD)
BODY_ROLL = -60.0*DEG_TO_RAD;
for(i =0 ; i < 6; i++ )
{
LEGPOS[0] = LEGGOAL[3*i] - cos(J3MAP[i])*20.0;
LEGPOS[1] = LEGGOAL[3*i+1] - sin(J3MAP[i])*20.0;
LEGPOS[2] = LEGGOAL[3*i+2];
Transform(BODY_YAW, BODY_PITCH, BODY_ROLL, LEGPOS);
AdjustPos(POS_GAIN, LEGPOS);
CenterToJoint3(LEGPOS,i,J3GOAL);
Joint3ToJoint2(J3GOAL, J2GOAL);
JointAngle(LEGPOS, J2GOAL,i,JANGLE);
SetJoint(JANGLE,i);
rcservo_SetAction(POSITION, STABLETIME);
STABLETIMER = timer_nowtime();
}
pre_pitch = pitch;
pre_roll = roll;
}
rcservo_PlayAction();
}
_RB_INLINE bool check_STOP_done(int dev) {
unsigned long nowtime;
for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
if (i2cmaster_CheckStopBit(dev) == false) goto STOP_SUCCESS;
nowtime = timer_nowtime();
while ((i2cmaster_CheckStopBit(dev) == true) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));
if (i2cmaster_CheckStopBit(dev) == true)
{
err_SetMsg(ERROR_I2CFAIL, "fail to stop the transaction");
return false;
}
STOP_SUCCESS:
return true;
}
DMPAPI(bool) i2c_Reset(int dev) {
unsigned long nowtime;
io_outpb(I2C_EXCTRL_REG(dev), io_inpb(I2C_EXCTRL_REG(dev)) | 0x80);
nowtime = timer_nowtime();
while (((io_inpb(I2C_EXCTRL_REG(dev)) & 0x80) != 0) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));
if ((io_inpb(I2C_EXCTRL_REG(dev)) & 0x80) != 0)
{
err_SetMsg(ERROR_I2CFAIL, "fail to reset I2C module %d", dev);
return false;
}
//Remarks: due to Vortex86DX's poor I2C design, RESET bit may become 0 when dummy clocks are still being output
delay_ms(20L); //lazy trick to tackle the above issue
return true;
}
DMPAPI(int) usb_Send(void *vusb, BYTE *buf, int bsize)
{
int i;
DWORD pretime;
USB_Device *usb = (USB_Device *)vusb;
if (usb == NULL) { err_print((char*)"%s: USB device is null.\n", __FUNCTION__); return 0; }
// if (usb->state != USB_DEV_CONFIGURED)
// {
// err_print((char*)"%s: USB device is not ready.\n", __FUNCTION__);
// return 0;
// }
for (i = 0; i < bsize; i++)
{
if (usb->TimeOut != USB_NO_TIMEOUT) {
pretime = timer_nowtime();
while (usb->xmit->count >= usb->xmit->size && (timer_nowtime() - pretime) < usb->TimeOut);
if (usb->xmit->count >= usb->xmit->size) {
if (USB_TIMEOUT_DEBUG)
err_print((char*)"%s: USB device transmit timeout.\n", __FUNCTION__);
EP2_InHandler(usb);
return i;
}
}
else while (usb->xmit->count >= usb->xmit->size);
io_DisableINT();
{
PushQueue(usb->xmit, buf[i]);
if ((i == (bsize - 1) || usb->xmit->count >= usb->xmit->size) && usb->state == USB_DEV_CDC_CONNECT)
{
EP2_InHandler(usb);
}
}
io_RestoreINT();
}
return i;
}
DMP_INLINE(bool) check_TX_done(int dev) {
unsigned long nowtime;
unsigned char statreg;
for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
if (i2c_CheckTXDone(dev) == true) goto TX_SUCCESS;
nowtime = timer_nowtime();
while ((i2c_CheckTXDone(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));
TX_SUCCESS:
statreg = i2c_ReadStatREG(dev); //ugly code for speed:p
i2c_ClearTXDone(dev);
if ((statreg & 0x08) != 0) //if (i2cmaster_CheckARLoss(dev) == true)
{
err_SetMsg(ERROR_I2CARLOSS, "arbitration loss for I2C bus");
i2cmaster_ClearARLoss(dev);
}
if ((statreg & 0x10) != 0) //if (i2cmaster_CheckAckErr(dev) == true)
{
err_SetMsg(ERROR_I2CACKERR, "receive no ACK after transmitting");
i2cmaster_ClearAckErr(dev);
}
if ((statreg & (0x10 + 0x08)) != 0) return false;
if ((statreg & 0x01) == 0) //if (i2c_IsMaster(dev) == false)
{
err_SetMsg(ERROR_I2CFAIL, "I2C%d module isn't in Master Mode", dev);
return false;
}
if ((statreg & 0x20) == 0) //if (i2c_CheckTXDone(dev) == false)
{
err_SetMsg(ERROR_I2CFAIL, "I2C%d module doesn't respond", dev);
return false;
}
return true;
}
unsigned long millis() {
return timer_nowtime();
}
void PlayMotion(void)
{
int i;
for(i =0; i < 6; i++)
{
switch(LEGFRAME[i])
{
case -1:
if(timer_nowtime() - PLAYTIMER[i] > STARTTIME[i])
{
LEGFRAME[i] = 0;
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
case 11:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = 0;
LEGTIMER[i] = timer_nowtime();
}
break;
case 0:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
case 9:
case 1:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i] + STEP_1;
LEGTIMER[i] = timer_nowtime();
}
break;
case 3:
case 6:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i] + STEP_2;
LEGTIMER[i] = timer_nowtime();
}
break;
default:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
}
}
rcservo_PlayAction();
}
unsigned long long int Encoder::_pulseIn(uint8_t pin, uint8_t state, unsigned long timeout) {
unsigned long data, _timeout;
unsigned long long int povdata = 0L, result = 0L;
int stat;
if(_pcapAttchINT == true || mode != MODE_CAPTURE || pin > 2) return 0L;
_timeout = timer_nowtime() + timeout;
if(state == HIGH)
{
while(1)
{
if(timeout != 0L && timer_nowtime() > _timeout) return 0L;
if(readCapStat[pin](mcn, MCSIF_MODULEB) != MCENC_CAPFIFO_EMPTY &&
readCapFIFO[pin](mcn, MCSIF_MODULEB, &data) == MCPFAU_CAP_0TO1EDGE)
break;
}
while(1)
{
if(timeout != 0L && timer_nowtime() > _timeout) return 0L;
if(readCapStat[pin](mcn, MCSIF_MODULEB) != MCENC_CAPFIFO_EMPTY)
{
stat = readCapFIFO[pin](mcn, MCSIF_MODULEB, &data);
if(stat == MCPFAU_CAP_CAPCNT_OVERFLOW)
povdata += 0x10000000L;
else if(stat == MCPFAU_CAP_1TO0EDGE)
{
result = data + povdata;
break;
}
}
}
}
else
{
while(1)
{
if(timeout != 0L && timer_nowtime() > _timeout) return 0L;
if(readCapStat[pin](mcn, MCSIF_MODULEB) != MCENC_CAPFIFO_EMPTY &&
readCapFIFO[pin](mcn, MCSIF_MODULEB, &data) == MCPFAU_CAP_1TO0EDGE)
break;
}
while(1)
{
if(timeout != 0L && timer_nowtime() > _timeout) return 0L;
if(readCapStat[pin](mcn, MCSIF_MODULEB) != MCENC_CAPFIFO_EMPTY)
{
stat = readCapFIFO[pin](mcn, MCSIF_MODULEB, &data);
if(stat == MCPFAU_CAP_CAPCNT_OVERFLOW)
povdata += 0x10000000L;
else if(stat == MCPFAU_CAP_0TO1EDGE)
{
result = data + povdata;
break;
}
}
}
}
return result;
}