void Pheeno::sensorFusionPositionUpdate(float timeStep, float northOffset){
if (millis() - positionUpdateTimeStart >= timeStep){
timeStep = (millis() - positionUpdateTimeStart)/1000; //Convert ms to s
positionUpdateTimeStart = millis();
readEncoders();
int countL = encoderCountL;
int countR = encoderCountR;
float Dr = pi * wheelDiameter * (countR - oldSFPUEncoderCountR) / (encoderCountsPerRotation * motorGearRatio); // Linear distance right wheel has rotated.
float Dl = pi * wheelDiameter * (countL - oldSFPUEncoderCountL) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
//Check integer roll over!
if (countL < 0 && oldSFPUEncoderCountL > 0){
Dl = pi*wheelDiameter * ((countL - (-32768)) + (32767 - oldSFPUEncoderCountL)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
if (countR < 0 && oldEPUEncoderCountR > 0){
Dr = pi*wheelDiameter * ((countR - (-32768)) + (32767 - oldSFPUEncoderCountR)) / (encoderCountsPerRotation * motorGearRatio); // Linear distance left wheel has rotated.
}
float Dc = (Dr + Dl)/2; //Distance center of bot has moved read by encoders.
oldSFPUEncoderCountR = countR;
oldSFPUEncoderCountL = countL;
float botD = 0.8 * Dc + 0.2 * botVel * timeStep;
readCompass(northOffset);
if (botA0 > -pi/2 && botA0 < pi/2){
botA = 0.9 * wrapToPi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapToPi(deg2rad(IMUOrientation));
}
else{
botA = 0.9 * wrapTo2Pi(botA + (Dr - Dl)/axelLength) + 0.1 * wrapTo2Pi(deg2rad(IMUOrientation));
}
botXPos += botD * cos(botA0 + (botA-botA0)/2);
botYPos += botD * sin(botA0 + (botA-botA0)/2);
readAccel();
botVel = 0.95 * Dc/timeStep + 0.05 * (botVel + IMUACCY * timeStep);
botA0 = botA;
}
}
void collisioncone_findnewcmd( float cc[2][6],
float *v_des, float *psi_des,
float psisearch, int nfilters )
{
int i;
int count = 1;
// int ng = 1;
bool flag = true;
float psi_add;
deg2rad(psisearch, &psi_add);
float psi0 = *psi_des;
float vx, vy;
while (*v_des <= 2.0) {
polar2cart(*v_des, *psi_des, &vx, &vy);
for (i = 0; i < nfilters; i++) { // Check if we succeed
flag = collisioncone_checkdanger(cc[i], vx, vy);
if (flag)
break;
}
if(!flag) // No issues found
return;
*psi_des = psi0 + (count * psi_add);
wrapTo2Pi(psi_des);
// ng = ng * -1;
count++;
if (count >= (2*M_PI)/psi_add) {
*v_des = *v_des + *v_des;
count = 1;
}
}
// Failed to find a solution, so just stop this time
*v_des = 0.0;
};
inline T wrapToPi(T a)
{
return wrapTo2Pi( a + static_cast<T>(M_PI) )-static_cast<T>(M_PI);
}
