Ejemplo n.º 1
0
task reverseFlywheel () {
	while(true) {
		if(vexRT(Btn7L) && flywheelVelocity > flywheelReverseStartThreshold) {
			stopFlywheel();
			clearLCDLine(1);
			while(flywheelVelocity>0) {
				setFlywheel(flywheelVelocity>flywheelSlowDownVelocity?0:-pow(abs((flywheelVelocity/1000)-flywheelSlowDownVelocity/1000),1.3));
				clearLCDLine(0);
				displayLCDNumber(0,0,flywheelVelocity);
				displayLCDNumber(0,10,motor[flywheel1]);
				delay(25);
			}
		}
		else {
			while(vexRT(Btn7L)) {
				stopFlywheel();
				setFlywheel(-127);
				delay(25);
			}
			if(flywheelVelocity < 0 && !debugFlywheelActive) {
				setFlywheel(0);
			}
		}
		delay(25);
	}
}
Ejemplo n.º 2
0
task usercontrol()
{
	word button5U2Last,
		button5D2Last;

	// User control code here, inside the loop

	while (true)
	{
		arcade4(vexRT[Ch1], vexRT[Ch3], flWheel, blWheel, frWheel, brWheel);
		joyDigiMtr2(roller, vexRT[Btn6U], 127, vexRT[btn6D], -127);
		setMotor(elevator, vexRT[Ch2Xmtr2]);
		if(vexRT[Btn5UXmtr2] && !button5U2Last) {
			flyTarget += 5;
		}
		else if(vexRT[Btn5DXmtr2] && !button5D2Last) {
			flyTarget -= 5;
		}
		if(vexRT[Btn5U]) {
			if(abs(flyTarget - velAvg) > flyThresh + 50 || abs(lVelAvg - rVelAvg) > lRFlyThresh) {
				setLeds(0);
			}
			else if(abs(flyTarget - velAvg) < flyThresh + 50 && abs(flyTarget - velAvg) > flyThresh) {
				setLeds(1);
			}
			else {
				setLeds(2);
			}
			if(abs(lVelAvg - rVelAvg) > lRFlyThresh) {
				if(lVelAvg > rVelAvg) {
					motor[blFly] = motor[tlFly] = 95;
					motor[brFly] = motor[trFly] = 127;
				}
				else {
					motor[brFly] = motor[trFly] = 95;
					motor[blFly] = motor[tlFly] = 127;
				}
			}
			else if(flyTarget - velAvg > flyThresh) {
				setFlywheel(127);
			}
			else if(velAvg - flyTarget > flyThresh) {
				setFlywheel(63);
			}
			else {
				setFlywheel(79);
			}
		}
		else if(vexRT[Btn5D] && velAvg <= 50) {
			setFlywheel(-31);
		}
		else {
			setFlywheel(0);
			setLeds(3);
		}
		button5U2Last = vexRT[Btn5UXmtr2];
		button5D2Last = vexRT[Btn5DXmtr2];
		wait1Msec(20);
	}
}
Ejemplo n.º 3
0
void startFlywheel (flywheelShot shot) {
	currentShot.velocity = shot.velocity;
	currentShot.highSpeed = shot.highSpeed;
	currentShot.lowSpeed = shot.lowSpeed;
	currentShot.ramp = shot.ramp;
	currentShot.wait = shot.wait;
	currentShot.kP = shot.kP;
	currentShot.velocityShot = shot.velocityShot;
	currentShot.velocityThreshold = shot.velocityThreshold;
	if(flywheelVelocity >= 0)
		startTask(flywheelControl, kHighPriority);
	else
		setFlywheel(0);
}
Ejemplo n.º 4
0
task jonflywheelControl () {
	float kP = 0.02;

	setFlywheel(20);

	flywheelRampUp (currentShot.lowSpeed);

	int flywheelSpeed;

	while(true) {
		flywheelSpeed = motor[flywheel4] + (currentShot.velocity-flywheelVelocity)*kP;

		clearLCDLine(0);
		displayLCDNumber(0,1,flywheelSpeed);
		displayLCDNumber(0,5,flywheelVelocity);
		displayLCDNumber(0,10,(currentShot.velocity-flywheelVelocity)*kP);

		flywheelSpeed = flywheelSpeed>100?100:flywheelSpeed;
		flywheelSpeed = flywheelSpeed<0?0:flywheelSpeed;

		setFlywheel(flywheelSpeed);
	}
}
Ejemplo n.º 5
0
task autonomous()
{
	startTask(rpmCalc);
	while(true) {
		if(abs(flyTarget - velAvg) > flyThresh + 50 || abs(lVelAvg - rVelAvg) > lRFlyThresh) {
			setLeds(0);
		}
		else if(abs(flyTarget - velAvg) < flyThresh + 50 && abs(flyTarget - velAvg) > flyThresh) {
			setLeds(1);
		}
		else {
				setLeds(2);
		}
		if(abs(lVelAvg - rVelAvg) > lRFlyThresh) {
			if(lVelAvg > rVelAvg) {
				motor[blFly] = motor[tlFly] = 95;
				motor[brFly] = motor[trFly] = 127;
			}
			else {
				motor[brFly] = motor[trFly] = 95;
				motor[blFly] = motor[tlFly] = 127;
			}
		}
		else if(flyTarget - velAvg > flyThresh) {
			setFlywheel(127);
		}
		else if(velAvg - flyTarget > flyThresh) {
			setFlywheel(63);
		}
		else {
			setFlywheel(79);
			motor[elevator] = 127;

		}
		wait1Msec(20);
	}
}
Ejemplo n.º 6
0
task flywheelControl() {

	setFlywheel(20);

	flywheelRampUp (currentShot.lowSpeed);

	int flywheelSpeedA, flywheelSpeedB;

	while(true) {

		flywheelSpeedA = currentShot.highSpeed + (currentShot.velocity-flywheelVelocity) * currentShot.kP;
		flywheelSpeedB = currentShot.lowSpeed + (currentShot.velocity-flywheelVelocity) * currentShot.kP;

		flywheelSpeedA = flywheelSpeedA>100?100:flywheelSpeedA;
		flywheelSpeedB = flywheelSpeedB>100?100:flywheelSpeedB;

		flywheelSpeedA = flywheelSpeedA<0?0:flywheelSpeedA;
		flywheelSpeedB = flywheelSpeedB<0?0:flywheelSpeedB;

		if(flywheelVelocity < currentShot.velocity+currentShot.ramp) {
			setFlywheel(flywheelSpeedA);
		} else {
			setFlywheel(flywheelSpeedB);
		}

		if(debugMode) {
			flywheelLCD();
			flywheelLED();
			clearLCDLine(1);
			displayLCDNumber(1,1,flywheelSpeedA);
			displayLCDNumber(1,5,flywheelSpeedB);
		}

		delay(flywheelControlUpdateFrequency);
	}
}
Ejemplo n.º 7
0
/*-----------------------------------------------------------------------------*/
task
FwControlTask()
{
    fw_controller *fw = &flywheel;

    // Set the gain
    fw->gain = 0.99;

    // We are using Speed geared motors
    // Set the encoder ticks per revolution
    fw->ticks_per_rev = MOTOR_TPR_393S;

    while(1)
        {
        // debug counter
        fw->counter++;

				// Calculate velocity
				FwCalculateSpeed( fw );

        // Set current speed for the tbh calculation code
        fw->current = fw->v_current;

        // Do the velocity TBH calculations
        FwControlUpdateVelocityTbh( fw ) ;

        // Scale drive into the range the motors need
        fw->motor_drive  = (fw->drive * FW_MAX_POWER) + 0.5;

        // Final Limit of motor values - don't really need this
        if( fw->motor_drive >  127 ) fw->motor_drive =  127;
        if( fw->motor_drive < -127 ) fw->motor_drive = -127;

        // and finally set the motor control value
        setFlywheel( fw->motor_drive );

        // Run at somewhere between 20 and 50mS
        wait1Msec( FW_LOOP_SPEED );
        }
}
Ejemplo n.º 8
0
task usercontrol() {

	init();

	while (true) {

		if(!debugDrivebaseActive)
			logDrive();

		if(vexRT(Btn8U)) {
			startFlywheel(pipeShot);
			while(vexRT(Btn8U)) { delay(10); }
		}

		else if(vexRT(Btn8R)) {
			startFlywheel(midShot);
			while(vexRT(Btn8R)) { delay(10); }
		}

		else if(vexRT(Btn8L)) {
			startFlywheel(longShot);
			while(vexRT(Btn8L)) { delay(10); }
		}

		else if(vexRT(Btn7D)) {
			//startFlywheel(holdShot);
			startFlywheel(200, 30, 60, 10, 0);
			while(vexRT(Btn7D)) { delay(10); }
		}

		// for debugging - consider removing
		else if(vexRT(Btn7U))
			setFlywheel(60);

		else if(vexRT(Btn8D))
			stopFlywheel();
	}
}
Ejemplo n.º 9
0
task usercontrol() {


	init();

	while (true) {

		if(!debugDrivebaseActive)
			logDrive();

		if(vexRT(Btn8U)){
			startFlywheel(310, 0.0 );

			intakeLongShot = false;
			while(vexRT(Btn8U)) { delay(10); }
		}
		else if(vexRT(Btn8R)) {
			startFlywheel( 350, 0.0 );

			intakeLongShot = false;
			while(vexRT(Btn8R)) { delay(10); }
		}

		else if(vexRT(Btn8L)) {
			startFlywheel( 420, 0.7 );
			intakeLongShot = true;

			while(vexRT(Btn8L)) { delay(10); }
		}

		// for debugging - consider removing
		else if(vexRT(Btn7U))
			setFlywheel(60);

		else if(vexRT(Btn8D))
			stopFlywheel();
	}
}
Ejemplo n.º 10
0
void stopFlywheel () {
	stopTask(FWControlTask);
	setFlywheel(0);
}
Ejemplo n.º 11
0
void stopFlywheel () {
	stopTask(flywheelControl);
	setFlywheel(0);
}