void driveForward(int x) {
int imeAccumulator = 0;
imeReset(0); // rest rightLine I.M.E
//Read decoder into counts
imeGet(0, &imeAccumulator);
//Move forward at max speed until desired x
while (abs(imeAccumulator) < x) {
motorSet(MOTOR1, FORWARD_VELOCITY);
motorSet(MOTOR2, FORWARD_VELOCITY);
motorSet(MOTOR10, FORWARD_VELOCITY);
motorSet(MOTOR9, FORWARD_VELOCITY);
imeGet(0, &imeAccumulator); // keep getting the value
}
//Cancel forward inertia
motorSet(MOTOR1, -INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR2, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR10, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR9, -INERTIA_CANCELLATION_FACTOR);
delay(65);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -MOTOR_TURN_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -MOTOR_TURN_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -10) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 10) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 10) ;
delay (45);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 7) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 7) ;
delay (65);
motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
delay (200);
}
void scrapeRightBack(int x) {
int counts = 0;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, -90);
motorSet(MOTOR9, -90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR1, 0); // no intertia
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 10);
motorSet(MOTOR9, 10);
delay(55);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void scrapeLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 90) ;
motorSet (motor2,90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -10) ; // no intertia
motorSet (motor2, -10) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (55);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (200);
}
void turnRight(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, -64);
motorSet(MOTOR2, -64);
motorSet(MOTOR10, 64);
motorSet(MOTOR9, 64);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, 10); // no inertia
motorSet(MOTOR2, 10);
motorSet(MOTOR10, -10);
motorSet(MOTOR9, -10);
delay(45);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void vTaskPIController() {
//initialize values
mutexTake(piSetpointMutex, -1);
leftMotor.position=0; leftMotor.setpoint=0; leftMotor.output = 0; leftMotor.i_accum = 0;
rightMotor.position=0; rightMotor.setpoint=0; rightMotor.output = 0; rightMotor.i_accum = 0;
mutexGive(piSetpointMutex);
unsigned long taskWakeTime;
//update PI control every 25ms
while(1) {
taskWakeTime = millis();
// take the mutex
mutexTake(piSetpointMutex, -1);
// update odometry
imeGet(LEFT_IME_ADDR, &(leftMotor.position));
imeGet(RIGHT_IME_ADDR, &(rightMotor.position));
// compute control laws
pi_update(&leftMotor);
pi_update(&rightMotor);
// set motor outputs
motorSet(LEFT_MOTOR_CHANNEL, leftMotor.output);
motorSet(RIGHT_MOTOR_CHANNEL, rightMotor.output);
// release mutex
mutexGive(piSetpointMutex);
// sleep for 25 milliseconds
taskDelayUntil(&taskWakeTime, 25);
}
}
void driveBack(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, REVERSE_VELOCITY);
motorSet(MOTOR2, REVERSE_VELOCITY);
motorSet(MOTOR10, REVERSE_VELOCITY);
motorSet(MOTOR9, REVERSE_VELOCITY);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR2, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR10, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR9, INERTIA_CANCELLATION_FACTOR);
delay(65);
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void turnLeft(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 64) ;
motorSet (motor2, 64) ;
motorSet (motor10, -64) ;
motorSet (motor9, -64) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -10) ; // no inertia
motorSet (motor2, -10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (45);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void scrapeRight(int x) {
int counts = 0;
imeReset(1); // rest left I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 90);
motorSet(MOTOR9, 90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, -10);
motorSet(MOTOR9, -10);
delay(55);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet(MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_MAX);
motorSet(MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_MAX);
motorSet(MOTOR_DRIVE_LEFT_LINE_BACK, MOTOR_MAX);
motorSet(MOTOR_DRIVE_LEFT_LINE_FRONT, MOTOR_MAX);
imeGet(0, &counts); // keep getting the value
}
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -7) ; // no inertia
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -7) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -7) ;
delay (65);
motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 0) ;
motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 0) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 0) ;
motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 0) ;
delay(200);
}
void driveBack(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -100) ;
motorSet (motor2, -100) ;
motorSet (motor10, -100) ;
motorSet (motor9, -100) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 7) ; // no inertia
motorSet (motor2, 7) ;
motorSet (motor10, 7) ;
motorSet (motor9, 7) ;
delay (65);
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void driveBackSlow(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, -40);
motorSet(MOTOR2, -40);
motorSet(MOTOR10, -40);
motorSet(MOTOR9, -40);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR1, 10); // no inertia
motorSet(MOTOR2, 10);
motorSet(MOTOR10, 10);
motorSet(MOTOR9, 10);
delay(45);
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
delay(200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 127) ;
motorSet (motor2, 127) ;
motorSet (motor10, 127) ;
motorSet (motor9, 127) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, -7) ; // no inertia
motorSet (motor2, -7) ;
motorSet (motor10, -7) ;
motorSet (motor9, -7) ;
delay (65);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0) ;
motorSet (motor9, 0) ;
delay(200);
}
void LiftTask(void *ignore) {
int counts;
while (1) {
imeGet(IME_LIFT, &counts); // get lift encoder count
if (joystickGetDigital(1, 8, JOY_UP) || joystickGetDigital(2, 8, JOY_UP)) { // check stash waypoint
liftPIDRequestedValue = LIFT_STASH_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_LEFT) || joystickGetDigital(2, 8, JOY_LEFT)) { // check bump waypoint
liftPIDRequestedValue = LIFT_BUMP_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_DOWN) || joystickGetDigital(2, 8, JOY_DOWN)) { // check floor waypoint
liftPIDRequestedValue = LIFT_FLOOR_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_RIGHT) || joystickGetDigital(2, 8, JOY_RIGHT)) { // check hang waypoint
liftPIDRequestedValue = LIFT_HANG_HEIGHT;
} else if (joystickGetDigital(1, 6, JOY_UP) || joystickGetDigital(2, 6, JOY_UP)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
mutexTake(liftMutex, ULONG_MAX);
liftSet(127);
liftPIDRequestedValue = counts;
mutexGive(liftMutex);
} else if (joystickGetDigital(1, 6, JOY_DOWN) || joystickGetDigital(2, 6, JOY_DOWN)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
mutexTake(liftMutex, ULONG_MAX);
liftSet(-127);
liftPIDRequestedValue = counts;
mutexGive(liftMutex);
} else {
liftSet(0);
}
taskDelay(20);
}
}
int integratedEncoderGet(IntegratedEncoder encoder) {
int value;
imeGet(encoder.imeAddress, &value);
if (encoder.inverted) {
value = -value;
}
return value;
}
void driveBackSlow(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -40) ;
motorSet (motor2, -40) ;
motorSet (motor10, -40);
motorSet (motor9, -40) ;
imeGet(0, &counts); // keep getting the value
}
delay (200);
}
int getIME(IME ime)
{
int value;
imeGet(ime.port, &value);
return (ime.inverted) ? -value : value;
}
void scrapeLeftBack (int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 0) ;
motorSet (motor2,0) ;
motorSet (motor10, 90);
motorSet (motor9, 90) ;
imeGet(0, &counts); // keep getting the value
}
delay (200);
}
void turnLeftArc(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 127);
motorSet(MOTOR2, 127);
motorSet(MOTOR10, 0);
motorSet(MOTOR9, 0);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
delay(200);
}
void turnRightArc(int x) {
int counts = 1;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR1, 0);
motorSet(MOTOR2, 0);
motorSet(MOTOR10, 127);
motorSet(MOTOR9, 127);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR1);
motorStop(MOTOR2);
motorStop(MOTOR10);
motorStop(MOTOR9);
}
void scrapeLeftBack (int x)
{
int counts = 0;
imeReset(0); // rest left ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, -90) ;
motorSet (motor2,-90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 10) ; // no intertia
motorSet (motor2, 10) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (55);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay (200);
/*motorSet (motor1, 90) ;
motorSet (motor2, 90) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
delay(x);
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0);
motorSet (motor9, 0) ;
*/
}
void turnLeftArc(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 127) ;
motorSet (motor2, 127) ;
motorSet (motor10, 0) ;
motorSet (motor9, 0) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void driveForward(int x)
{
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 100) ;
motorSet (motor2, 100) ;
motorSet (motor10, 100) ;
motorSet (motor9, 100) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
delay (200);
}
void turnRightArc(int x)
{
int counts = 1;
imeReset(1); // rest rightLine ime
imeGet(1, &counts);
while (abs(counts) < x)
{
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 127) ;
motorSet (motor9, 127) ;
imeGet(0, &counts); // keep getting the value
}
motorStop (motor1);
motorStop (motor2);
motorStop (motor10);
motorStop (motor9);
}
void startIOTask(void *ignore) {
while(1) {
setDriveTrainMotors(driveTrainStyle);
motorSet(ARMLeft.port, ARMLeft.out * ARMLeft.reflected);
motorSet(ARMRight.port, ARMRight.out * ARMRight.reflected);
motorSet(ARMTopLeft.port, ARMTopLeft.out * ARMTopLeft.reflected);
motorSet(ARMTopRight.port, ARMTopRight.out * ARMTopRight.reflected);
motorSet(ARMBottomLeft.port, ARMBottomLeft.out * ARMBottomLeft.reflected);
motorSet(ARMBottomRight.port, ARMBottomRight.out * ARMBottomRight.reflected);
if(useIMEs) {
imeGet(IMELEFT, &encVals[0]);
imeGet(IMERIGHT, &encVals[1]);
} else {
encVals[0] = encoderGet(encLeft);
encVals[1] = encoderGet(encRight);
}
gyroVal = gyroGet(gyro);
delay(10);
}
}
void LiftPIDTask(void *ignore) {
int counts;
while (1) {
imeGet(IME_LIFT, &counts); // block until lift resources are available
if (mutexTake(liftMutex, ULONG_MAX)) { // block until mutex is available.
pidSensorCurrentValue = counts; // getting current position
pidError = pidSensorCurrentValue - liftPIDRequestedValue; // calculating error signal
if (abs(pidError) < PID_INTEGRAL_LIMIT) { // calculating integral factor, given it is within bounds
pidIntegral = pidIntegral + pidError;
} else {
pidIntegral = 0;
}
pidDerivative = pidError - pidLastError; // calculate derivative factor
pidLastError = pidError;
pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative); // sum all factors
if (pidDrive > PID_DRIVE_MAX) { // limit max output
pidDrive = PID_DRIVE_MAX;
}
if (pidDrive < PID_DRIVE_MIN) {
pidDrive = PID_DRIVE_MIN;
}
liftSet (pidDrive * PID_MOTOR_SCALE); // set lift motors
mutexGive(liftMutex); // give control back to LiftTask.
} else { // reset all
pidError = 0;
pidLastError = 0;
pidIntegral = 0;
pidDerivative = 0;
}
taskDelay(25);
}
}
short getSensorValue(Sensor s) {
short retVal = -1;
switch (s.type) {
case SHAFT_ENCODER:
if (DEBUG) {
print("got encoder value");
}
retVal = (short)encoderGet(s.enc);
break;
case GYROSCOPE:
retVal = (short)gyroGet(s.gyr);
break;
case ULTRASONIC:
retVal = (short)ultrasonicGet(s.ult);
break;
case ACCELEROMETER:
retVal = (short)analogReadCalibratedHR(s.port);
break;
case BUMPER_SWITCH:
case LIMIT_SWITCH:
retVal = (short)digitalRead(s.port);
break;
case LINE_TRACKER:
case POTENTIOMETER:
retVal = (short)analogRead(s.port);
break;
case TIME:
retVal = (short)getTimer(s.port, true);
break;
case IME: ; // so apparently due to history this ";" has to be here to declare
// a variable inside a case statement?
int imeVal = -1;
imeGet(s.port, &imeVal);
retVal = (short)imeVal;
break;
}
return retVal;
}
void classic15Blue()
{
int armMin = 290;
int wallHeight = 1000;
int goalHeight = 1700;
int dead1 = 1200;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead(8);
//encoder values
int encoder1 = 900; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 325; //back up
int encoder5 = 1350; //back up across the barrier again
int encoder6 = 80; // turn towards center large ball
int encoder7 = 600; // hit 1st ball off the barrier
int encoder8 = 350; // drive back
int encoder9 = 200; // turn towards other large ball
int encoder10 = 400; // hit 2nd ball off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 290; // turn to barf
int encoder13 = 800; // drive to barf + pickup
int encoder14 = 400;
int encoder15 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
// driveforward (some curve)////////////////////////////////////////////////////
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < encoder1)
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
///////////////////////////////////////////////////////////////////////////////
//driveForward(encoder1); // drive to goal
armUp(goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead(); //drive slow, adjust to 90 degrees
delay(1000);
outtake(1700); // outtake
driveBack(encoder4); // back up
delay(300);
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
delay(300);
turnLeft(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
delay(500);
driveBack(encoder8); // drive back
delay(300);
turnRight(encoder9); // turn towards other large ball
driveForward(encoder10); // hit 2nd ball off the barrier
delay(500);
driveBack(encoder11); // drive back to square
delay(600);
turnLeft(encoder12); // turn to barf
delay(300);
armDown(armMin);
intakeDead(); // pick up barf
driveForward(encoder13); // drive towards barf + intake it
delay(500);
//end of routine
stopAll();
delay(60000); ///////////////////////////////////////////////////////////////////////////////////
}
void operatorControl() {
lcdInit(uart1);
lcdClear(uart1);
lcdSetBacklight(uart1, true);
liftTaskCreate();
bool hookButtonPressed = false;
int imeLeft;
int imeRight;
while (true) {
imeGet(0, &imeLeft);
imeGet(1, &imeRight);
lcdPrint(uart1, 1, "IME Left %d", imeLeft);
lcdPrint(uart1, 2, "IME Right %d", imeRight);
// toggle hook
bool hookButtonPressedNow = joystickGetDigital(1, 7, JOY_UP);
if (hookButtonPressedNow && hookButtonPressedNow != hookButtonPressed) {
if (isHookDeployed()) {
retractHook();
} else {
deployHook();
}
}
hookButtonPressed = hookButtonPressedNow;
if (joystickGetDigital(1, 8, JOY_UP)) {
hang();
}
if (joystickGetDigital(1, 8, JOY_DOWN)) {
unhang();
}
// Drive
int leftY = ramp(joystickGetAnalog(1, 3));
int rightY = ramp(joystickGetAnalog(1, 2));
motorSet(driveFrontRight, -rightY);
motorSet(driveMiddleRight, -rightY);
motorSet(driveBackRight, -rightY);
motorSet(driveFrontLeft, leftY);
motorSet(driveMiddleLeft, leftY);
motorSet(driveBackLeft, leftY);
// Lift
int liftUp = joystickGetDigital(1, 6, JOY_UP);
int liftDown = joystickGetDigital(1, 6, JOY_DOWN);
if (liftUp && liftDown) {
//motorSet(liftLeft, 40);
//motorSet(liftRight, -40);
} else if (liftUp) {
liftManual(LIFT_SPEED);
//motorSet(liftLeft, 127);
//motorSet(liftRight, -127);
} else if (liftDown) {
liftManual(-LIFT_SPEED);
//motorSet(liftLeft, -127);
//motorSet(liftRight, 127);
} else {
//motorSet(liftLeft, 0);
//motorSet(liftRight, 0);
}
// Manipulator
int manipulatorIn = joystickGetDigital(1, 5, JOY_DOWN);
int manipulatorOut = joystickGetDigital(1, 5, JOY_UP);
if (manipulatorIn) {
motorSet(intakeLeft, -127);
motorSet(intakeRight, 127);
} else if (manipulatorOut) {
motorSet(intakeLeft, 60);
motorSet(intakeRight, -60);
} else {
motorSet(intakeLeft, 0);
motorSet(intakeRight, 0);
}
delay(50);
}
}
