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; } } }