void TankDrive::doDrive(int driveTime)
{
_mpuRet = mympu_update();
float baseAngle = mympu.ypr[0];
long startTime = millis();
long cTime = 0;
int cSpeed = 150;
float cAngle = 0;
float correction=0;
float correctLeft=0;
float correctRight=0;
_simpleDeadband = 0.75;
_simpleKp=20;
TrapPath.setRunTime(driveTime);
TrapPath.setStartTime(startTime);
//Using Standard P to control direction
while (millis() - startTime < driveTime) {
_mpuRet = mympu_update();
_currentAngle = mympu.ypr[0];
cSpeed = TrapPath.getSetPoint(millis()-startTime);
cAngle = baseAngle-_currentAngle;
if (_usePID) {
correction = MotorPID.Compute(cAngle,baseAngle);
correctLeft = -correction;
correctRight = correction;
}
else { //use standard Kp multiplier
if (cAngle>_simpleDeadband){
correctLeft = -cAngle*_simpleKp;
correctRight = cAngle*_simpleKp;
}
else if (cAngle<(_simpleDeadband*-1)){
correctLeft = cAngle*_simpleKp;
correctRight = -cAngle*_simpleKp;
}
}
//Serial Monitor report of P follwing
Serial.print("Angle: ");Serial.print(cAngle);
Serial.print(" Speed: ");Serial.print(cSpeed);
//Serial.print(" Correct Left: ");Serial.print(correctLeft);
//Serial.print(" Correct Right: ");Serial.println(correctRight);
Serial.print(" Correct Left: ");Serial.print(constrain(cSpeed+correctLeft,5,255));
Serial.print(" Correct Right: ");Serial.println(constrain(cSpeed+correctRight,5,255));
digitalWrite(_speedPinLeft, constrain(cSpeed+correctLeft,0,255));
digitalWrite(_speedPinRight, constrain(cSpeed+correctRight,0,255));
}
fullStop();
}
int TankDrive::InitializeMPU()
{
int notStable = 1;
int stableRead = 0;
int oldAngle=180;
long calibrationStart = millis();
_mpuRet = mympu_open(200);
Serial.print("MPU init: "); Serial.println(_mpuRet);
Serial.println("Starting MPU stabilization check. This can take a minute.");
Serial.println("The MPU requires 8 seconds of no movement to calibrate the Gyro with Accelerometer.");
delay(8000); //Delay 2 seconds to settle the minitbot for MPU initialization
while (notStable){
_mpuRet = mympu_update();
_currentAngle = mympu.ypr[0];
if (_currentAngle == oldAngle) {
stableRead=stableRead+1;
}
else {
stableRead=0;
}
//Serial.println(_currentAngle);
if (stableRead > 5000 and notStable){
Serial.print("Stablised in ");Serial.print((millis()-calibrationStart)/1000);Serial.print(" seconds");
Serial.print(" at ");Serial.print(_currentAngle);Serial.println(" degrees.");
notStable=0;
}
oldAngle=_currentAngle;
}
}
//*
// Timer2 Interrupt Service Routine
void __ISR(_TIMER_2_VECTOR, ipl2) handlesTimer2Ints(void)
{
if (!flag_writingScreen && (IMU_counter++ > IMU_MAXCOUNT)) {
mympu_update(&mympu);
IMU_counter = 0;
}
mT2ClearIntFlag(); // Clears the interrupt flag so that the program returns to the main loop
} // END Timer2 ISR
void TankDrive::doTurn(int turnDegrees)
{
_mpuRet = mympu_update();
float baseAngle = mympu.ypr[0];
int cSpeed = 120;
float cAngle = 0;
float correction=0;
float correctLeft=0;
float correctRight=0;
while(cAngle < turnDegrees) {
_mpuRet = mympu_update();
_currentAngle = mympu.ypr[0];
cAngle = baseAngle-_currentAngle;
cAngle = abs(cAngle);
digitalWrite(_speedPinLeft, cSpeed);
digitalWrite(_speedPinRight, cSpeed);
}
fullStop();
}
