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 }