task c_vexAutonomous(void *arg) { (void)arg; vexTaskRegister("auton"); StartTask(armTask); autonomous(); while (!chThdShouldTerminate()) { vexSleep(25); } return (task)0; }
void operatorControl() { //encoder1= encoderInit (2,3,false); while (1) { //int pot = analogRead (8); //printf("%d\r\n", pot); int encodeCount = encoderGet(encoder1); printf("%d\r\n", encodeCount); if (digitalRead(1) == LOW) // jumper 1 in = autonomous { autonomous (); } // declare joystick inputs///////////////////////////////////////////////////////////////////////////////// int channel2 = joystickGetAnalog(1, 2); // (joystick 1, channel 2) int channel3 = joystickGetAnalog(1, 3); // (joystick 1, channel 4) // drive functions//////////////////////////////////////////////////////////////////////////////////////// motorSet (motor1, channel2); // right drive back motorSet (motor2, channel2); // right drive front motorSet (motor10, channel3); // left drive abck motorSet (motor9, channel3); // left drive front // arm function///////////////////////////////////////////////////////////////////////////////////////////// const int INPUT_RIGHT_TRIGGER = 6; const int INPUT_LEFT_TRIGGER = 5; const int ARM_POWER = 127; const int INTAKE_POWER = 127; int rightTriggerUp = joystickGetDigital(1,INPUT_RIGHT_TRIGGER,JOY_UP); int rightTriggerDown = joystickGetDigital(1,INPUT_RIGHT_TRIGGER,JOY_DOWN); int leftTriggerUp = joystickGetDigital(1,INPUT_LEFT_TRIGGER,JOY_UP); int leftTriggerDown = joystickGetDigital(1,INPUT_LEFT_TRIGGER,JOY_DOWN); if(rightTriggerUp) { motorSet (motor4, ARM_POWER) ;// arm right up motorSet (motor3, ARM_POWER) ;// arm right down motorSet (motor8, ARM_POWER) ;// arm left down motorSet (motor7, ARM_POWER) ;// arm left up }else { motorSet (motor4, 1) ;// arm right up motorSet (motor3, 1) ;// arm right down motorSet (motor8, 1) ;// arm left down motorSet (motor7, 1) ;// arm left up } if(rightTriggerDown) { motorSet (motor4, -ARM_POWER) ;// arm right up motorSet (motor3,-ARM_POWER) ;// arm right down motorSet (motor8, -ARM_POWER) ;// arm left down motorSet (motor7, -ARM_POWER) ;// arm left up } if(leftTriggerDown) { motorSet (motor6, - INTAKE_POWER) ;// arm right up motorSet (motor5,-INTAKE_POWER) ;// arm right down } else if(leftTriggerUp) { motorSet (motor6, INTAKE_POWER) ;// arm right up motorSet (motor5, INTAKE_POWER) ;// arm right down }else { motorSet (motor6, 0) ;// arm right up motorSet (motor5, 0) ;// arm right down } #define motor5 5 // = right intake #define motor6 6 // = left intake }// operator close bracket }// end of armbot
/* * Runs the user operator control code. This function will be started in its own task with the * default priority and stack size whenever the robot is enabled via the Field Management System * or the VEX Competition Switch in the operator control mode. If the robot is disabled or * communications is lost, the operator control task will be stopped by the kernel. Re-enabling * the robot will restart the task, not resume it from where it left off. * * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will * run the operator control task. Be warned that this will also occur if the VEX Cortex is * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached. * * Code running in this task can take almost any action, as the VEX Joystick is available and * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly * recommended to give other tasks (including system tasks such as updating LCDs) time to run. * * This task should never exit; it should end with some kind of infinite loop, even if empty. */ void operatorControl() { #ifdef AUTO autonomous(); #elif defined(TEST) #define DEFAULT_SHOOTER_SPEED 0 #define SHOOTER_SPEED_INCREMENT 5 int8_t xSpeed, ySpeed, rotation; int8_t lifterSpeed/*, intakeSpeed*/; int16_t shooterSpeed = DEFAULT_SHOOTER_SPEED; //shooter is on when robot starts int8_t frontIntakeSpeed = INTAKE_SPEED; bool isShooterOn = true; bool isAutoShootOn = false; //lfilterClear(); toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT); // auto shoot on off toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down while (true) { printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54); toggleBtnUpdateAll(); // drive xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS); ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS); rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2; if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) { ySpeed = 0; } if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) { xSpeed = 0; } drive(xSpeed, ySpeed, rotation, false); // lifter up down if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) { lifterSpeed = LIFTER_SPEED; } else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) { lifterSpeed = -LIFTER_SPEED; } else { lifterSpeed = 0; } lifter(lifterSpeed); takeInInternal(lifterSpeed); if (isShooterOn) { if (isAutoShootOn) { shooterSpeed = calculateShooterSpeed(); } else { // shooter increase speed if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) { shooterSpeed += SHOOTER_SPEED_INCREMENT; if (shooterSpeed > SHOOTER_MAX_SPEED) { shooterSpeed = SHOOTER_MAX_SPEED; } } // shooter decrease speed if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { shooterSpeed -= SHOOTER_SPEED_INCREMENT; if (shooterSpeed < SHOOTER_MIN_SPEED) { shooterSpeed = SHOOTER_MIN_SPEED; } } } // auto shooter on off if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) { isAutoShootOn = !isAutoShootOn; } } // shooter on off if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { isShooterOn = !isShooterOn; shooterSpeed = isShooterOn ? DEFAULT_SHOOTER_SPEED : 0; } shooter(shooterSpeed); // intake mode if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED || toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) { frontIntakeSpeed = 0; } else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) { frontIntakeSpeed = -INTAKE_SPEED; } else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { frontIntakeSpeed = INTAKE_SPEED; } takeInFront(frontIntakeSpeed); delay(20); } #else int8_t xSpeed, ySpeed, rotation; int8_t lifterSpeed/*, intakeSpeed*/; int8_t defaultPreset = 0; int8_t currentPreset = defaultPreset; int16_t shooterSpeed = shooterSpeedPresets[defaultPreset]; //shooter is on when robot starts int8_t frontIntakeSpeed = INTAKE_SPEED; bool isShooterOn = true; //lfilterClear(); toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down while (true) { printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54); toggleBtnUpdateAll(); // drive xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS); ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS); rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2; if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) { ySpeed = 0; } if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) { xSpeed = 0; } drive(xSpeed, ySpeed, rotation, false); // lifter up down if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) { lifterSpeed = LIFTER_SPEED; } else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) { lifterSpeed = -LIFTER_SPEED; } else { lifterSpeed = 0; } lifter(lifterSpeed); takeInInternal(lifterSpeed); if (isShooterOn) { // shooter increase speed if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) { ++currentPreset; if (currentPreset >= NUM_SHOOTER_SPEED_PRESETS) { currentPreset = NUM_SHOOTER_SPEED_PRESETS - 1; } } // shooter decrease speed if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { --currentPreset; if (currentPreset < 0) { currentPreset = 0; } } shooterSpeed = shooterSpeedPresets[currentPreset]; } // shooter on off if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { isShooterOn = !isShooterOn; shooterSpeed = isShooterOn ? shooterSpeedPresets[defaultPreset] : 0; } shooter(shooterSpeed); // intake mode if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED || toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) { frontIntakeSpeed = 0; } else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) { frontIntakeSpeed = -INTAKE_SPEED; } else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) { frontIntakeSpeed = INTAKE_SPEED; } takeInFront(frontIntakeSpeed); delay(20); } #endif }
void operatorControl() { int current_pot_val=analogRead(arm_pot); if (digitalRead(2)==LOW) { //JUMPER IN DIG2=GO AUTONOMOUs autonomous(); digitalWrite(limit_bot, HIGH); digitalWrite(limit_top, HIGH); } while (1) { //Drive motors, tank config motorSet(DRIVE_FL, joystickGetAnalog(1,3)); motorSet(DRIVE_ML, joystickGetAnalog(1,3)); motorSet(DRIVE_FR, -joystickGetAnalog(1,2)); motorSet(DRIVE_MR, -joystickGetAnalog(1,2)); //Arm motors, right trigger buttons if((joystickGetDigital(1,6,JOY_UP) == 1) && (digitalRead(limit_top) == 1)) { current_pot_val=analogRead(arm_pot); motorSet(ARM_TL, -127); motorSet(ARM_BL, -127); motorSet(ARM_TR, -127); motorSet(ARM_BR, 127); if(digitalRead(limit_top)==0) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } else if((joystickGetDigital(1,6,JOY_DOWN) == 1) && (digitalRead(limit_bot) == 1)) { current_pot_val=analogRead(arm_pot); motorSet(ARM_TL, 127); motorSet(ARM_BL, 127); motorSet(ARM_TR, 127); motorSet(ARM_BR, -127); if(digitalRead(limit_bot)==0) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } else {//keep arm up if(analogRead(arm_pot)<=current_pot_val) { //higher than idle motorSet(ARM_TL, arm_idle_speed); motorSet(ARM_BL, arm_idle_speed); motorSet(ARM_TR, arm_idle_speed); motorSet(ARM_BR, -(arm_idle_speed)); if((analogRead(arm_pot)==current_pot_val)||(digitalRead(limit_bot)==0)) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } if(analogRead(arm_pot)>=current_pot_val) { //lower than idle motorSet(ARM_TL, -(arm_idle_speed)); motorSet(ARM_BL, -(arm_idle_speed)); motorSet(ARM_TR, -(arm_idle_speed)); motorSet(ARM_BR, (arm_idle_speed)); if((analogRead(arm_pot)==current_pot_val)||(digitalRead(limit_top)==0)) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } } //Intake motors, left trigger buttons if(joystickGetDigital(1,5,JOY_UP) == 1) { motorSet(IN_L, 127); motorSet(IN_R, -127); } else if(joystickGetDigital(1,5,JOY_DOWN) == 1) { motorSet(IN_L, -127); motorSet(IN_R, 127); } else { motorStop(IN_L); motorStop(IN_R); } //preset arm heights if(joystickGetDigital(1, 7,JOY_UP) == 1) { while((analogRead(arm_pot)>2202) && (digitalRead(limit_top) == 1)) { current_pot_val=2202; motorSet(ARM_TL, -127); motorSet(ARM_BL, -127); motorSet(ARM_TR, -127); motorSet(ARM_BR, 127); if(digitalRead(limit_top)==0) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } } if(joystickGetDigital(1, 7,JOY_DOWN) == 1) { while((analogRead(arm_pot)<4040) && (digitalRead(limit_bot) == 1)) { current_pot_val=4040; motorSet(ARM_TL, 127); motorSet(ARM_BL, 127); motorSet(ARM_TR, 127); motorSet(ARM_BR, -127); if(digitalRead(limit_bot)==0) { motorStop(ARM_TL); motorStop(ARM_BL); motorStop(ARM_TR); motorStop(ARM_BR); } } } //LIGHTS if(digitalRead(limit_top)==0) digitalWrite(led_r, LOW); else digitalWrite(led_r, HIGH); if(digitalRead(limit_bot)==0) digitalWrite(led_g, LOW); else digitalWrite(led_g, HIGH); } printf("%d\r\n", current_pot_val); }
/* * Runs the user operator control code. This function will be started in its own task with the * default priority and stack size whenever the robot is enabled via the Field Management System * or the VEX Competition Switch in the operator control mode. If the robot is disabled or * communications is lost, the operator control task will be stopped by the kernel. Re-enabling * the robot will restart the task, not resume it from where it left off. * * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will * run the operator control task. Be warned that this will also occur if the VEX Cortex is * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached. * * Code running in this task can take almost any action, as the VEX Joystick is available and * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly * recommended to give other tasks (including system tasks such as updating LCDs) time to run. * * This task should never exit; it should end with some kind of infinite loop, even if empty. */ void operatorControl() { //Config variables 'n stuff bool autoM = false; bool first = true; bool reset = true; const int LFRONT=1; const int LBACK=2; const int RFRONT=3; const int RBACK=4; const int WINCH1=5; const int WINCH2=6; const int FLAGDWN=7; long startM; long endM; double circ; double MPH; //main loop while (1) { //motorSet(2,50); //auto code autoM = joystickGetDigital(1, 8, JOY_LEFT); if (autoM && first) { autonomous(); first = false; } //delay(20); //Teleop code if (joystickGetDigital(1, 7, JOY_UP)) { //Drive straight int speed = joystickGetAnalog(1, 3); motorSet(LFRONT, speed); //delay(5); motorSet(LBACK, speed); //delay(5); motorSet(RFRONT, speed); //delay(5); motorSet(RBACK, speed); //delay(5); } else { //tank drive int LSpeed = joystickGetAnalog(1, 3); int RSpeed = joystickGetAnalog(1, 2); motorSet(LFRONT, LSpeed); motorSet(LBACK, LSpeed); motorSet(RFRONT, RSpeed); motorSet(RBACK, RSpeed); } //Winch 1 if (joystickGetDigital(1, 5, JOY_UP)) { motorSet(WINCH1, 60); } else if (joystickGetDigital(1, 5, JOY_DOWN)) { motorSet(WINCH1, -60); } else motorSet(WINCH1, 0); //Winch 2 if (joystickGetDigital(1, 6, JOY_UP)) { motorSet(WINCH2, 60); } else if (joystickGetDigital(1, 6, JOY_DOWN)) { motorSet(WINCH2, -60); } else motorSet(WINCH2, 0); //Flag if (joystickGetDigital(1, 8, JOY_DOWN)) { motorSet(FLAGDWN, 60); } else if (joystickGetDigital(1,8, JOY_UP)){ motorSet(FLAGDWN, -60); } else motorSet(FLAGDWN, 0); //Anemometer if(reset){ startM=micros(); encoderReset(ameter); reset=false; } long reading = encoderGet(ameter); if(reading/360==4){ endM=micros(); double revPerSec = (4.0*pow(10.0, 6.0))/((double)(startM-endM)); double revPerMin = revPerSec*60.0; MPH = circ/12.0/5280.0*revPerMin*60.0; reset=true; } //Flush Data delay(20); } }