Пример #1
0
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;
}
Пример #2
0
/*
 * 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);
	}

}
Пример #3
0
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;


		}
	}

}