// Main user task
task main()
{
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask( FwControlTask );
// Main user control loop
while(1)
{
// Different speeds set by buttons
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( &flywheel, 144, 0.55 );
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( &flywheel, 120, 0.38 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( &flywheel, 50, 0.2 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( &flywheel, 00, 0 );
// Display useful things on the LCD
sprintf( str, "%4d %4d %5.2f", flywheel.target, flywheel.current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", flywheel.drive, flywheel.drive_at_zero );
displayLCDString(1, 0, str );
// Don't hog the cpu :)
wait1Msec(10);
}
}
task usercontrol()
{
float maxRPM = 230;
// Start the flywheel control task
startTask( FwControlTask );
while (true)
{
// Different speeds set by buttons
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( .9*maxRPM, 0.80 );//Test These Numbers
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( .8*maxRPM, 0.70 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( .7*maxRPM, 0.60 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( 00, 0 );
int driveX = -vexRT[Ch2];
int driveY = vexRT[Ch1] ;
int intakeForward = vexRT[Btn5U];
int intakeBackwards = vexRT[Btn5D];
driveArcade(driveY * 127 / 128, driveX * 127 / 128);
setIntake(intakeForward*127, intakeBackwards*127);
wait1Msec(10);
}
}
//int rPower=75, lPower=55;
task usercontrol()
{
while(1) {
// writeDebugStreamLine("%d,%d,%d,%d,%d,%d,%d",nPgmTime, lFly.current, lFly.motor_drive, lFly.raw_last_rpm, rFly.current, rFly.motor_drive, rFly.raw_last_rpm);
// wait1Msec(25);
//Driving Motor Control
motor[lDriveFront] = -vexRT[Ch2];
motor[rDriveFront] = -vexRT[Ch3];
motor[lDriveBack] = -vexRT[Ch2];
motor[rDriveBack] = -vexRT[Ch3];
//Arm Control
if(vexRT[Btn6D] == 1)
{
motor[intakeRoller] = -127;
}
else if(vexRT[Btn6U] == 1)
{
motor[intakeRoller] = 127;
}
else
{
motor[intakeRoller] = 0;
}
if(vexRT[Btn5U] == 1)
{
motor[intakeChain] = -127;
}
else if(vexRT[Btn5D] == 1)
{
motor[intakeChain] = 127;
}
else
{
motor[intakeChain] = 0;
}
if(vexRT[Btn7D] == 1)
{
initializeTBH();
FwVelocitySet(lFly, 115, normalizeMotorPower(78));
FwVelocitySet(rFly, 115, normalizeMotorPower(78));
}
else if(vexRT[Btn7L] == 1)
{
initializeTBH();
FwVelocitySet(lFly, 25, normalizeMotorPower(50));
FwVelocitySet(rFly, 25, normalizeMotorPower(50));
}
else if(vexRT[Btn8D] == 1)
{
stopFlywheel();
}
}
}
// Main user task
task main()
{
// Line 211
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask( FwControlTask );
// Main user control loop
//startTask( Collect );
while(1)
{
// Different speeds set by buttons
if( vexRT[ Btn8L ] == 1 )
FwVelocitySet( &flywheel, 2325, 0.7 ); //2500 norm
if( vexRT[ Btn8U ] == 1 )
FwVelocitySet( &flywheel, 120, 0.38 );
if( vexRT[ Btn8R ] == 1 )
FwVelocitySet( &flywheel, 50, 0.2 );
if( vexRT[ Btn8D ] == 1 )
FwVelocitySet( &flywheel, 00, 0 );
/*
if(vexRT[Btn6U] == 1){
motor[CollectionA] = 127;
motor[CollectionB] = 127;
}else if(vexRT[Btn6D] == 1){
motor[CollectionA] = -127;
motor[CollectionB] = -127;
}else{
motor[CollectionA] = 127;
motor[CollectionB] = 127;
}*/
// Display useful things on the LCD
sprintf( str, "%4d %4d %5.2f", flywheel.target, flywheel.current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", flywheel.drive, flywheel.drive_at_zero );
displayLCDString(1, 0, str );
// Don't hog the cpu :)
wait1Msec(10);
}
}
task autonomous()
{
initializeTBH();
FwVelocitySet(lFly, 74, 0.623);
FwVelocitySet(rFly, 74, 0.623);
wait1Msec(2810); //wait time 1st ball
motor[intakeTop] = 125;
wait1Msec(1220); //2nd ball wait
moveIntake(621, 125);
wait1Msec(1220); //3rd
moveIntake(921, 125);
wait1Msec(1320); //4th
moveIntake(921, 125);
}
task autonomous()
{
//driveRobot(1,100,350);
//setLFly(60);
//setRFly(60);
//wait10Msec(200)
//motor[intakeRoller] = 50;
//motor[intakeChain] = -50;
//driveRobot(-1,100,20);
//driveRobot(1,0,1000);
initializeTBH();
FwVelocitySet(lFly, 136, normalizeMotorPower(85));
FwVelocitySet(rFly, 136, normalizeMotorPower(85));
wait1Msec(2500);
//1st ball
motor[intakeChain] = 125;
motor[intakeRoller] = 125;
wait1Msec(750);
motor[intakeChain] = 0;
motor[intakeRoller] = 0;
wait1Msec(750);
//2nd ball
motor[intakeChain] = 125;
motor[intakeRoller] = 125;
wait1Msec(750);
motor[intakeChain] = 0;
motor[intakeRoller] = 0;
wait1Msec(750);
//3rd ball
motor[intakeChain] = 125;
motor[intakeRoller] = 125;
wait1Msec(750);
motor[intakeChain] = 0;
motor[intakeRoller] = 0;
wait1Msec(750);
//4th ball
motor[intakeChain] = 125;
motor[intakeRoller] = 125;
wait1Msec(750);
motor[intakeChain] = 0;
motor[intakeRoller] = 0;
wait1Msec(750);
}
task stopFlywheel() {
//in order to continuing monitoring RPM after the flywheel is stopped (to prevent balls from being shot when the flywheel is not spinning fast enough to shoot them out of the robot),
// the flywheel is stopped using a special instace of the PIC controller, which will monitor flywheel RPM until it reaches 5 and then shutdown the flywheel motors completely)
// This is structured such that starting the flywheel will immediately override anything this task does.
while(1) {
if(flywheelMode == 0.5) { //trigger this by changing the value of flywheelMode to 0.5 rather than using a function call
//stop the flywheel tasks so we can restart them with our new controllers
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
//create the new controllers. Both P constants are high so that the motor value ends up being 0 (KpBallShot is the same as KpNorm so that the P constant has a constant value regardless of whether the controller thinks a ball has been shot [or if a ball has actually been shot])
tbhInit(lFly, 392, 1, 1, 0, 0, 0, 20); //initialize PID for left side of the flywheel //left side might be able to have a higher P
tbhInit(rFly, 392, 1, 1, 0, 0, 0, 20); //initialize PID for right side of the flywheel //x.x481
//restart the flywheel tasks with these new controllers
startTask(leftFwControlTask);
startTask(rightFwControlTask);
FwVelocitySet(lFly, 0, 0);
FwVelocitySet(rFly, 0, 0);
//wait for the flywheels to have a velocity <= 5 RPM (for this only one side needs to meet this condition since the sides are mechanically linked)
while ((lFly.current > 5 || rFly.current > 5) && flywheelMode == 0.5) {
//wait to continue
wait1Msec(50);
}
//the above while loop can be exited for one of two reasons:
// 1. flywheel velocity on one side drops below 5 RPM
// 2. flywheel mode changes (i.e., the user selects a new flywheel mode [close, purple, or long]
//In case 1, we can stop the flywheel completely. In case 2, we need to stop the flywheel stop process and start the flywheel back up (starting the flywheel is
// handled in the usercontrol task).
if (flywheelMode == 0.5) { //only shutdown the flywheel if the user hasn't restarted the flywheel
//return to open-loop control so we can control the flywheel motor powers
stopTask(leftFwControlTask);
stopTask(rightFwControlTask);
//turn off the flywheel motors
setLeftFwSpeed(0);
setRightFwSpeed(0);
flywheelMode = 0; //make sure we know that the flywheel is fully stopped
}
}
wait1Msec(50); //don't overload the cpu
}
}
//int rPower=75, lPower=55;
task usercontrol()
{
initializeTBH();
//ignoring these for a bit because fl
//FwVelocitySet(lFly, 110, normalizeMotorPower(67));
//FwVelocitySet(rFly, 110, normalizeMotorPower(67));
FwVelocitySet(lFly, 135, normalizeMotorPower(110));
FwVelocitySet(rFly, 135, normalizeMotorPower(110));
//setRFly(rPower)
//setLFly(lPower);
wait1Msec(750);
motor[intakeChain] = 127;
motor[intakeRoller] = 127;
while(1) {
writeDebugStreamLine("%d,%d,%d,%d,%d,%d,%d",nPgmTime, lFly.current, lFly.motor_drive, lFly.raw_last_rpm, rFly.current, rFly.motor_drive, rFly.raw_last_rpm);
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
}
}
task usercontrol()
{
bool flywheelRunning = false;
int LY = 0;
int LX = 0;
int RY = 0;
int RX = 0;
int threshold = 15;
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[lFrontDrive] = LY + LX;
motor[lBackDrive] = LY - LX;
motor[rFrontDrive] = RY + RX;
motor[rBackDrive] = RY - RX;
if (vexRT[Btn6U] == 1) {
motor[intakeBack] = 125;
}
else if (vexRT[Btn6D] == 1) {
motor[intakeBack] = -125;
}
else
{
motor[intakeBack] = 0;
}
if (vexRT[Btn5U] == 1 && flywheelRunning) {
motor[intakeTop] = 125;
}
else if (vexRT[Btn5D] == 1) {
motor[intakeTop] = -125;
}
else
{
motor[intakeTop] = 0;
}
if (vexRT[Btn7L] == 1) { //matches
if (!flywheelRunning) {
initializeTBH();
}
FwVelocitySet(lFly, 74, 0.623);
FwVelocitySet(rFly, 74, 0.623);
flywheelRunning = true;
} else if(vexRT[Btn8R] == 1) { //skills
if (!flywheelRunning) {
initializeTBHSkills();
}
FwVelocitySet(lFly, 56, 0.472);
FwVelocitySet(rFly, 56, 0.472);
flywheelRunning = true;
}
if (vexRT[Btn8D] == 1 && vexRT[Btn7D] == 1 && flywheelRunning) {
stopFlywheel();
flywheelRunning = false;
}
}
}
void startFlywheel (int velocity, float predicted) {
startTask(FwControlTask);
FwVelocitySet( &flywheel, velocity, predicted );
}
//int rPower=75, lPower=55;
task usercontrol()
{
while(1) {
motor[lDriveFront] = vexRT[Ch3];
motor[rDriveFront] = vexRT[Ch2];
motor[lDriveBack] = vexRT[Ch3];
motor[rDriveBack] = vexRT[Ch2];
if(testing == 3) //Tests auto
{
driveRobot(1,100,3685);
initializeTBHfast();
FwVelocitySet(lFly, 155, normalizeMotorPower(85));
FwVelocitySet(rFly, 155, normalizeMotorPower(85));
wait10Msec(7000)
motor[intakeRoller] = 50;
motor[intakeChain] = -50;
driveRobot(-1,100,20);
driveRobot(1,0,100000);
wait1Msec(5000);
}
if(vexRT[Btn6D] == 1)
{
motor[intakeRoller] = -127;
}
else if(vexRT[Btn6U] == 1 || testing == 1|| testing == 2)
{
motor[intakeRoller] = 127;
}
else
{
motor[intakeRoller] = 0;
}
if(vexRT[Btn5D] == 1 || testing == 1 || testing == 2)
{
motor[intakeChain] = -127;
}
else if(vexRT[Btn5U] == 1)
{
motor[intakeChain] = 127;
}
else
{
motor[intakeChain] = 0;
}
if(vexRT[Btn7U] == 1) //far shooting
{
initializeTBH();
FwVelocitySet(lFly, 146, normalizeMotorPower(85));
FwVelocitySet(rFly, 146, normalizeMotorPower(85));
}
else if(vexRT[Btn7D] == 1) //close shooting
{
initializeTBHClose();
FwVelocitySet(lFly, 94, normalizeMotorPower(55));
FwVelocitySet(rFly, 94, normalizeMotorPower(55));
}
else if(vexRT[Btn7R] == 1) //skills (purple) shooting
{
initializeTBHSkills();
FwVelocitySet(lFly, 105, normalizeMotorPower(70));
FwVelocitySet(rFly, 105, normalizeMotorPower(70));
}
else if(vexRT[Btn8D] == 1)
{
stopFlywheel();
}
writeDebugStreamLine("%d,%d,%d,%d,%d,%d,%d",nPgmTime, lFly.current, lFly.motor_drive, lFly.raw_last_rpm, rFly.current, rFly.motor_drive, rFly.raw_last_rpm);
wait1Msec(25);
}
}
task autonomous()
{
initializeTBH();
FwVelocitySet(lFly, 74, 0.623);
FwVelocitySet(rFly, 74, 0.623);
}
