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