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