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
}
/*
* Runs user initialization code. This function will be started in its own task
* with the default
* priority and stack size once when the robot is starting up. It is possible
* that the VEXnet
* communication link may not be fully established at this time, so reading from
* the VEX
* Joystick may fail.
*
* This function should initialize most sensors (gyro, encoders, ultrasonics),
* LCDs, global
* variables, and IMEs.
*
* This function must exit relatively promptly, or the operatorControl() and
* autonomous() tasks
* will not start. An autonomous mode selection menu like the pre_auton() in
* other environments
* can be implemented in this task if desired.
*/
void initialize() {
print("[Init] Initializing the robot\n");
// Set up the LCD and start it
print("[Init] Setting up the LCD\n");
lcdInitMenu(1, 3, 1); // Min 1, max 3, home 1
lcdSetUpdater(implUpdateLCD);
lcdSetMenuBack(implMenuBack);
lcdSetMenuNext(implMenuNext);
lcdSetCycles(true);
// Set up our drive
print("[Init] Setting up drive motors\n");
lcdSetText(uart2, 1, "Init drive...");
driveInit(MOTOR_DRIVE_FL, MOTOR_DRIVE_BL, MOTOR_DRIVE_FR, MOTOR_DRIVE_BR);
driveSetReverse(MOTOR_DRIVE_FL_REV, MOTOR_DRIVE_BL_REV, MOTOR_DRIVE_FR_REV,
MOTOR_DRIVE_BR_REV);
// enableSlew(15); // Set slew rate to 15
// Set up our autonomous to these modes
print("[Init] Setting up autonomous modes\n");
lcdSetText(uart2, 1, "Init auton...");
autonInit(4); // 3 auton modes
// Set up our gyroscope
print("[Init] Setting gyroscope\n");
lcdSetText(uart2, 1, "Init gyro...");
// To tune: 196*((360*rotations)/gyroValue)
gyro = gyroInit(ANALOG_GYRO, 190); // default is 196, this is after tune
// Set up our encoders
print("[Init] Setting up encoders\n");
lcdSetText(uart2, 1, "Init Encs...");
initDriveEncoders();
// initPidControl();
// Sets communication port for JINX data and start task to parse incoming
// messages.
print("[Init] Setting up JINX\n");
lcdSetText(uart2, 1, "Init JINX...");
initJINX(stdout);
delay(100);
taskCreate(JINXRun, TASK_DEFAULT_STACK_SIZE, NULL, (TASK_PRIORITY_DEFAULT));
delay(100);
// Done init
print("[Init] Finished, starting LCD menu\n");
lcdSetText(uart2, 1, "Init menu...");
lcdStartMenu();
// TODO Remove
hc05Init(1, false);
}
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
}
