#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None) #pragma config(I2C_Usage, I2C1, i2cSensors) #pragma config(Sensor, in1, LiftPot, sensorPotentiometer) #pragma config(Sensor, in2, AutonSelectColor, sensorPotentiometer) #pragma config(Sensor, in3, AutonSelectObject, sensorPotentiometer) #pragma config(Sensor, in4, AutonSelect, sensorPotentiometer) #pragma config(Sensor, dgtl1, btn1, sensorDigitalIn) #pragma config(Sensor, dgtl2, btn2, sensorDigitalIn) #pragma config(Sensor, dgtl3, ClawOpen, sensorDigitalOut) #pragma config(Sensor, dgtl5, LiftReset, sensorDigitalIn) #pragma config(Sensor, I2C_1, RightDrive, sensorQuadEncoderOnI2CPort, , AutoAssign) #pragma config(Sensor, I2C_2, LeftDrive, sensorQuadEncoderOnI2CPort, , AutoAssign) #pragma config(Sensor, I2C_3, LeftLift, sensorQuadEncoderOnI2CPort, , AutoAssign) #pragma config(Sensor, I2C_4, RightLift, sensorQuadEncoderOnI2CPort, , AutoAssign) #pragma config(Motor, port1, bLeftDrive, tmotorVex393_HBridge, openLoop, driveLeft) #pragma config(Motor, port2, LiftOne, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_3) #pragma config(Motor, port3, LiftTwo, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_4) #pragma config(Motor, port4, fLeftDrive, tmotorVex393_MC29, openLoop, driveLeft, encoderPort, I2C_2) #pragma config(Motor, port5, fRightDrive, tmotorVex393_MC29, openLoop, reversed, driveRight, encoderPort, I2C_1) #pragma config(Motor, port6, Claw, tmotorVex393_MC29, openLoop) #pragma config(Motor, port7, LiftThree, tmotorVex393_MC29, openLoop) #pragma config(Motor, port8, LiftFour, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port9, Intake, tmotorVex393_MC29, openLoop, reversed) #pragma config(Motor, port10, bRightDrive, tmotorVex393_HBridge, openLoop, driveRight) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// #pragma platform(VEX) //Competition Control #pragma competitionControl(Competition) void LiftStraightUp(int Pot) // Lifting up with an autocorrecting feature { SensorValue[LeftLift] = 0; SensorValue[RightLift] = 0; while (SensorValue[LiftPot] < Pot) { if(abs(SensorValue[LeftLift]) < SensorValue[RightLift]) { motor[LiftOne] = 127; motor[LiftThree] = 127; motor[LiftTwo] = 110; motor[LiftFour] = 110; } else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift]) { motor[LiftOne] = 110; motor[LiftThree] = 110; motor[LiftTwo] = 127; motor[LiftFour] = 127; } else(abs(SensorValue[LeftLift]) == SensorValue[RightLift]); { motor[LiftOne] = 127; motor[LiftThree] = 127; motor[LiftTwo] = 127; motor[LiftFour] = 127; } } } void LiftStraightDown(int Pot) // Lifting down with an autocorrecting feature { SensorValue[LeftLift] = 0; SensorValue[RightLift] = 0; while (SensorValue[LiftPot] > Pot) { if(abs(SensorValue[LeftLift]) < SensorValue[RightLift]) { motor[LiftOne] = -127; motor[LiftThree] = -127; motor[LiftTwo] = -110; motor[LiftFour] = -110; } else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift]) { motor[LiftOne] = -110; motor[LiftThree] = -110; motor[LiftTwo] = -127; motor[LiftFour] = -127; } else(abs(SensorValue[LeftLift]) == SensorValue[RightLift]); { motor[LiftOne] = -127; motor[LiftThree] = -127; motor[LiftTwo] = -127; motor[LiftFour] = -127; } } } void StopLift() { if(SensorValue(LiftPot) >= 800) // Hold Lift Up { motor[LiftOne] = 10; motor[LiftTwo] = 10; motor[LiftThree] = 10; motor[LiftFour] = 10; } else // Hold Lift Down { motor[LiftOne] = -10; motor[LiftTwo] = -10; motor[LiftThree] = -10; motor[LiftFour] = -10; } } void StopDrive() { motor[fLeftDrive] = 0; motor[bLeftDrive] = 0; motor[fRightDrive] = 0; motor[bRightDrive] = 0; } void DriveStraightBack(int Drive) // Driving backwards with an autocorrecting feature { SensorValue[RightDrive] = 0; SensorValue[LeftDrive] = 0; while(abs(SensorValue[LeftDrive]) < Drive) { if(abs(SensorValue[LeftDrive]) < SensorValue[RightDrive]) { motor[fLeftDrive] = -127; motor[bLeftDrive] = -127; motor[fRightDrive] = -110; motor[bRightDrive] = -110; } else if(abs(SensorValue[LeftDrive]) > SensorValue[RightDrive]) { motor[fLeftDrive] = -110; motor[bLeftDrive] = -110; motor[fRightDrive] = -127; motor[bRightDrive] = -127; } else; { motor[fLeftDrive] = -127; motor[bLeftDrive] = -127; motor[fRightDrive] = -127; motor[bRightDrive] = -127; } } StopDrive(); } void DriveStraightForward (int Drive) // Driving forwards with an autocorrecting feature { SensorValue[RightDrive] = 0; SensorValue[LeftDrive] = 0; while(abs(SensorValue[LeftDrive]) < Drive) { if(abs(SensorValue[LeftDrive]) < SensorValue[RightDrive]) { motor[fLeftDrive] = 127; motor[bLeftDrive] = 127; motor[fRightDrive] = 110; motor[bRightDrive] = 110; } else if(abs(SensorValue[LeftDrive]) > SensorValue[RightDrive]) { motor[fLeftDrive] = 110; motor[bLeftDrive] = 110; motor[fRightDrive] = 127; motor[bRightDrive] = 127; } else; { motor[fLeftDrive] = 127; motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = 127; } } StopDrive(); } void TurnLeft(int Turn) //Turning Left { while (abs(SensorValue[LeftDrive]) < Turn) { motor[fLeftDrive] = -127; motor[bLeftDrive] = -127; motor[fRightDrive] = 127; motor[bRightDrive] = 127; } StopDrive(); } void TurnRight(int Turn) //Turning Right { while (abs(SensorValue[LeftDrive]) < Turn) { motor[fLeftDrive] = 127; motor[bLeftDrive] = 127; motor[fRightDrive] = -127; motor[bRightDrive] = -127; } StopDrive(); } void StrafeLeft(int Strafe) // Strafe Left { while(abs(SensorValue[LeftDrive]) < Strafe) { motor[fLeftDrive] = -127; motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = -127; } StopDrive(); } void StrafeRight(int Strafe) // Strafe Right { while(abs(SensorValue[LeftDrive]) < Strafe) { motor[fLeftDrive] = 127; motor[bLeftDrive] = -127; motor[fRightDrive] = -127; motor[bRightDrive] = 127; } StopDrive(); } #include "Vex_Competition_Includes.c" // Main competition background code...do not modify! void pre_auton() ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// { // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between bStopTasksBetweenModes = true; } task autonomous() //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// { SensorValue[LeftDrive] = 0; SensorValue[RightDrive] = 0; SensorValue[LeftLift] = 0; SensorValue[RightLift] = 0; if(SensorValue[AutonSelectObject] > 2500) // Skyrise Building { if(SensorValue[AutonSelectColor] < 2500) // Blue Skyrise Building { motor[fLeftDrive] = -127; motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = -127; wait1Msec(750); } else // Red Skyrise Building { motor[fLeftDrive] = 127; motor[bLeftDrive] = -127; motor[fRightDrive] = -127; motor[bRightDrive] = 127; wait1Msec(750); } motor[Intake] = 0; StopDrive(); motor[Claw] = -127; // Move Skyrise builder back wait1Msec(2000); motor[Claw] = 0; // Stop Skyrise builder SensorValue[ClawOpen] = 1; // Pick up first Skyrise motor[Claw] = 127; // Move Skyrise builder forward wait1Msec(2000); motor[Claw] = 0; // Stop Skyrise builder wait1Msec(300); SensorValue[ClawOpen] = 0; // Place first Skyrise motor[Claw] = -127; // Move Skyrise builder back wait1Msec(2000); motor[Claw] = 0; // Stop Skyrise builder SensorValue[ClawOpen] = 1; // Pick up second Skyrise motor[Claw] = 127; // Move Skyrise builder forward wait1Msec(1200); motor[Claw] = 0; // Stop Skyrise builder LiftStraightUp(800); // Lift up StopLift(); // Stop lift motor[Claw] = 127; // Move Skyrise builder forward wait1Msec(1200); motor[Claw] = 0; // Stop Skyrise builder SensorValue[ClawOpen] = 0; // Place second Skyrise motor[Claw] = -127; // Move Skyrise builder back wait1Msec(2000); motor[Claw] = 0; // Stop Skyrise builder LiftStraightDown(750); // Lift down StopLift(); // Stop lift motor[fLeftDrive] = -80; // Drive back from Skyrise motor[bLeftDrive] = -80; motor[fRightDrive] = -80; motor[bRightDrive] = -80; wait1Msec(200); StopDrive(); } else // Skyrise tile cube scoring { motor[Claw] = -127; // Move Skyrise builder back wait1Msec(1500); motor[Claw] = 0; motor[fLeftDrive] = 127; // Drive forward motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = 127; motor[Intake] = 127; wait1Msec(1500); if(SensorValue[AutonSelectColor] > 2500) // Blue Skyrise tile cube scorer { motor[fLeftDrive] = 127; // Strafe left motor[bLeftDrive] = -127; motor[fRightDrive] = -127; motor[bRightDrive] = 127; wait1Msec(1000); motor[fLeftDrive] = -127; // Turn right motor[bLeftDrive] = -127; motor[fRightDrive] = 127; motor[bRightDrive] = 127; wait1Msec(1150); } if(SensorValue[AutonSelectColor] < 2500) // Red Skyrise tile cube scorer { motor[fLeftDrive] = -127; // Strafe right motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = -127; wait1Msec(1000); motor[fLeftDrive] = 127; // Turn left motor[bLeftDrive] = 127; motor[fRightDrive] = -127; motor[bRightDrive] = -127; wait1Msec(1150); } StopDrive(); LiftStraightUp(1450); StopLift(); motor[fLeftDrive] = 127; // Drive forward motor[bLeftDrive] = 127; motor[fRightDrive] = 127; motor[bRightDrive] = 127; wait1Msec(600); StopDrive(); motor[Intake] = -127; // Score cube wait1Msec(3000); motor[Intake] = 0; } } task usercontrol() /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// { SensorValue[LeftLift] = 0; SensorValue[RightLift] = 0; while (true) { // Drive with Mecanum Wheels motor[bLeftDrive] = (vexRT[Ch3] - vexRT[Ch4]); // (y - x) motor[fLeftDrive] = (vexRT[Ch3] + vexRT[Ch4]); // (y + x) motor[bRightDrive] = (vexRT[Ch2] + vexRT[Ch1]); // (y + x) motor[fRightDrive] = (vexRT[Ch2] - vexRT[Ch1]); // (y - x) if(vexRT[Btn5U] == 1) // Lift Up with an autocorrecting feature { if(abs(SensorValue[LeftLift]) < SensorValue[RightLift]) // Slow right { motor[LiftOne] = 127; motor[LiftThree] = 127; motor[LiftTwo] = 110; motor[LiftFour] = 110; } else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift]) // Slow left { motor[LiftOne] = 110; motor[LiftThree] = 110; motor[LiftTwo] = 127; motor[LiftFour] = 127; } else // Normal up { motor[LiftOne] = 127; motor[LiftThree] = 127; motor[LiftTwo] = 127; motor[LiftFour] = 127; } } else if(vexRT[Btn5D] == 1) // Lift Down with an autocorrecting feature { if(abs(SensorValue[LeftLift]) < SensorValue[RightLift]) // Slow right { motor[LiftOne] = -127; motor[LiftThree] = -127; motor[LiftTwo] = -110; motor[LiftFour] = -110; } else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift]) // Slow left { motor[LiftOne] = -110; motor[LiftThree] = -110; motor[LiftTwo] = -127; motor[LiftFour] = -127; } else // Normal down { motor[LiftOne] = -127; motor[LiftThree] = -127; motor[LiftTwo] = -127; motor[LiftFour] = -127; } } else // Correct and maintain lift position { if(SensorValue[LeftLift] > SensorValue[RightLift]) // Lift right side up { motor[LiftOne] = 0; motor[LiftTwo] = 127; motor[LiftThree] = 0; motor[LiftFour] = 127; } else if(SensorValue[LeftLift] < SensorValue[RightLift]) // Drop right side down { motor[LiftOne] = 0; motor[LiftTwo] = -127; motor[LiftThree] = 0; motor[LiftFour] = -127; } else // Hold lift position { if(SensorValue[LiftPot] > 800) // Hold up { motor[LiftOne] = 10; motor[LiftTwo] = 10; motor[LiftThree] = 10; motor[LiftFour] = 10; } else // Hold down { motor[LiftOne] = -10; motor[LiftTwo] = -10; motor[LiftThree] = -10; motor[LiftFour] = -10; } } } if(SensorValue[LiftReset] == 1) // Reset lift encoder values { SensorValue[LeftLift] = 0; SensorValue[RightLift] = 0; } if(vexRT(Btn6U) == 1) // Intake In { motor(Intake) = 127; } else if(vexRT(Btn6D) == 1) // Intake Out { motor(Intake) = -127; } else // Hold Intake Position { motor(Intake) = 5; } if(vexRT(Btn8U) == 1) // Move claw forward { motor(Claw) = 127; } else if(vexRT(Btn8D) == 1) // Move claw backward { motor(Claw) = -127; } else { motor(Claw) = 0; } if(vexRT(Btn8L) == 1) // Grab Skyrise { SensorValue[ClawOpen] = 1; } else if(vexRT(Btn8R) == 1) // Let go of Skyrise { SensorValue[ClawOpen] = 0; } } }