void compassTurn(int heading)
{
setTarget(normalizeTheta360(heading));
while (abs(readRelativeCompass())>2)
{
nxtDisplayTextLine(2, "%f, %d, %f", readRelativeCompass(), heading, readCompass());
int dif=-readRelativeCompass();
int sign;
if (dif<0) sign=-1;
else sign=1;
dif=abs(dif);
if(abs(dif)<25)
{
motor[frontLeft] =sign*-20;
motor[frontRight]=sign*-20;
motor[backLeft] =sign*20;
motor[backRight] =sign*20;
}
else
{
motor[frontLeft] =sign*-dif*4/(float)5;
motor[frontRight]=sign*-dif*4/(float)5;
motor[backLeft] =sign*dif*4/(float)5;
motor[backRight] =sign*dif*4/(float)5;
}
}
motor[frontLeft]=0;
motor[frontRight]=0;
motor[backLeft]=0;
motor[backRight]=0;
//pause();
}
void writeTeleopForwards()
{
TFileHandle hFileHandle;
TFileIOResult nIoResult;
float teleopForwards=readCompass();
short nFileSize=sizeof(teleopForwards);
OpenWrite(hFileHandle, nIoResult, TELEOPFORWARDSDAT, nFileSize);
WriteFloat(hFileHandle, nIoResult, teleopForwards);
Close(hFileHandle, nIoResult);
}
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 Compass::refresh(){
readCompass();
}
float readRelativeCompass()
{
//Taken from HTMC-driver.h
int temp = readCompass() - compassTarget + 180;
return (temp >= 0 ? temp % 360 : 359 - (-1 - temp)%360) - 180;
}
