//run the intake chain for a distance, and keep the roller running while doing it
void intakeDistance (int encoderCounts, int direction, float power, int time) {
nMotorEncoder[intakeChain] = 0;
time1[T2] = 0;
motor[intakeRoller] = 127*direction;
while (abs(nMotorEncoder[intakeChain]) < encoderCounts && time1[T2] < time) {
motor[intakeChain] = power*direction;
}
setIntakeMotors(0);
}
/*
* Task that manages intake controls. Decides what button 6U does depending on state of flywheel and balls in intake.
*/
task intakeController() {
while (1) {
if(userIntakeControl)
{
if(flywheelMode < 1 && SensorValue[lowerIntakeBallLF] < 1850 && SensorValue[fwBallLF] < 1800)
{
setIntakeMotors(-vexRT[Btn5U]*127);
}
else if (flywheelMode < 1 && SensorValue[fwBallLF] < 1800)
{
setIntakeRoller(vexRT[Btn6D]*127-vexRT[Btn5U]*127);
setIntakeChain(-vexRT[Btn5U]*127);
}else if (flywheelMode == 1 && vexRT[Btn6D]) { //macro for close shooting
overrideAutoIntake = true; //prevent autoIntake from enabling userIntakeControl
userIntakeControl = false; //prevent user from controlling intake while macro is running
intakeDistance(2000, 1, 127, 2100);
userIntakeControl = true; //return intake control to user
flywheelMode = 0.5; //turn off the flywheel. The stopFlywheel task will recognize this value and stop the flywheel
}/* else if (flywheelMode == 2 && vexRT[Btn6D]) { //macro for center shooting
//overrideAutoIntake = true; //prevent autoIntake from enabling userIntakeControl
//userIntakeControl = false; //prevent user from controlling intake while macro is running
//intakeDistance(2000, 1, 100, 2100);
//userIntakeControl = true; //return intake control to user
//flywheelMode = 0.5; //turn off the flywheel. The stopFlywheel task will recognize this value and stop the flywheel
} */ else if (flywheelMode == 4 || flywheelMode == 2) { //Button 6U also controls intake chain for center, purple and long shooting
setIntakeMotors(vexRT[Btn6D]*127-vexRT[Btn5U]*127);
} else if (flywheelMode < 1&& rollerState == 1 && SensorValue[lowerIntakeBallLF] < 1500 && SensorValue[fwBallLF] > 1800) {
userIntakeControl = false;
intakeChainDistance(340,1,127,900); //move the second stage up
userIntakeControl = true;
} else {
setIntakeRoller(vexRT[Btn6D]*127-vexRT[Btn5U]*127);
setIntakeChain(-vexRT[Btn5U]*127);
}
wait1Msec(25);
}
}
}
//autonomous plays are in Position PID.c; use View > User Include Files to access
task usercontrol()
{
bool testMode = false;
if (testMode) {
//startTask(autonomous);
//stopTask(usercontrol);
flywheelMode = 3;
initializePIDMid();
FwVelocitySet(lFly,112.5,.7);
FwVelocitySet(rFly,112.5,.7);
wait1Msec(1500);
userIntakeControl = false;
setIntakeMotors(100);
}
//initalize tasks to control various subsystems that need to run concurrently during driver control
//tasks in use normally. Comment out to test shooting
//Use tasks intakeController, drivetrainController, flashLED, stopFlywheel
startTask(intakeController);
startTask(drivetrainController);
startTask(flashLED);
startTask(stopFlywheel);
while (true)
{
//controls lift actuation
//important: make sure to set a flywheelMode when testing the flywheel to ensure that this doesn't interfere
if (vexRT[Btn8L] && vexRT[Btn8U] && flywheelMode == 0) { //release hammer; to prevent damage to flywheel motors, make sure flywheel not going forwards before going backwards
setLeftFwSpeed(-60);
setRightFwSpeed(-60);
} else if (flywheelMode == 0) { //if the flywheel is not running, then give turn off the flywheel motors; otherwise, give precedence to PIC
setLeftFwSpeed(0);
setRightFwSpeed(0);
}
//flywheel speed control
//7U - long, 7L - purple, 8U - shoot from field edge, 7R - center 7D - short
//8R - stop
if (vexRT[Btn7U] == 1 && flywheelMode != 4) { //second condition prevents reinitialization of long shooting if the flywheel is currently in long shooting mode
//mode 0.5 is for when the flywheel has been shutdown but is still spinning. Since the control tasks are used for this process, the flywheel tasks need to be restarted.
if (flywheelMode >= 0.5) { //if the flywheel is currently running (modes 0.5,1-4), we need to stop the controller tasks before re-initializing the PID controller
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
}
ballsInIntake = 0; //reset the intake ball counter for simplicity
//next 4 lines have to run every time to run flywheel
flywheelMode = 4; //make sure we set the flywheel mode
initializePIDLong(); //prepare controller for long shooting
//set long shooting velocities
FwVelocitySet(lFly,139.75,.7);
FwVelocitySet(rFly,139.75,.7);
//yellowLEDFlashTime = 320; //flash the yellow LED for pacing
} else if (vexRT[Btn7D] == 1 && flywheelMode != 3.5) { //field edge shooting
//mode 0.5 is for when the flywheel has been shutdown but is still spinning. Since the control tasks are used for this process, the flywheel tasks need to be restarted.
if (flywheelMode >= 0.5) { //if the flywheel is currently running (modes 0.5,1-4), we need to stop the controller tasks before re-initializing the PID controller
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
userIntakeControl = true;
}
ballsInIntake = 0; //reset the intake ball counter for simplicity
//next 4 lines have to run every time to run flywheel
flywheelMode = 3.5;
//Uncomment these lines when this shooting mode has been tested
//initializePIDFieldEdge();
//FwVelocitySet(lFly,118.5,.7);
//FwVelocitySet(rFly,118.5,.7);
} else if (vexRT[Btn7R] == 1 && flywheelMode != 3) { //purple shooting
//mode 0.5 is for when the flywheel has been shutdown but is still spinning. Since the control tasks are used for this process, the flywheel tasks need to be restarted.
if (flywheelMode >= 0.5) { //if the flywheel is currently running (modes 0.5,1-4), we need to stop the controller tasks before re-initializing the PID controller
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
userIntakeControl = true;
}
moveIntakeBack();
ballsInIntake = 0; //reset the intake ball counter for simplicity
//next 4 lines have to run every time to run flywheel
flywheelMode = 3;
initializePIDMid();
FwVelocitySet(lFly,117.8,.7);
FwVelocitySet(rFly,117.8,.7);
} else if (vexRT[Btn7L] == 1 && flywheelMode != 2) { //center shooting
//mode 0.5 is for when the flywheel has been shutdown but is still spinning. Since the control tasks are used for this process, the flywheel tasks need to be restarted.
if (flywheelMode >= 0.5) { //if the flywheel is currently running (modes 0.5,1-4), we need to stop the controller tasks before re-initializing the PID controller
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
userIntakeControl = true;
}
moveIntakeBack();
ballsInIntake = 0; //reset the intake ball counter for simplicity
//next 4 lines have to run every time to run flywheel
//Uncomment these lines once midfield shooting has been tested
flywheelMode = 2;
initializePIDMid();
FwVelocitySet(lFly,112.5,.7);
FwVelocitySet(rFly,112.5,.7);
} else if ((vexRT[Btn5D] == 1 || indirectCloseShootStart) && flywheelMode != 1) { //close shooting
//mode 0.5 is for when the flywheel has been shutdown but is still spinning. Since the control tasks are used for this process, the flywheel tasks need to be restarted.
if (flywheelMode >= 0.5) { //if the flywheel is currently running (modes 0.5,1-4), we need to stop the controller tasks before re-initializing the PID controller
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
userIntakeControl = true;
}
indirectCloseShootStart = false; //setting this to true will do the same thing as pressing Btn7D on the joystick. Once the variable has activated
moveIntakeBack(); //move the intake back a little before startin the flywheel
ballsInIntake = 0; //reset the intake ball counter for simplicity
//next 4 lines have to run every time to run flywheel
flywheelMode = 1;
initializePIDShort();
FwVelocitySet(lFly, 83.5,.5);
FwVelocitySet(rFly, 83.5,.5);
} else if (vexRT[Btn8R] == 1 && flywheelMode >= 1) { //this is an else statement so that if two buttons are pressed, we won't switch back and forth between starting and stopping the flywheel
// flywheelMode needs to be >=1 and not >=0.5 because we don't want to stop the flywheel again if it is currently in the process of the stopping,
// although since the value of flywheelMode would not change in that case, it would appear as if nothing happened
userIntakeControl = true; //make sure the driver can control the intake again
overrideAutoIntake = false; //allow the autoIntake task to have control over the userIntakeControl variable again
//below line triggers flywheel shutdown procedure
flywheelMode = 0.5;
}
writeDebugStreamLine("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",nPgmTime,lFly.current, lFly.motor_drive, lFly.rpm_average, lFly.p, lFly.i, lFly.d, lFly.constant, 50*lFly.postBallLaunch, rFly.current, rFly.motor_drive, rFly.p, rFly.i, rFly.d, rFly.constant, 60*rFly.postBallLaunch);
wait1Msec(25); //don't overload the CPU
}
}
void intakeStop()
{ setIntakeMotors(0); } // stop the intake motors
void intakeOut()
{ setIntakeMotors(OUTTAKE_SPEED); } // speed out can be reduced for ease of scoring
void intakeIn()
{ setIntakeMotors(INTAKE_SPEED); } // this is full speed in
