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()
{
// startTask(shooter);
startTask(drive);
startTask(intake);
speedUpFlywheel();
while(true){
if(vexRT(Btn8U)){
motor[LUflywheel] = 127;
motor[LDflywheel] = 127;
motor[RUflywheel] = 127;
motor[RDflywheel] = 127;
}
else if(vexRT(Btn8D)){
motor[LDflywheel] = 100;
motor[RUflywheel] = 100;
motor[RDflywheel] = 100;
}
else if(vexRT(Btn8L))
slowDownFlywheel();
wait1Msec(25);
}
}
task usercontrol()
{
startTask(shooter);
startTask(drive);
startTask(intake);
startTask(flywheelVelocity);
//speedUpFlywheel();
while(true){
if(vexRT(Btn8U)){
motor[LUflywheel] = 127;
motor[LDflywheel] = 127;
motor[RUflywheel] = 127;
motor[RDflywheel] = 127;
}
else if(vexRT(Btn8D)){
motor[LDflywheel] = 90;
motor[RUflywheel] = 90;
motor[RDflywheel] = 90;
}
if(vexRT(Btn8L)){
stopTask(flywheelP);
slowDownFlywheel();
}
wait1Msec(25);
}
}
task usercontrol() {
while (true) {
if(vexRT(Btn7U)){
motor(flywheel1) = -127;
motor(flywheel2) = -127;
motor(flywheel3) = -127;
motor(flywheel4) = -127;
}
else if(vexRT(Btn7D)){
motor(flywheel1) = 100;
motor(flywheel2) = 100;
motor(flywheel3) = 100;
motor(flywheel4) = 100;
}
else{
motor(flywheel1) = 0;
motor(flywheel2) = 0;
motor(flywheel3) = 0;
motor(flywheel4) = 0;
}
}
}
void chaDrive(){
int LY = vexRT(Ch3);
int LX = vexRT(Ch4);
int RX = vexRT(Ch1);
if(abs(LY+LX) <=deadZone){
LY = 0;
LX = 0;
}
if(abs(RX) <=17){
RX = 0;
}
if(abs(RX) >17){
LX = 0;
LY = 0;
}
mTR(LX - LY + RX);
mBR(-LY - LX + RX);
mBL(LY - LX + RX);
mTL(LX + LY + RX);
}
void armMotor(){
if(vexRT[Btn6U] == 1){
// If 6U is pressed, arm goes up
motor[topRightLift] = 127;
motor[topLeftLift] = 127;
motor[bottomLeftLift] = 127;
motor[bottomRightLift] = 127;
}
else if(vexRT[Btn6D] == 1){
// If 6D is pressed, arm goes down
motor[topRightLift] = -127;
motor[topLeftLift] = -127;
motor[bottomLeftLift] = -127;
motor[bottomRightLift] = -127;
}
else{
// otherwise, do nothing
motor[topRightLift] = 0;
motor[topLeftLift] = 0;
motor[bottomLeftLift] = 0;
motor[bottomRightLift] = 0;
}
if(vexRT(Btn8U) == 1){
//If 8U is pressed, bring arm to maximum height
userArm(rightMaxPot,leftMaxPot,RAISE ,127);
}
else if(vexRT(Btn8D) == 1){
//If 8D is pressed, bring arm to minimum height
userArm(rightMinPot,leftMinPot,LOWER ,127);
}
}
task usercontrol() {
startTask(drive);
while(true) {
if(vexRT(Btn8U)) {
shooterPowerDown();
autoFeeder = false;
}
if(vexRT(Btn8D) || autoStartShooter) {
autoFeeder = true;
if(fastMode) {
speed = 103;
feederWaitTime = 1000;
} else {
speed = 98;
feederWaitTime = 1500;
}
startTask(shooterDJ);
}
if(vexRT(Btn5D))
motor[feeder] = 127;
else if(!autoFeeder)
motor[feeder] = 0;
if(vexRT(Btn5U))
motor[intake1] = 127;
else
motor[intake1] = 0;
if(vexRT(Btn6D)) {
speed = 55;
startTask(shooter);
}
wait1Msec(25);
}
}
task usercontrol() {
while (true) {
driveForwardTurnStrafe(DRIVE_SLEW_RATE, vexRT(Ch3), vexRT(Ch4), vexRT(Ch1));
liftSpeeds(LIFT_SLEW_RATE, straight( buttonsToSpeed(Btn5U, Btn5D) ) );
intakeSpeed(INTAKE_SLEW_RATE, buttonsToSpeed(Btn6U, Btn6D) );
constantLoopTime(); //Important for slew
}
}
task drive(){
while(true){
motor[LFdrive] = vexRT(Ch3);
motor[LBMdrive] = vexRT(Ch3);
motor[RFdrive] = vexRT(Ch2);
motor[RBMdrive] = vexRT(Ch2);
wait1Msec(25);
}
}
//Slow down the controls if a button is pressed
void SlowButton()
{
if(vexRT(Btn8D) == true)
{
boolSlowButton = true;
}
if(vexRT(Btn8U) == true)
{
boolSlowButton = false;
}
}
//FUNCTION that controls switches between arcade drive and tank drive
void controlSwitch(){
//Declaring and initializing variables
int btn1 = vexRT(Btn7L);
int btn2 = vexRT(Btn7R);
//Says if btn1 is pressed it goes to tank control, otherwise it goes to arcade drive
if(btn1==1){
switched=true;
}else if(btn2==1){
switched=false;
}
}
void manualDrive()
{
if( abs( vexRT(Ch3) ) > 15 )
motor[DriveLF] = motor[DriveLB] = vexRT(Ch3);
else
motor[DriveLF] = motor[DriveLB] = 0;
if( abs( vexRT(Ch2) ) > 15 )
motor[DriveRB] = motor[DriveRF] = vexRT(Ch2);
else
motor[DriveRB] = motor[DriveRF] = 0;
}
task shooter(){
while(true){
if(vexRT(Btn6U)){
while(!vexRT(Btn6D))
speedUpFlywheel();
}
else if(vexRT(Btn6D)){
while(!vexRT(Btn6U))
slowDownFlywheel();
}
wait1Msec(25);
}
}
//This function controls the speed of the wheels, so the drivers will be able to change the speed of the robot based on te situation
void gearShift(){
//Declare and initialize variables
int buttonUp = vexRT(Btn8L);
int buttonDown = vexRT(Btn8D);
//Says if buttonUp is pressed, motor controls are full speed
if (buttonUp==1){
speedControl=1;
}
//Says if buttonDown is pressed, motor controls are half speed
if (buttonDown==1){
speedControl=0.5;
}
}
void checkMode(){
if(vexRT(Btn5U) == 1){
currentMode = SHOOTING_MODE;
} else if(vexRT(Btn5D) == 1){
currentMode = COLLECTING_MODE;
}
if(vexRT(Btn5UXmtr2) == 1){
currentMode2 = SHOOTING_MODE;
} else if(vexRT(Btn5DXmtr2) == 1){
currentMode2 = COLLECTING_MODE;
}
}
//Robot B Driver Control program
task main()
{
// Set the motors initially
int Presets[4] = {0, 55, 75, 127};
int currentPreset = 0;
motor[REF] = Presets[currentPreset];
motor[RF] = motor[REF];
motor[LF] = motor[RF];
while(true)
{
// Flywheel Speed
if(vexRT(Btn8U))
currentPreset = 0;
else if (vexRT(Btn8R))
currentPreset = 1;
else if (vexRT(Btn8D))
currentPreset = 2;
else if (vexRT(Btn8L))
currentPreset = 3;
motor[REF] = Presets[currentPreset];
motor[RF] = motor[REF];
motor[LF] = motor[RF];
// Fit the input to an exponential curve
// Squares are always positive, so a ternary operator is needed for negative numbers
// The abs is just in case we decide to go with odd exponents or even roots (like 3/2)
// We devide by 128 because the program can theoretically reach -128
int verticalL = pow(abs(vexRT(Ch3))/128.0,2.7)*127.0 *((vexRT(Ch3)>0)?(1):(-1));
int verticalR = pow(abs(vexRT(Ch2))/128.0,2.7)*127.0 *((vexRT(Ch2)>0)?(1):(-1));
// Move Robot
motor[LRW] = verticalL;
motor[LFW] = motor[LRW];
motor[RRW] = verticalR;
motor[RFW] = motor[RRW];
//Feeder control
if(vexRT(Btn6U))
motor[I1] = 127;
else if(vexRT(Btn5U))
motor[I1] = -127;
else
motor[I1] = 0;
motor[I2] = motor[I1];
}
}
task main()
{
int x = 30;
while(true) {
gearSpeed(x);
if(vexRT(Btn8U)==1){
x += 10;
}
if(vexRT(Btn8D)==1){
x += 10;
}
delay(10);
}
}
//this code is also called tank drive. One joystick controls 1 side of the robot base.
void StandardDrive()
{
if(boolSlowButton == true)
{
motor[RightMotor] = vexRT(Ch2)/3;
motor[LeftMotor] = vexRT(Ch3)/3;
}
else
{
motor[RightMotor] = vexRT(Ch2);
motor[LeftMotor] = vexRT(Ch3);
}
}
void drive()
{
if(vexRT(Ch3) >= drivingThreshhold)
{
left(vexRT(Ch3));
}
else if(vexRT(Ch3) <= -drivingThreshhold)
{
left(vexRT(Ch3));
}
else
{
left(0);
}
if(vexRT(Ch2) >= drivingThreshhold)
{
right(vexRT(Ch2));
}
else if(vexRT(Ch2) <= -drivingThreshhold)
{
right(vexRT(Ch2));
}
else
{
right(0);
}
if(destroyAllHumans == 1)
{
killeveryone();
}
}
void reverseFlywheel() {
if(vexRT(Btn7L)) {
if(getFlywheelVelocity()>10) {
stopTask(abi);
motor[flywheel4] = -5;
while(VexRT(Btn7L) && getFlywheelVelocity()>10) { delay(25); }
} else {
motor[flywheel4] = -127;
while(vexRT(Btn7L)) { delay(25); }
while(motor[flywheel4]<0) { motor[flywheel4]+=2; delay(25); }
motor[flywheel4] = 0;
}
}
}
task shooter () {
bool canRunAgain = true;
while (true)
{
if(vexRT(Btn7U))
speed++;
if(vexRT(Btn7D))
speed--;
FwMotorSet(speed);
//sprintf( str, "%4d %4d", target_velocity, motor_velocity, nImmediateBatteryLevel/1000.0 );
wait1Msec(200);
}
}
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
// Set the motors initially
int Presets[4] = {0, 55, 85, 127};
int currentPreset = 0;
motor[Inside1] = Presets[currentPreset];
motor[Inside2] = motor[Inside1];
motor[Outside1] = motor[Inside1];
motor[Outside2] = motor[Inside1];
while(true)
{
// Flywheel Speed
if(vexRT(Btn8U))
currentPreset = 0;
else if (vexRT(Btn8R))
currentPreset = 1;
else if (vexRT(Btn8D))
currentPreset = 2;
else if (vexRT(Btn8L))
currentPreset = 3;
motor[Inside1] = Presets[currentPreset];
motor[Inside2] = motor[Inside1];
motor[Outside1] = motor[Inside1];
motor[Outside2] = motor[Inside1];
//Fit the input to an exponential curve
//Squares are always positive, so a ternary operator is needed for negative numbers
//The abs is just in case we decide to go with odd exponents
int verticalL = abs(pow(vexRT(Ch3)/127.0,2.0))*127.0 *((vexRT(Ch3)>0)?(1):(-1));
int verticalR = abs(pow(vexRT(Ch2)/127.0,2.0))*127.0 *((vexRT(Ch2)>0)?(1):(-1));
// Move Robot
motor[LRWheel] = verticalL;
motor[LFWheel] = motor[LRWheel];
motor[RRWheel] = verticalR;
motor[RFWheel] = motor[RRWheel];
if(vexRT(Btn6U))
SensorValue[Feeder] = 1;
else if (vexRT(Btn5U))
SensorValue[Feeder] = 0;
}
}
task intakeControl () {
while(true) {
string speed;
sprintf(speed, "%d", indexerSpeed);
line(1,speed);
while(true) {
motor[intake] = ((vexRT(Btn5U)||autonIndex)-vexRT(Btn5D))*127;
while (vexRT(Btn5U) || autonIntake) {
if(vexRT(Btn6D) && sensorValue[indexHigh]) {
motor[indexer] = -127;
delay(250);
} else if(vexRT(Btn6U) || autonShoot) {
//if(sensorValue[indexHigh] && getFlywheelVelocity()<currentGoalVelocity+30) {
if(SensorValue[indexHigh]) {
while(time1[T1]<=waitTime) {
motor[indexer] = -7;
delay(25);
}
if(getFlywheelVelocity()>0) {
motor[indexer] = 127;
while(SensorValue[indexHigh] && (vexRT(Btn6U)||autonShoot)) { delay(5); }
clearTimer(T1);
}
else {
motor[indexer] = -7;
}
}
else {
motor[indexer] = 127;
}
delay(50);
} else if(SensorValue[indexLow] || SensorValue[indexHigh]) {
motor[indexer] = -7;
} else {
motor[indexer] = (vexRT(Btn5U)-vexRT(Btn5D))*127;
}
delay(25);
}
if(vexRT(Btn6D) && SensorValue[indexHigh]) {
motor[indexer] = -127;
delay(250);
}
motor[indexer] = vexRT(Btn5D)?-127:0;
delay(25);
}
}
}
task intake(){
while(true){
if(vexRT(Btn5U)){
motor[intake1] = 127;
}
else if(vexRT(Btn5D)){
motor[feeder] = 90;
}
else{
motor[intake1] = 0;
motor[feeder] = 0;
}
wait1Msec(25);
}
}
task main()
{
while(true)
{
//Tank Drive
motor[RMotor]=vexRT(Ch2)*0.75;
motor[LMotor]=vexRT(Ch3)*0.75;
//Arm Control
if (vexRT[Btn6U]==1)
{
motor[Arm]=60;
}
else if (vexRT[Btn6D]==1)
{
motor[Arm]=-60;
}
else
{
motor[Arm]=15;
}
//Claw Control
if (vexRT[Btn5U]==1)
{
motor[Claw]=-60;
}
else
{
motor[Claw]=40;
}
//Thumb Control
if (vexRT[Btn5U]==1)
{
motor[Thumb]=60;
}
else if (vexRT[Btn5D]==1)
{
motor[Thumb]=-30;
}
else
{
motor[Thumb]=0;
}
}
}
/*This is the function that controls the motor for the corn door
if the first button is pressed the door opens
if the second button is pressed the door closes
otherwise the motor does not move
*/
void doorMotorControl(){
//Declare and initialize btn1 as a button on the vex controller, which may be changed based on driver requests.
int btn1 = vexRT(Btn6U);
int btn2 = vexRT(Btn6D);
if((btn1 == 1)&&(SensorValue(limit)==1)){
//This opens the corn door
motor[doorMotor] = 175;
}
else if(btn2 == 1){
//This part will turn the motor to close the door
motor[doorMotor] = -175;
}
else{
//This part says that if the buttons are not pushed, the motor does not turn
motor[doorMotor] = 0;
}
}
void cheesyDrive(){
int wheel;
int throttle;
bool quickTurn = false;
wheel = vexRT(Ch1); //set right axis on joystick equal to wheel
throttle = vexRT(Ch3);//set left axis on joystick equal to throttle
if(abs(wheel) > QUICKSENS) quickTurn = true; //if I'm beyond a certain point on my axis, enable quickturn
if(quickTurn) wheel *= QUICKVAL; //If i'm in quickturn, turn a lot faster than normal
else{
float newThrottle = throttle / 127; //If I'm not in quickturn turn a little bit slower for control
wheel = newThrottle * SPEEDVAL * wheel;
}
leftSide = throttle + wheel; //Enable arcade drive with the new values of wheel and throttle
rightSide = throttle - wheel;
leftSide = limit_motor(leftSide); //Limit both motors in case they're beyond 127
rightSide = limit_motor(rightSide);
}
task intakeControl () {
while(true) {
motor[intake] = ((vexRT(Btn5U)||intakeAutonomousIntake)-vexRT(Btn5D))*100;
//Move ball from high limit switch to low limit switch
if(vexRT(Btn6D) && SensorValue[indexHigh]) {
motor[indexer] = -127;
delay(intakeMoveDownTime);
}
//Shooting control
if (vexRT(Btn6U) || intakeAutonomousShoot) {
//if(intakeLongShot?abs(currentShot.velocity-flywheelVelocity)<currentShot.velocityThreshold:true)) {
if(time1[T1]>300 || !intakeLongShot) {
writeDebugStreamLine("%d", flywheelVelocity);
motor[indexer] = 127;
wait1Msec(150);
clearTimer(T1);
}
else {
motor[indexer] = (SensorValue[indexHigh])?0:127;
}
}
//Move ball down even if there is a sensor we want
else if (vexRT(Btn5D))
motor[indexer] = -127;
//Stop ball if ball is at a sensor
else if(SensorValue[indexLow]<intakeLightThreshold && !SensorValue[indexHigh]) {
motor[indexer] = 70;
clearTimer(T2);
while(time1[T2] < intakeMoveUpTime && !SensorValue[indexHigh]) { delay(20); }
motor[indexer] = 0;
}
else
motor[indexer] = 0;
delay(25);
}
}
task shooterDJ () {
startTask(feederWait);
int timesFed = 0;
bool canRunAgain = true;
while (true)
{
if(vexRT(Btn7U))
speed++;
if(vexRT(Btn7D))
speed--;
if(SensorValue[ballHigh]&&canRunAgain) {
timesFed++;
speed+=15;//7;
}
if(timesFed>0 & canRunAgain)
canRunAgain = false;
FwMotorSet(speed);
//sprintf( str, "%4d %4d", target_velocity, motor_velocity, nImmediateBatteryLevel/1000.0 );
wait1Msec(200);
}
}
task main() { //This holds the main tasks the robot initially starts with.
startTask(drive); //This starts the drivetask so we can actually drive the robot.
startTask(manipulator);
while(on) {
rightX = vexRT(Ch1);
if(abs(rightX)<thresh){ //This adds a deadzone in the controller's right X axis, so that drivers have more reliable control of the robot.
rightX = 0;
}
rightY = vexRT(Ch2);
if(abs(rightY)<thresh){ //This adds a deadzone in the controller's right Y axis, so that drivers have more reliable control of the robot.
rightY = 0;
}
leftY = vexRT(Ch3);
if(abs(leftY)<thresh){ //This adds a deadzone in the controller's left Y axis, so that drivers have more reliable control of the robot.
leftY = 0;
}
leftX = vexRT(Ch4);
if(abs(leftX)<thresh){ //This adds a deadzone in the controller's left X axis, so that drivers have more reliable control of the robot.
leftX = 0;
}
}
}
