//blue autonomous for skyrise function
void blueSkyriseautonomous()
{ //-1458.51243217 degrees will position the robot exactly 1.5 squares -- diagonal-- from the skyrise autoloader
	//the diameter of the wheen is 4pi inches. The squares are apporx 23.41 inches^2 in area.
	clearencoders();

	pidDrivecontrol(-500,-510,-490,1,1,2);//brings the drive back to -500 degrees

	pidLiftcontrol(-185,-195,-175,-1,-1,5);//should lift the lift to the autoloaders height----- must perfect the height value via trial and error calculations

	pidDrivecontrol(350,340,360,1,1,2);//drives forward to  grab the skyrise

	waitInMilliseconds(150);//waits .15 seconds before executing

	SensorValue[CLAW] = 1;//closes claw on skyrise

	pidLiftcontrol(-125,-135,-115,-1,-1,3);//should lift the skyrise out of the autoloader

	waitInMilliseconds(150);//waits .15 seconds before executing

	pidDrivecontrol(-270,-275,-265,1,1,2);//brings the drive back to -270 degrees

	pidDrivecontrol(155,150,160,-1,1,3);//turns right to position the skyrise to be scored

	pidDrivecontrol(70,65,75,1,1,3);// drives slightly forward to align itself with the skyrise

	pidLiftcontrol(180,170,190,-1,-1,4);//lowers lift to score skyrise

	SensorValue[CLAW] = 0;//releases claw

	pidDrivecontrol(-500,-510,-490,1,1,2);//brings the drive back to -500 degrees to get it out of the way

	/***********************************************************************************************************

	Deleted code that scores a second skyrise

	/**********************************************************************************************************

	pidDrivecontrol(-150,-160,-140,1,1,2);//brings the drive back to -720 degrees or moves the robot

	pidLiftcontrol(-60,-70,-50,-1,-1,2);//raises lift

	pidDrivecontrol(-130,-140,120,1,-1,3);//turns left

	pidDrivecontrol(-260,-265,-255,-1,-1,2);//brings the drive back to -360 degrees or moves the robot back one turn

	SensorValue[CLAW] = 1;//releases claw

	pidLiftcontrol(-125,-135,-115,-1,-1,3);//should lift the lift to the autoloaders height

	pidDrivecontrol(155,150,160,-1,1,3);//turns right

	pidDrivecontrol(70,65,75,1,1,3);

	pidLiftcontrol(180,170,190,-1,-1,4);//lowers lift to score skyrise

	SensorValue[CLAW] = 0;//releases claw
	*/
}
task main(){
	int Forever = 1;
	while (Forever == 1){
		writeDebugStreamLine("X position: %d", SensorValue[xAccel]);
		waitInMilliseconds(250);
		writeDebugStreamLine("Y position: %d", SensorValue[yAccel]);
		waitInMilliseconds(250);
		}

	while(Forever == 1) {
		writeDebugStreamLine("Angle: %d", SensorValue[Degree]);
		waitInMilliseconds(1000);
	}
}
Пример #3
0
task activateLib()
{
	for (int i = 0; i < 4; i++) {
		motorCtrl[i].active = false;
		motorCtrl[i].timer = (i == 0 ? T1 : i == 1 ? T2 : i == 2 ? T3 : T4);
		motorCtrl[i].ms = 0;
		motorCtrl[i].powerPct = 0;
		motorCtrl[i].encoderInUse = false;
		motorCtrl[i].brake = false;
	}

	for (int i = 0; i < PSUEDO_TIMER_SIZE; i++) {
		psuedoTimer[i].timerId = 0;
		psuedoTimer[i].duration = 0;
		psuedoTimer[i].expirationTime = 0;
		psuedoTimer[i].recurring = false;
	}

	startTask(checkPsuedoTimer);

	while (true) {
		checkMotors();
		waitInMilliseconds(5);
	}
}
Пример #4
0
void checkMotorPosition(byte i)
{
	if (!motorCtrl[i].encoderInUse) return;

	// Get difference between current position and encoder stopping position
	int posDiff = abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos);
	int lastPosDiff = abs(motorCtrl[i].lastKnownPos - motorCtrl[i].encoderStopPos);

	// Set new slower motor speed when getting close to final position
	if (posDiff < abs(motorCtrl[i].encoderStopPos - motorCtrl[i].encoderSlowPos)) {
		turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, motorCtrl[i].slowPct);
	}

	// Stop motor and timer when encoder position is reached or overshot (or turning the wrong way)
	if (posDiff < 50 && posDiff - 1 > lastPosDiff) {	// subtract a bit to allow for some sensor error
		//writeDebugStreamLine("posDiff: %d, lastPosDiff: %d", posDiff, lastPosDiff);
		// Slight motor reverse acts as brake
		if (motorCtrl[i].brake) {
			turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, (motorCtrl[i].powerPct < 0 ? 20 : -20));
			waitInMilliseconds(40);
		}

		// Experiment with fixing overshot by keeping motor reverse running a little longer
		if (abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos) > 40) {
			//writeDebugStreamLine("Overshot adj, reqt pos: %d, cur pos: %d",
			//	motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);
			waitInMilliseconds(30);
		}

		// Stop Motor
		stopMotor(motorCtrl[i].motorPort1);
		if (motorCtrl[i].motorPort1 != motorCtrl[i].motorPort2) stopMotor(motorCtrl[i].motorPort2);

		//writeDebugStreamLine("Encoder stop, reqt pos: %d, cur pos: %d",
		//	motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);

		// Clear array
		motorCtrl[i].ms = 0;
		motorCtrl[i].powerPct = 0;
		motorCtrl[i].slowPct = 0;
		motorCtrl[i].brake = false;
		motorCtrl[i].encoderInUse = false;
		motorCtrl[i].active = false;
	}

	motorCtrl[i].lastKnownPos = SensorValue[motorCtrl[i].encoderPort];
}
task main()
{

		untilButtonPress(vexRT[Btn8U]);


		setDriveLeftRight(90, 100);
	  waitInMilliseconds(3000);

    stopDriveMotors();
		waitInMilliseconds(100);

		setDriveLeftRight(100, -100);
		waitInMilliseconds(600);

		stopDriveMotors();
		waitInMilliseconds(100);

		setDriveLeftRight(90, 100);
		waitInMilliseconds(1000);

		stopDriveMotors();

	//while (true)
	{
		  //setDriveLeftRight(vexRT[Ch3], vexRT[Ch2]);
      //controlFlipper();
      //controlScissorLift();


    //motor[LeftDriveA]   = vexRT[Ch3];
    //motor[LeftDriveB]   = vexRT[Ch3];
    //motor[RightDriveA]  = vexRT[Ch2];
    //motor[RightDriveB]  = vexRT[Ch2];
	}
}
Пример #6
0
task checkPsuedoTimer()
{
	while (true) {
		for (int i = 0; i < PSUEDO_TIMER_SIZE; i++) {
			if (psuedoTimer[i].expirationTime > 0) {
				if (psuedoTimer[i].expirationTime <= nSysTime) {
					int timerId = psuedoTimer[i].timerId;
					if (psuedoTimer[i].recurring) {
						psuedoTimer[i].expirationTime = nSysTime + psuedoTimer[i].duration;
					} else {
						psuedoTimer[i].timerId = 0;
						psuedoTimer[i].expirationTime = 0;
						psuedoTimer[i].duration = 0;
					}
					timerCallback(timerId);
				}
			}
		}
		waitInMilliseconds(1);
	}
}