task resetCatapult() {
setCatapultMotors(-127);
while(SensorValue[platformAttached]) { //continue to move catapult motors while catapult and platform are attached
wait1Msec(25);
}
while(!SensorValue[platformAttached] || platformReleased) { //...and until the catapult is reattached to the platform
wait1Msec(25);
}
setCatapultMotors(0);
SensorValue[platformSolenoid] = 0;
platformReleased = false;
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
bool *lcdBacklight = initLcdVals();
taskResume(catTask);
while (1) {
// Joystick control of drive
checkDrive();
// Tower and intake control (Y-cabled into ports 3 and 8)
setTowerAndIntake();
// Manual catapult control
setCatapultMotors();
// Perform battery checks and update LCD
showBatteryOnLcd(uart1 );
// Show potentiometer values on second LCD
showPotVals(uart2, 1);
// Toggle backlight using buttons
bool newBacklight = checkBacklight(uart1, lcdBacklight[0]);
bool newSecondBacklight = checkBacklight(uart2, lcdBacklight[1]);
lcdBacklight[0] = newBacklight;
lcdBacklight[1] = newSecondBacklight;
// Delay 20 ms to concede to other tasks
delay(20);
}
}
task main()
{
int LY = 0;
int LX = 0;
int RY = 0;
int RX = 0;
int threshold = 15;
bool stopCatapult = false;
//startTask(lineTrackTest);
//stopTask(main);
startTask(log);
while(1)
{
//for deadzones; when the joystick value for an axis is below the threshold, the motors controlled by that joystick will not move in that direction
LY = (abs(vexRT[Ch3]) > threshold) ? vexRT[Ch3] : 0;
LX = (abs(vexRT[Ch4]) > threshold) ? vexRT[Ch4] : 0;
RY = (abs(vexRT[Ch2]) > threshold) ? vexRT[Ch2] : 0;
RX = (abs(vexRT[Ch1]) > threshold) ? vexRT[Ch1] : 0;
motor[lDriveFront] = LY + LX;
motor[lDriveBack] = LY - LX;
motor[rDriveFront] = RY - RX;
motor[rDriveBack] = RY + RX;
if (!vexRT[Btn7D] && !testMode) {
if(vexRT[Btn5U] == 1)
{
setCatapultMotors(127);
}
else if((vexRT[Btn5D] == 1 && !SensorValue[platformAttached]) || (vexRT[Btn5D] && vexRT[Btn8D]))
{
setCatapultMotors(-127);
}
else
{
setCatapultMotors(0);
}
if (vexRT[Btn6U]) {
SensorValue[platformSolenoid] = 1;
}
if (vexRT[Btn6D]) {
SensorValue[platformSolenoid] = 0;
}
if (vexRT[Btn8U]) {
SensorValue[openGate] = 1;
SensorValue[closeGate] = 0;
}
if (vexRT[Btn8D]) {
SensorValue[openGate] = 0;
SensorValue[closeGate] = 1;
}
}
//graph armPot, catapultUp, platformSolenoid, platformAttached v. time
else { //7D is pressed
//startTask(resetCatapult); //set the catapult motors to -127 to release the catapult and then start moving it down again; this is in a separate task so that the catapult will stop when
// it is supposed to even if the catapultUp condition (below) is never met
//play warning tone and wait before starting
playImmediateTone(1100, 100); //duration in 10Msec
wait1Msec(1500);
SensorValue[platformSolenoid] = 0; //make sure platform will be attached
SensorValue[openGate] = 1;
SensorValue[closeGate] = 0;
wait1Msec(500);
setCatapultMotors(-127); //start catapult motors to fire catapult
bool catapultUpTimedOut = false;
int catapultTimeOutTime = 3000; //in milliseconds; wait 3 seconds at most for the catapult to go all the way up
time1[T1] = 0; //start timing how long we've been waiting for the catapult to go all the way up
while(!SensorValue[catapultUp] || SensorValue[catapultPot] < 3700) { //when the catapult is up and the limit switch indicating this is activated, move on
if (time1[T1] > catapultTimeOutTime) {
catapultUpTimedOut = true;
break; //exit this while loop
}
wait1Msec(25);
}
if (!catapultUpTimedOut) { //if catapult up didn't time out
SensorValue[platformSolenoid] = 1; //release platform
//setCatapultMotors(127);
//wait1Msec(125);
time1[T1] = 0;
int catapultResetTimeOutTime = 7000; //check this value
while (!SensorValue[platformAttached] || SensorValue[catapultPot] > 2300) { //when platform is attached and catapult is down, move one
if (time1[T1] > catapultResetTimeOutTime) {
break; //exit this while loop
}
wait1Msec(25);
}
}
//if either wait operation times out, the catapult will stop moving and the platform solenoid will extend out; the variable platform released is not used
setCatapultMotors(0); //stop moving catapult
SensorValue[platformSolenoid] = 0; //reattach platform
platformReleased = false;
}
}
}