//Auton
task autonomous()
{
if(SensorValue[autonSelector] > 1000){
bLCDBacklight = true;
//First shots
startTask(speedControl);
target = 8;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 22000){
if(speed >= speeds[target]-2){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
//Drive across
autoDrive(-210, 210, 70, false);
wait1Msec(300);
autoDrive(2100, 2100, 70, true);
wait1Msec(300);
autoDrive(250, -250, 70, false);
wait1Msec(300);
autoDrive(100, 100, 70, false);
wait1Msec(100);
//Second shots
startTask(speedControl);
target = 8;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 22000){
if(speed >= speeds[target]-2){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
//Display flywheel controls
clearLCDLine(0);
displayLCDString(0, 0, "EL:");
sprintf(mainBattery, "%d RL:%d", 0, SensorValue[leftEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display flywheel controls
clearLCDLine(1);
displayLCDString(1, 0, "ER:");
sprintf(mainBattery2, "%d RR:%d", 0, SensorValue[rightEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery2);
//Wait
wait1Msec(2000);
}
else{
//First shots
startTask(speedControl);
target = 12;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 9000){
if(speed >= speeds[target]){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
}
}
void killClawCompensation() // lets us turn off all compensation
{
stopTask(clawCompensatePID);
setClawMotors(0);
}
/*
The consequenses of giving commands through the app are happening here.
Several statusses are used to determine the possibl commands that can be given.
*/
task main(){
while (true){
/*
Standby The robot stays still untill it is told to either follow the line or go over to manual mode.
FIRE: Status goes to active.
A: Status goes to manual.
*/
while (status == "standby"){
CheckString();
if( s == "FIRE"){
startTask(Follow);
status = "active";
}
if (s == "A"){
status = "manual";
}
}
/*
Active The robot is following the line untill it is told to go standby or if it sees an object.
UP: Command to make the robot go straight on the next crossing.
LEFT: Command to make the robot go left on the next crossing.
RIGHT:Command to make the robot go right on the next crossing.
FIRE: Command to make the robot wait for a command on the next crossing.
C: Status goes to standby.
*/
while (status == "active"){
CheckString();
if( s == "C") {
status = "standby";
speed = 0;
stopTask(Follow);
StopSound();
Straight(speed);
}
if (s == "RIGHT"){
command = "right";
}
if (s == "LEFT"){
command = "left";
}
if (s == "UP"){
command = "straight";
}
}
/*
Engage The robot has seen an object and is waiting for a command that decides if it will charge or evade said object.
FIRE: Robot evades the object. (Evasion handled in task Follow)
B: The robot Charges.
C: Status goes to standby.
*/
while (status == "engage"){
CheckString();
if(s == "FIRE"){
}
if(s == "B"){
stopTask(Follow);
StopSound();
Charge();
s = "";
status = "standby";
}
if (s == "C"){
CheckString();
status = "standby";
speed = 0;
stopTask(Follow);
stopTask(AvoidObject);
StopSound();
Straight(speed);
}
if (SensorValue[S1] == 1){
status = "standby";
speed = 0;
stopTask(Follow);
stopTask(AvoidObject);
StopSound();
Straight(speed);
}
}
/*
Manual The robot can be manually controlled untill it is told to go back to standby.
UP: The robot moves forward.
RIGHT:Turn right.
LEFT: Turn left.
DOWN: The robot moves backward.
FIRE: Brake.
B: The robot charges.
C: Status goes to standby.
*/
while(status == "manual"){
CheckString();
Straight(0);
while (s == "UP"){
startTask(Sound);
CheckString();
Straight(max_Speed);
}
while (s == "DOWN"){
startTask(Sound);
CheckString();
Straight((max_Speed/2)*-1);
}
while (s == "LEFT"){
CheckString();
startTask(Sound);
motor[motorB]= 0;
motor[motorC]= max_Speed/2;
}
while (s == "RIGHT"){
startTask(Sound);
CheckString();
motor[motorC]= 0;
motor[motorB]= max_Speed/2;
}
if (s == "C"){
CheckString();
status = "standby";
speed = 0;
stopTask(Follow);
StopSound();
Straight(speed);
}
while (s == "FIRE"){
StopSound();
CheckString();
motor[motorB]= 0;
motor[motorC]= 0;
}
if (s == "B"){
StopSound();
Charge();
}
}
wait1Msec(1);
}
}
void allTasksStop()
{
stopTask(1);
stopTask(2);
stopTask(3);
stopTask(4);
#if defined(VEX2)
stopTask(5);
stopTask(6);
stopTask(7);
stopTask(8);
stopTask(9);
stopTask(10);
stopTask(11);
stopTask(12);
stopTask(13);
stopTask(14);
stopTask(15);
stopTask(16);
stopTask(17);
stopTask(18);
stopTask(19);
#endif
}
void Dialog::badParameter( const QString &msg )
{
QMessageBox::warning( this, "Bad parameter", msg );
stopTask();
}
/** @details
* Cause the gyro to be reinitialized by stopping and then restarting the
* polling task
*/
void
GyroReinit()
{
stopTask( GyroTask );
startTask( GyroTask );
}
// Get a statement
numvar getstatement(void) {
numvar retval = 0;
char *fetchmark;
chkbreak();
//#define LINEMODE
#ifdef LINEMODE
if (sym == s_while) {
// at this point sym is pointing at s_while, before the conditional expression
// save fetchptr so we can restart parsing from here as the while iterates
char *fetchmark = fetchptr;
for (;;) {
fetchptr = fetchmark; // restore to mark
primec(); // set up for mr. getsym()
getsym(); // fetch the start of the conditional
if (!getnum()) {
//longjmp(env, X_EXIT); // get the conditional; exit on false
sym = s_eof; // we're finished here. move along.
return;
}
if (sym != s_colon) expectedchar(':');
getsym(); // eat :
getstatementlist();
}
}
else if (sym == s_if) {
getsym(); // fetch the start of the conditional
if (!getnum()) {
//longjmp(env, X_EXIT); // get the conditional; exit on false
sym = s_eof;
return;
}
if (sym != s_colon) expectedchar(':');
getsym(); // eat :
getstatementlist();
}
// The switch statement: call one of N macros based on a selector value
// switch <numval>: macroid1, macroid2,.., macroidN
// numval < 0: numval = 0
// numval > N: numval = N
else if (sym == s_switch) {
getsym(); // eat "switch"
numvar selector = getnum(); // evaluate the switch value
if (selector < 0) selector = 0;
if (sym != s_colon) expectedchar(':');
// we sit before the first macroid
// scan and discard the <selector>'s worth of macro ids
// that sit before the one we want
for (;;) {
getsym(); // get an id, sets symval to its eeprom addr as a side effect
if (sym != s_macro) expected (6); // TODO: define M_macro instead of 6
getsym(); // eat id, get separator; assume symval is untouched
if ((sym == s_semi) || (sym == s_eof)) break; // last case is default so we exit always
if (sym != s_comma) expectedchar(',');
if (!selector) break; // ok, this is the one we want to execute
selector--; // one down...
}
// call the macro whose addr is squirreled in symval all this time
// on return, the parser is ready to pick up where we left off
domacrocall(symval);
// scan past the rest of the unused switch options, if any
// TODO: syntax checking for non-chosen options could be made much tighter at the cost of some space
while ((sym != s_semi) && (sym != s_eof)) getsym(); // scan to end of statement without executing
}
#else
// new statement handling
if (sym == s_while) {
// at this point sym is pointing at s_while, before the conditional expression
// save fetchptr so we can restart parsing from here as the while iterates
fetchmark = fetchptr;
for (;;) {
fetchptr = fetchmark; // restore to mark
primec(); // set up for mr. getsym()
getsym(); // fetch the start of the conditional
if (getnum()) {
retval = getstatement();
if (sym == s_returning) break; // exit if we caught a return
}
else {
skipstatement();
break;
}
}
}
else if (sym == s_if) {
getsym(); // eat "if"
if (getnum()) {
retval = getstatement();
if (sym == s_else) {
getsym(); // eat "else"
skipstatement();
}
} else {
skipstatement();
if (sym == s_else) {
getsym(); // eat "else"
retval = getstatement();
}
}
}
else if (sym == s_lcurly) {
getsym(); // eat "{"
while ((sym != s_eof) && (sym != s_returning) && (sym != s_rcurly)) retval = getstatement();
if (sym == s_rcurly) getsym(); // eat "}"
}
else if (sym == s_return) {
getsym(); // eat "return"
if ((sym != s_eof) && (sym != s_semi)) retval = getnum();
sym = s_returning; // signal we're returning up the line
}
else if (sym == s_switch) retval = getswitchstatement();
else if (sym == s_function) cmd_function();
#endif
else if (sym == s_run) { // run macroname
getsym();
if (sym != s_macro) unexpected(M_id);
// address of macroid is in symval via parseid
// check for [,snoozeintervalms]
getsym(); // eat macroid to check for comma; symval untouched
if (sym == s_comma) {
vpush(symval);
getsym(); // eat the comma
getnum(); // get a number or else
startTask(kludge(vpop()), expval);
}
else startTask(kludge(symval), 0);
}
else if (sym == s_stop) {
getsym();
if (sym == s_mul) { // stop * stops all tasks
initTaskList();
getsym();
}
else if ((sym == s_semi) || (sym == s_eof)) {
if (background) stopTask(curtask); // stop with no args stops the current task IF we're in back
else initTaskList(); // in foreground, stop all
}
else stopTask(getnum());
}
else if (sym == s_boot) reboot();
#if !defined(TINY85)
else if (sym == s_rm) { // rm "sym" or rm *
getsym();
if (sym == s_macro) {
eraseentry(idbuf);
}
else if (sym == s_mul) nukeeeprom();
else if (sym != s_undef) expected(M_id);
getsym();
}
else if (sym == s_ps) { getsym(); showTaskList(); }
else if (sym == s_peep) { getsym(); cmd_peep(); }
else if (sym == s_ls) { getsym(); cmd_ls(); }
else if (sym == s_help) { getsym(); cmd_help(); }
else if (sym == s_print) { getsym(); cmd_print(); }
else if (sym == s_semi) { ; } // ;)
#endif
#ifdef HEX_UPLOAD
// a line beginning with a colon is treated as a hex record
// containing data to upload to eeprom
//
// TODO: verify checksum
//
else if (sym == s_colon) {
// fetchptr points at the byte count
byte byteCount = gethex(2); // 2 bytes byte count
int addr = gethex(4); // 4 bytes address
byte recordType = gethex(2); // 2 bytes record type; now fetchptr -> data
if (recordType == 1) reboot(); // reboot on EOF record (01)
if (recordType != 0) return; // we only handle the data record (00)
if (addr == 0) nukeeeprom(); // auto-clear eeprom on write to 0000
while (byteCount--) eewrite(addr++, gethex(2)); // update the eeprom
gethex(2); // discard the checksum
getsym(); // and re-prime the parser
}
#endif
else getexpression();
if (sym == s_semi) getsym(); // eat trailing ';'
return retval;
}
void fw_stopFlyControl(){
stopTask(flw_tsk_FeedForwardCntrl);
}
task main()
{
string sig = "";
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
while (true)
{
// Check to see if a message is available
nSizeOfMessage = cCmdMessageGetSize(INBOX);
writeDebugStream("%c\n", nRcvBuffer);
if (nSizeOfMessage > kMaxSizeOfMessage)
nSizeOfMessage = kMaxSizeOfMessage;
if (nSizeOfMessage > 0){
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
string s = "";
stringFromChars(s, (char *) nRcvBuffer);
displayCenteredBigTextLine(4, s);
sig = nRcvBuffer;
writeDebugStream(sig);
if(sig == "U"){
setMotor(motorA, 50);
setMotor(motorB, 50);
wait1Msec(10);
}
if(sig=="D"){
setMotor(motorA, -50);
setMotor(motorB, -50);
wait1Msec(1);
}
if(sig=="L"){
setMotorTarget(motorA,0, 0);
setMotorTarget(motorB,360, 50);
waitUntilMotorStop(motorB);
startTask(Rijden);
k=1;
}
if(sig=="R"){
setMotorTarget(motorA,360, 50);
setMotor(motorB, 0);
waitUntilMotorStop(motorA);
startTask(Rijden);
}
if(sig=="F"){
setMotorTarget(motorA, 180,30);
setMotorTarget(motorB,180, 30);
waitUntilMotorStop(motorA);
waitUntilMotorStop(motorB);
k=1;
startTask(Rijden);
}
if(sig=="A"){
setMultipleMotors(motorA, motorB, 0);
stopTask(Rijden);
}
if(sig=="C"){
startTask(Rijden);
}
wait1Msec(100);
}
}
}
task main()
{
// Gekregen code voor BlueTooth
string sig = "";
TFileIOResult nBTCmdRdErrorStatus;
int nSizeOfMessage;
ubyte nRcvBuffer[kMaxSizeOfMessage];
// Einde gekregen code voor BlueTooth
while(true) // Main loop
{
/*
* Gekregen code voor BlueTooth
* leest berichten die BlueTooth stuurt uit en stopt die in een string.
*/
nSizeOfMessage = cCmdMessageGetSize(INBOX);
if (nSizeOfMessage > kMaxSizeOfMessage)
nSizeOfMessage = kMaxSizeOfMessage;
if (nSizeOfMessage > 0){
nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
nRcvBuffer[nSizeOfMessage] = '\0';
string s = "";
stringFromChars(s, (char *) nRcvBuffer);
displayCenteredBigTextLine(4, s);
/*
* Einde gekregen code voor BlueTooth
*/
sig = nRcvBuffer;// zet bluetoothsignaal in variabele
//writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven
// naar voren rijden zonder lijndetectie
if(sig == "U"){
setMotor(motorA, 30);
setMotor(motorB, 30);
wait1Msec(10);
}
// naar achter rijden zonder lijndetectie
if(sig=="D"){
setMotor(motorA, -30);
setMotor(motorB, -30);
wait1Msec(10);
}
// maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten)
if(sig=="L"){
setMotorTarget(motorA, 0, 0);
setMotorTarget(motorB, 360, 30);
waitUntilMotorStop(motorB);
startTask(Rijden);
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
}
// maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten)
if(sig=="R"){
setMotorTarget(motorA, 360, 30);
setMotor(motorB, 0);
waitUntilMotorStop(motorA);
startTask(Rijden);
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
}
// rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten)
if(sig=="F"){
setMotorTarget(motorA, 180, 30);
setMotorTarget(motorB, 180, 30);
waitUntilMotorStop(motorA);
waitUntilMotorStop(motorB);
k=1; // zet anti-lijnhump op 1
startTask(Rijden);
}
// Stoppen
if(sig=="A"){
setMultipleMotors(motorA, motorB, 0);
stopTask(Rijden);
clearSounds();
}
// Zet lijndetectie aan
if(sig=="C"){
k=1; // zet anti-lijnhump op 1
l=1; // zet anti-lijnhump op 1
startTask(Rijden);
}
/*
* nog 1 knop ongebruikt (B).
*/
// Na elk commando, wacht 0,1s.
wait1Msec(100);
}
}
}
void stopFlywheel () {
stopTask(flywheelControl);
setFlywheel(0);
}
void stopFlywheel () {
stopTask(FWControlTask);
setFlywheel(0);
}
// Get a statement
numvar getstatement(void) {
numvar retval = 0;
#if !defined(TINY_BUILD) && !defined(UNIX_BUILD)
chkbreak();
#endif
if (sym == s_while) {
// at this point sym is pointing at s_while, before the conditional expression
// save fetchptr so we can restart parsing from here as the while iterates
parsepoint fetchmark;
markparsepoint(&fetchmark);
for (;;) {
returntoparsepoint(&fetchmark, 0);
getsym(); // fetch the start of the conditional
if (getnum()) {
retval = getstatement();
if (sym == s_returning) break; // exit if we caught a return
}
else {
skipstatement();
break;
}
}
}
else if (sym == s_if) {
getsym(); // eat "if"
if (getnum()) {
retval = getstatement();
if (sym == s_else) {
getsym(); // eat "else"
skipstatement();
}
} else {
skipstatement();
if (sym == s_else) {
getsym(); // eat "else"
retval = getstatement();
}
}
}
else if (sym == s_lcurly) {
getsym(); // eat "{"
while ((sym != s_eof) && (sym != s_returning) && (sym != s_rcurly)) retval = getstatement();
if (sym == s_rcurly) getsym(); // eat "}"
}
else if (sym == s_return) {
getsym(); // eat "return"
if ((sym != s_eof) && (sym != s_semi)) retval = getnum();
sym = s_returning; // signal we're returning up the line
}
#if !defined(TINY_BUILD)
else if (sym == s_switch) retval = getswitchstatement();
#endif
else if (sym == s_function) cmd_function();
else if (sym == s_run) { // run macroname
getsym();
if ((sym != s_script_eeprom) && (sym != s_script_progmem) &&
(sym != s_script_file)) unexpected(M_id);
// address of macroid is in symval via parseid
// check for [,snoozeintervalms]
getsym(); // eat macroid to check for comma; symval untouched
if (sym == s_comma) {
vpush(symval);
getsym(); // eat the comma
getnum(); // get a number or else
startTask(vpop(), expval);
}
else startTask(symval, 0);
}
else if (sym == s_stop) {
getsym();
#if !defined(TINY_BUILD)
if (sym == s_mul) { // stop * stops all tasks
initTaskList();
getsym();
}
else if ((sym == s_semi) || (sym == s_eof)) {
if (background) stopTask(curtask); // stop with no args stops the current task IF we're in back
else initTaskList(); // in foreground, stop all
}
else
#endif
stopTask(getnum());
}
else if (sym == s_rm) { // rm "sym" or rm *
getsym();
if (sym == s_script_eeprom) {
eraseentry(idbuf);
}
#if !defined(TINY_BUILD)
else if (sym == s_mul) nukeeeprom();
#endif
else if (sym != s_undef) expected(M_id);
getsym();
}
else if (sym == s_ls) { getsym(); cmd_ls(); }
#if !defined(TINY_BUILD)
else if (sym == s_boot) cmd_boot();
else if (sym == s_ps) { getsym(); showTaskList(); }
else if (sym == s_peep) { getsym(); cmd_peep(); }
else if (sym == s_help) { getsym(); cmd_help(); }
#endif
else if (sym == s_print) { getsym(); cmd_print(); }
else if (sym == s_semi) { ; } // ;)
#ifdef HEX_UPLOAD
// a line beginning with a colon is treated as a hex record
// containing data to upload to eeprom
//
// TODO: verify checksum
//
else if (sym == s_colon) {
// fetchptr points at the byte count
byte byteCount = gethex(2); // 2 bytes byte count
int addr = gethex(4); // 4 bytes address
byte recordType = gethex(2); // 2 bytes record type; now fetchptr -> data
if (recordType == 1) reboot(); // reboot on EOF record (01)
if (recordType != 0) return; // we only handle the data record (00)
if (addr == 0) nukeeeprom(); // auto-clear eeprom on write to 0000
while (byteCount--) eewrite(addr++, gethex(2)); // update the eeprom
gethex(2); // discard the checksum
getsym(); // and re-prime the parser
}
#endif
else {
getexpression();
retval = expval;
}
if (sym == s_semi) getsym(); // eat trailing ';'
return retval;
}
void emergencyStop() {
stopTask(flywheel);
stopTask(flywheelStabilization);
initializeTasks();
}
//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 autonomous()
{
// Turn The LCD Backlight On
bLCDBacklight = true;
// Naming Convention, Due to Multiple Counts Running
int finalCount = count;
//Make Sure Lights are off
SensorValue[light1] = 0;
SensorValue[light2] = 1;
SensorValue[light3] = 0;
// Set The Intake to Open
SensorValue[dumpCubes] = 0;
//Stop The Menu Task
stopTask(Menu);
//Consult The Menu Class to Run Autonomous Mode
switch (finalCount)
{
case 0:
{
autonomous0();
break;
}
case 1:
{
autonomous1();
break;
}
case 2:
{
autonomous2();
break;
}
case 3:
{
autonomous3();
break;
}
case 4:
{
autonomous4();
break;
}
case 5:
{
autonomous5();
break;
}
case 6:
{
autonomous6();
break;
}
case 7:
{
autonomous7();
break;
}
case 8:
{
autonomous8();
break;
}
case 9:
{
autonomous9();
break;
}
case 10:
{
autonomous10();
break;
}
case 11:
{
autonomous11();
break;
}
case 12:
{
autonomous12();
break;
}
case 13:
{
autonomous13();
break;
}
case 14:
{
autonomous14();
break;
}
case 15:
{
autonomous15();
break;
}
case 16:
{
testing();
break;
}
case 17:
{
programming();
break;
}
}
}
void stopFlywheel () {
stopTask(flywheelControl);
motor[flywheel4] = 0;
}
