bool Omni4WD::PIDDisable() {
setCarStat(STAT_UNKNOWN);
_wheelUL->PIDDisable(); _wheelUL->runPWM(0,DIR_ADVANCE);
_wheelLL->PIDDisable(); _wheelLL->runPWM(0,DIR_ADVANCE);
_wheelLR->PIDDisable(); _wheelLR->runPWM(0,DIR_ADVANCE);
_wheelUR->PIDDisable(); _wheelUR->runPWM(0,DIR_ADVANCE);
return false;
}
int Omni4WD::setCarAdvance(int speedMMPS) {
setCarStat(STAT_ADVANCE);
//wheelULSetSpeedMMPS(speedMMPS,DIR_ADVANCE);
//wheelLLSetSpeedMMPS(speedMMPS,DIR_ADVANCE);
//wheelLRSetSpeedMMPS(speedMMPS,DIR_BACKOFF);
//wheelURSetSpeedMMPS(speedMMPS,DIR_BACKOFF);
//return wheelULGetSpeedMMPS();
return setCarMove(speedMMPS,PI/2,0);
}
int Omni4WD::setCarUpperLeft(int speedMMPS) {
setCarStat(STAT_UPPERLEFT);
//speedMMPS*=2;
//wheelULSetSpeedMMPS(0,DIR_ADVANCE);
//wheelLLSetSpeedMMPS(speedMMPS,DIR_ADVANCE);
//wheelLRSetSpeedMMPS(0,DIR_ADVANCE);
//wheelURSetSpeedMMPS(speedMMPS,DIR_BACKOFF);
//return wheelLLGetSpeedMMPS();
return setCarMove(speedMMPS,PI*3/4,0);
}
int Omni4WD::setCarRotateRightDegree(float degree, int speedMMPS)
{
setCarStat(STAT_ROTATERIGHT);
float pi = 3.1415926;
int d = 500; //mm
int ms = round(((degree * pi * d / 360) / speedMMPS) * 1000);
wheelULSetSpeedMMPS(speedMMPS);
wheelLRSetSpeedMMPS(speedMMPS);
wheelLLSetSpeedMMPS(speedMMPS);
wheelURSetSpeedMMPS(speedMMPS);
delayMS(ms);
setCarStop();
}
unsigned int setCarBackoff(unsigned int speedMMPS)
{
unsigned int temp=150+speedMMPS;
setCarStat(STAT_BACKOFF);
return setMotorAll(temp);
}
unsigned int setCarAdvance(unsigned int speedMMPS)
{
unsigned int temp=150-speedMMPS;
setCarStat(STAT_ADVANCE);
return setMotorAll(temp);
}
unsigned int setCarStop()
{
setCarStat(STAT_STOP);
return setMotorAll(150);
}
unsigned int Omni4WD::setCarStop(unsigned int mm) {
setCarStat(STAT_STOP);
return setMotorAllStop();
}
int Omni4WD::setCarRight(int speedMMPS) {
setCarStat(STAT_RIGHT);
return setCarMove(speedMMPS,0,0);
}
int Omni4WD::setCarRotateRight(int speedMMPS) {
setCarStat(STAT_ROTATERIGHT);
return setCarRotate(-speedMMPS/(sqrt(pow(getWheelspan()/2,2)*2)));
}
int Omni4WD::setCarRotateLeft(int speedMMPS) {
setCarStat(STAT_ROTATELEFT);
return setCarRotate(speedMMPS/(sqrt(pow(getWheelspan()/2,2)*2)));
}
int Omni4WD::setCarUpperRight(int speedMMPS) {
setCarStat(STAT_UPPERRIGHT);
return setCarMove(speedMMPS,PI/4,0);
}
int Omni4WD::setCarLowerRight(int speedMMPS) {
setCarStat(STAT_LOWERRIGHT);
return setCarMove(speedMMPS,PI*7/4,0);
}
int Omni4WD::setCarLowerLeft(int speedMMPS) {
setCarStat(STAT_LOWERLEFT);
return setCarMove(speedMMPS,PI*5/4,0);
}
unsigned int setCarUpperLeft(unsigned int speedMMPS,unsigned int radiusMM)
{
setCarStat(STAT_UPPERLEFT);
return setCarArcBase(speedMMPS,radiusMM);
}
unsigned int setCarUpperRight(unsigned int speedMMPS,unsigned int radiusMM)
{
setCarStat(STAT_UPPERRIGHT);
return setCarArcBase(speedMMPS,radiusMM);
}
int Omni4WD::setCarBackoff(int speedMMPS) {
setCarStat(STAT_BACKOFF);
return setCarMove(speedMMPS,PI*3/2,0);
}