void turnDegrees(int degrees) // uses a PID loop to turn a given angle (clockwise) - used in autonomous
{
float goal = degrees * TICKS_PER_DEGREE; // conversion factor from degrees to encoder ticks
if (bBlueAlliance == true) // this reflects the turns if we're on the blue alliance
{
goal = goal * -1;
}
int output; // speed to send to the drive motots, set in the loop
initDriveEncoders(); // reset drive encoder counts to zero
InitPidGoal(turnPid, goal); // initialize the drive PID with the goal
ClearTimer(T2); // clear the timer
while( (abs(turnPid.error) > turnErrorThreshold)
|| (abs(turnPid.derivative) > turnDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T2) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float currentPosition = (nMotorEncoder[leftBackDrive]-nMotorEncoder[rightBackDrive]) / 2.0; // (the current position is the (+) average of the drive encoders)
output = UpdatePid(turnPid, currentPosition); // ...update the motor speed with the drive PID...
setDriveMotors(-output, output); // ...and send that speed to the drive motors (left and right)
ClearTimer(T2); // clear the timer
}
}
setDriveMotors(0, 0); // remember to stop the motors!
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
void setArm() // reads Joystick, determines arm motor values and sends values to motors - used in driver control
{ // uses PID to hold the arm up when buttons are released
// note: this function does not decide where the upper/lower limits are, setArmMotors does
int currentPosition = SensorValue(leftArmPot);
if (vexRT[Btn5U] == 1) // if button 5 Up is pressed...
{
bHoldHeight = false; // ...don't hold position, instead...
armPid.goal = LEFT_ARM_UPPER_LIMIT; // ...set the goal to the upper limit.
}
else if (vexRT[Btn5D] == 1) // if button 5 Down is pressed...
{
bHoldHeight = false; // ...don't hold position, instead...
armPid.goal = LEFT_ARM_LOWER_LIMIT; // ...set the goal to the lower limit.
}
else // if neither button is pressed...
{
if (bHoldHeight == false) // ...and we weren't previously holding position...
{
bHoldHeight = true; // ...now we want to hold position, so...
armPid.goal = SensorValue(leftArmPot); // ...set the goal to the current height.
}
}
if (time1(T3) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float output = UpdatePid(armPid, currentPosition); // ...update the motor speed with the arm PID...
setArmMotors(output); // ...and send that speed to the arm motors...
ClearTimer(T3); // ...and reset the timer for the next update.
}
}
void armHeight(float percent) // uses a PID loop to drive the arm to a given height - used in autonomous
{
float goal = ( (percent/100) * (LEFT_ARM_UPPER_LIMIT - LEFT_ARM_LOWER_LIMIT) ) + LEFT_ARM_LOWER_LIMIT;
int output; // speed to send to the arm motots, set in the loop
InitPidGoal(armPid, goal); // initialize the arm PID with the goal
ClearTimer(T3); // clear the timer
while( (abs(armPid.error) > armErrorThreshold)
|| (abs(armPid.derivative) > armDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T3) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
output = UpdatePid(armPid, SensorValue(leftArmPot)); // ...update the motor speed with the arm PID...
setArmMotors(output); // ...and send that speed to the arm motors
ClearTimer(T3);
}
}
// don't set the motors back to 0 afterwards, we want them to hold position
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
void driveInches(int inches) // uses a PID loop to drive (straight) a given distance - used in autonomous
{
float goal = inches * TICKS_PER_INCH; // conversion factor from inches to encoder ticks
int output; // speed to send to the drive motots, set in the loop
initDriveEncoders(); // reset drive encoder counts to zero
InitPidGoal(drivePid, goal); // initialize the drive PID with the goal
ClearTimer(T1); // clear the timer
while( (abs(drivePid.error) > driveErrorThreshold)
|| (abs(drivePid.derivative) > driveDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T1) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float currentPosition = (-nMotorEncoder[leftBackDrive]-nMotorEncoder[rightBackDrive]) / 2.0; // (the current position is the average of the drive encoders)
output = UpdatePid(drivePid, currentPosition); // ...update the motor speed with the drive PID...
float offset = DRIVE_STRAIGHT_FACTOR * ( (-nMotorEncoder[leftBackDrive]) - (-nMotorEncoder[rightBackDrive]) ) ; // (proportional correction to straighten out)
setDriveMotors(output - offset, output + offset); // ...and send that speed to the drive motors (left and right)
ClearTimer(T1); // reset the timer so it starts counting again
}
}
setDriveMotors(0, 0); // remember to stop the motors!
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
//-----------------------------------------------------------------------------
//
void loop()
{
/* Update all the values */
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
//gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
temp = ((double)tempRaw + 12412.0) / 340.0;
// Print Data
Serial.print("accX: "); Serial.print(accX); Serial.print("\t");
Serial.print("accY: "); Serial.print(accY); Serial.print("\t");
Serial.print("accZ: "); Serial.print(accZ); Serial.print("\t");
Serial.print("gyroX: "); Serial.print(gyroX); Serial.print("\t");
Serial.print("gyroY: "); Serial.print(gyroY); Serial.print("\t");
Serial.print("gyroZ: "); Serial.print(gyroZ); Serial.print("\t");
Serial.print(accXangle); Serial.print("\t");
Serial.print(gyroXangle); Serial.print("\t");
Serial.print(compAngleX); Serial.print("\t");
Serial.print("kalAngleY: "); Serial.print(kalAngleX); Serial.print("\t");
Serial.print("\t");
Serial.print(accYangle); Serial.print("\t");
Serial.print(gyroYangle); Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print("kalAngleY: "); Serial.print(kalAngleY); Serial.print("\t");
Serial.print(temp); Serial.print("\t");
int corr = UpdatePid( 178, kalAngleX );
Serial.print(" corr:");
Serial.print(corr);
if( (corr > 0 && prevCmd < 0) ||
(corr < 0 && prevCmd > 0) )
{
m1.Brake();
m2.Brake();
}
prevCmd = corr;
if( corr > 0 )
{
m1.Backward( corr );
m2.Backward( corr );
}
else if( corr < 0 )
{
m1.Forward( -corr );
m2.Forward( -corr );
}
else
{
m1.Coast();
m2.Coast();
}
// LOOP CONTROL
lastLoopUsefulTime = millis() - loopRefTime;
if( lastLoopUsefulTime < FREQ )
{
delay( FREQ - lastLoopUsefulTime );
}
loopRefTime = millis();
Serial.print("\r\n");
}
