unsigned int setCarArcBase(unsigned int speedMMPS,unsigned int radiusMM)
{
unsigned int V1,V2,delta1,delta2;
delta1=(int)(0.5/radiusMM*16+0.5);//上下浮动范围为8，整体范围16，后面加0.5是为了四舍五入
if(delta1>8) delta1=8;
delta2=delta1;
if(delta2>2) delta2=delta2+27; //越过空白间隙 137为正转初值，165为倒转初值
V1= speedMMPS - delta1; //脉宽减小，正向加速
V2= speedMMPS + delta2; //脉宽增大，减速，直至反向
// V1=150-(speedMMPS-delta);//转弯半径限制在1-16米，由最小分辨率决定
// V2=150-(speedMMPS+delta);
switch(getCarStat())
{
case STAT_UPPERLEFT:
setCarBase(V1,V2); break;
case STAT_UPPERRIGHT:
setCarBase(V2,V1); break;
}
return speedMMPS;
}
int Omni4WD::setCarSpeedMMPS(int speedMMPS,unsigned int ms) {
unsigned int carStat=getCarStat();
int currSpeed=getCarSpeedMMPS();
int (Omni4WD::*carAction)(int speedMMPS);
switch(carStat) {
case STAT_UNKNOWN: // no break here
case STAT_STOP:
return currSpeed;
case STAT_ADVANCE:
carAction=&Omni4WD::setCarAdvance; break;
case STAT_BACKOFF:
carAction=&Omni4WD::setCarBackoff; break;
case STAT_LEFT:
carAction=&Omni4WD::setCarLeft; break;
case STAT_RIGHT:
carAction=&Omni4WD::setCarRight; break;
case STAT_ROTATELEFT:
carAction=&Omni4WD::setCarRotateLeft; break;
case STAT_ROTATERIGHT:
carAction=&Omni4WD::setCarRotateRight; break;
case STAT_UPPERLEFT:
carAction=&Omni4WD::setCarUpperLeft; break;
case STAT_LOWERLEFT:
carAction=&Omni4WD::setCarLowerLeft; break;
case STAT_LOWERRIGHT:
carAction=&Omni4WD::setCarLowerRight; break;
case STAT_UPPERRIGHT:
carAction=&Omni4WD::setCarUpperRight; break;
}
if(ms<100 || abs(speedMMPS-currSpeed)<10) {
(this->*carAction)(speedMMPS);
return getCarSpeedMMPS();
}
for(int time=20,speed=currSpeed;time<=ms;time+=20) {
speed=map(time,0,ms,currSpeed,speedMMPS);
(this->*carAction)(speed);
delayMS(20);
}
(this->*carAction)(speedMMPS);
return getCarSpeedMMPS();
}
// 201208
float Omni4WD::getCarSpeedRad() const { // Omega
unsigned char carStat=getCarStat();
switch(carStat) {
case STAT_STOP:
case STAT_ADVANCE:
case STAT_BACKOFF:
case STAT_LEFT:
case STAT_RIGHT:
case STAT_LOWERLEFT:
case STAT_UPPERRIGHT:
case STAT_UPPERLEFT:
case STAT_LOWERRIGHT:
return 0;break;
case STAT_ROTATELEFT:
case STAT_ROTATERIGHT:
return wheelULGetSpeedMMPS()/sqrt(pow(getWheelspan()/2,2)*2*2); break;
//return abs(wheelULGetSpeedMMPS()/getWheelspan()); break;
case STAT_UNKNOWN: // Not implemented yet
break;
}
return 0;
}
