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);
}
}
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);
}
}
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);
}
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);
}
}
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);
}
}
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);
}
}
/*-----------------------------------------------------------------------------*/
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 );
}
}
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();
}
}
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();
}
}
void stopFlywheel () {
stopTask(FWControlTask);
setFlywheel(0);
}
void stopFlywheel () {
stopTask(flywheelControl);
setFlywheel(0);
}