I have been trying to get the multithreading to work for AutoIntakeStarter, AutoIntakeStopper, and NoNegative
. I already got CounterUP
and CounterDOWN
to work but I can’t figure out what is wrong with the other ones.
My Code
↓
#include "vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
int autonType = -1;
int Discs = 0;
double intakeRotate = (200 * 0.01666666666 * 3);
int timerSet = 0;
void CounterUP(void) {
Discs = Discs + 1;
Controller1.Screen.clearLine(3);
Controller1.Screen.print(Discs);
vex::task::sleep(1000);
}
void CounterDOWN(void) {
Discs = Discs - 1;
Controller1.Screen.clearLine(3);
Controller1.Screen.print(Discs);
vex::task::sleep(1000);
}
void AutoIntakeStarter(void) {
timerSet = 3;
Discs = 0;
}
void NoNegative(void) {
Discs = 0;
}
void AutoIntakeStopper(void) {
this_thread::sleep_for(1000);
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
Brain.Screen.printAt(1, 100, "Out-taking");
Controller1.Screen.clearLine(3);
Controller1.Screen.print(Discs);
Intake.startSpinFor(reverse, intakeRotate, rev, 600, rpm);
this_thread::sleep_for(3000);
timerSet = timerSet - 3;
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
Intake.setRotation(0, rev);
}
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
bool autonSelecterStatus = false;
ColorSense1.setLight(ledState::on);
ColorSense1.gestureEnable();
Brain.Screen.printAt(1, 40, "Auton");
while (true) {
if (autonSelecter.value() > 0 && autonSelecterStatus == false) {
autonSelecterStatus = true;
} else if (autonSelecter.value() == 0 && autonSelecterStatus == true) {
Brain.Screen.clearScreen();
if (autonType == 0 || autonType == -1) {
autonType = 1;
Brain.Screen.printAt(1, 40, "1st Auton");
}else if (autonType == 1) {
autonType = 2;
Brain.Screen.printAt(1, 40, "2nd Auton");
}else if (autonType == 2) {
autonType = 3;
Brain.Screen.printAt(1, 40, "3rd Auton");
}else if (autonType == 3) {
autonType = 4;
Brain.Screen.printAt(1, 40, "4th Auton");
}else if (autonType == 4) {
autonType = 5;
Brain.Screen.printAt(1, 40, "5th Auton");
}else if (autonType == 5) {
autonType = 0;
Brain.Screen.printAt(1, 40, "Do Nothing");
}
Controller1.Screen.print(autonType);
autonSelecterStatus = false;
}
wait (20, msec);
}
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
/*---------------------------------------------------------------------------*/
/* Description */
/*---------------------------------------------------------------------------*/
if(autonType == 0) {
wait(10, msec);
}
if (autonType == 1) {
L1.spin(fwd, 10, percent);
R1.spin(fwd, 10, percent);
L2.spin(fwd, 10, percent);
R2.spin(fwd, 10, percent);
wait(5, sec);
L1.stop(brake);
L2.stop(brake);
R1.stop(brake);
R2.stop(brake);
}
/*---------------------------------------------------------------------------*/
/* Description */
/*---------------------------------------------------------------------------*/
if (autonType == 2) {
wait(10, msec);
}
/*---------------------------------------------------------------------------*/
/* Description */
/*---------------------------------------------------------------------------*/
if (autonType == 3) {
}
/*---------------------------------------------------------------------------*/
/* Description */
/*---------------------------------------------------------------------------*/
if (autonType == 4) {
}
/*---------------------------------------------------------------------------*/
/* Fake Auto if we need it */
/*---------------------------------------------------------------------------*/
if (autonType == 5) {
}
}
/*---------------------------------------------------------------------------*/
/* End of Auton's */
/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/
/* */
/* User Control Task */
/* */
/* This task is used to control your robot during the user control phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void usercontrol(void) {
bool brakeSystem = false;
bool shootSystem = false;
bool intakeSystem = false;
ColorSense1.setLight(ledState::on);
ColorSense1.setLightPower(100);
ColorSense1.gestureEnable();
FWSM1.setRotation(0, rev);
FWSM2.setRotation(0, rev);
Intake.setRotation(0, rev);
Brake.setRotation(0, rev);
while (1) {
//Driver Controls for Mechanum Wheels.
if(Controller1.Axis3.position() > -15 && Controller1.Axis3.position() < 15 && Controller1.Axis1.position() > -15 && Controller1.Axis1.position() < 15 && Controller1.Axis4.position() > -15 && Controller1.Axis4.position() < 15) {
Brain.Screen.clearLine(10);
Brain.Screen.clearLine(11);
L1.stop(brake);
L2.stop(brake);
R1.stop(brake);
R2.stop(brake);
Brain.Screen.printAt(1, 200, "Not Moving");
}else {
Brain.Screen.clearLine(10);
Brain.Screen.clearLine(11);
L1.spin(fwd, Controller1.Axis3.position() + Controller1.Axis1.position() + Controller1.Axis4.position(), percent);
R1.spin(fwd, Controller1.Axis3.position() - Controller1.Axis1.position() - Controller1.Axis4.position(), percent);
L2.spin(fwd, Controller1.Axis3.position() + Controller1.Axis1.position() - Controller1.Axis4.position(), percent);
R2.spin(fwd, Controller1.Axis3.position() - Controller1.Axis1.position() + Controller1.Axis4.position(), percent);
Brain.Screen.printAt(1, 200, "Moving");
}
//Shooting system
if(Controller1.ButtonR1.pressing() && shootSystem == false) {
shootSystem = true;
} else if (!Controller1.ButtonR1.pressing() && shootSystem == true) {
if(FWSM1.rotation(rev) < .5 && FWSM2.rotation(rev) < .5) {
FWSM1.spin(fwd, 100, pct);
FWSM2.spin(reverse, 100, pct);
Brain.Screen.printAt(1, 70, "Shooting");
}else {
FWSM1.stop(brake);
FWSM2.stop(brake);
FWSM1.setRotation(0, rev);
FWSM2.setRotation(0, rev);
Brain.Screen.clearLine(3);
Brain.Screen.clearLine(4);
}
shootSystem = false;
}
//Intake System
//Color Sensor stopping the Intake after 3 Discs
/* if (Discs == 3) {
timerSet = 3;
Discs = 0;
}
if(Discs < 0) {
Discs = 0;
}
if (timerSet != 0) {
while(timerSet != 0) {
this_thread::sleep_for(1000);
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
Brain.Screen.printAt(1, 100, "Out-taking");
Controller1.Screen.clearLine(3);
Controller1.Screen.print(Discs);
Intake.startSpinFor(reverse, intakeRotate, rev, 600, rpm);
this_thread::sleep_for(3000);
timerSet = timerSet - 3;
Intake.setRotation(0, rev);
}
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
Intake.setRotation(0, rev);
}
*/
//Manual Start-Stop of the Intake System
if (Controller1.ButtonR2.pressing() && intakeSystem == false) {
intakeSystem = true;
} else if (!Controller1.ButtonR2.pressing() && intakeSystem == true) {
if (Intake.rotation(rev) < .5) {
Intake.spin(fwd, 100, pct);
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
Brain.Screen.printAt(1, 100, "Intaking");
} else {
Intake.stop(brake);
Intake.setRotation(0, rev);
Brain.Screen.clearLine(5);
Brain.Screen.clearLine(6);
}
intakeSystem = false;
}
//Brake System
if(Controller1.ButtonL2.pressing() == true && brakeSystem == false) {
brakeSystem = true;
}
else if (!Controller1.ButtonL2.pressing() && brakeSystem == true) {
if(Brake.rotation(rev) < .3 && Brake.rotation(rev) > -100) {
Brain.Screen.clearLine(7);
Brain.Screen.printAt(1, 140, "Brake Down");
Brake.startSpinFor(fwd, 1.2, rev, 200, rpm);
}else {
Brain.Screen.clearLine(7);
Brain.Screen.printAt(1, 140, "Brake Up");
Brake.startSpinFor(reverse, 1.2, rev, 200, rpm);
Brake.setRotation(0, rev);
}
brakeSystem = false;
Brake.setRotation(0, rev);
}
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods and for when the color sensor counts the incoming discs.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
ColorSense1.gestureUp(CounterUP);
ColorSense1.gestureDown(CounterDOWN);
if (Discs == 3) {
AutoIntakeStarter();
} if(Discs < 0) {
NoNegative();
} if (timerSet != 0) {
AutoIntakeStopper();
}
// Run the pre-autonomous function.
pre_auton();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}