void arcadeRightStyle(MotorHandle * motors, MotorSetter * motorSet)
{
int cmdLeft = joystickGetAnalog(1, 2) + joystickGetAnalog(1, 1);
int cmdRight = joystickGetAnalog(1, 2) - joystickGetAnalog(1, 1);
motorSet[DRIVE_TANK_LEFT](motors[DRIVE_TANK_LEFT], cmdLeft);
motorSet[DRIVE_TANK_RIGHT](motors[DRIVE_TANK_RIGHT], cmdRight);
}
void tankStyle(MotorHandle * motors, MotorSetter * motorSet)
{
int cmdLeft = joystickGetAnalog(1, 3);
int cmdRight = joystickGetAnalog(1, 2);
motorSet[DRIVE_TANK_LEFT](motors[DRIVE_TANK_LEFT], cmdLeft);
motorSet[DRIVE_TANK_RIGHT](motors[DRIVE_TANK_RIGHT], cmdRight);
}
/*
* 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() {
while (1) {
//Joystick Axis
int C1LX = joystickGetAnalog(1, 4);
int C1LY = joystickGetAnalog(1, 3);
int C1RX = joystickGetAnalog(1, 1);
int C1RY = joystickGetAnalog(1, 2);
//Joytick button
bool isConveyorRunning = joystickGetDigital(1, 8, JOY_DOWN);
bool isFlyWheelRunning = joystickGetDigital(1, 7, JOY_DOWN);
//Update motors
//Drivetrain
motorSet(frontLeft,C1LY + C1RX);
motorSet(frontRight,C1LY - C1RX);
motorSet(backLeft,C1LY + C1RX);
motorSet(backRight,C1LY - C1RX);
motorSet(middleH, C1LX);
//Conveyor
if (isConveyorRunning){
motorSet(conveyor, 127);
}
//Flywheel
if (isFlyWheelRunning){
motorSet(leftTop, 127);
motorSet(rightTop, 127);
motorSet(leftBottom, -127);
motorSet(rightBottom, -127);
}
delay(20);
}
}
int joystickAxes(int channel){
if (abs(joystickGetAnalog(1,channel))>15){
return joystickGetAnalog(1, channel);
}
else {
return 0;
}// This function only returns the joystick value if it passes the threshold
}
/*
* Checks joystick input and sets all Motor structs to appropriate output
* @param controlStyle The format of the joystick input.
* Can be:
* TANKDRIVE
* ARCADEDRIVE
* CHEEZYDRIVE
* MECANUMDRIVE
* HDRIVE
*/
void getJoystickForDriveTrain() {
int x1 = joystickGetAnalog(1, 4);
int y1 = joystickGetAnalog(1, 3);
int x2 = joystickGetAnalog(1, 1);
int y2 = joystickGetAnalog(1, 2);
switch(controlStyle) {
case CTTANKDRIVE:
MOTDTFrontLeft.out = y1;
MOTDTFrontMidLeft.out = y1;
MOTDTMidLeft.out = y1;
MOTDTBackLeft.out = y1;
MOTDTFrontRight.out = y2;
MOTDTFrontMidRight.out = y2;
MOTDTMidRight.out = y2;
MOTDTBackRight.out = y2;
break;
case CTARCADEDRIVE:
MOTDTFrontLeft.out = (y1 + x1) / 2;
MOTDTFrontMidLeft.out = (y1 + x1) / 2;
MOTDTMidLeft.out = (y1 + x1) / 2;
MOTDTBackLeft.out = (y1 + x1) / 2;
MOTDTFrontRight.out = (y1 - x1) / 2;
MOTDTFrontMidRight.out = (y1 - x1) / 2;
MOTDTMidRight.out = (y1 - x1) / 2;
MOTDTBackRight.out = (y1 - x1) / 2;
break;
case CTCHEEZYDRIVE:
MOTDTFrontLeft.out = (y1 + x2) / 2;
MOTDTFrontMidLeft.out = (y1 + x2) / 2;
MOTDTMidLeft.out = (y1 + x2) / 2;
MOTDTBackLeft.out = (y1 + x2) / 2;
MOTDTFrontRight.out = (y1 - x2) / 2;
MOTDTFrontMidRight.out = (y1 - x2) / 2;
MOTDTMidRight.out = (y1 - x2) / 2;
MOTDTBackRight.out = (y1 - x2) / 2;
break;
case CTMECANUMDRIVE:
MOTDTFrontLeft.out = y1 + x2 + x1;
MOTDTBackLeft.out = y1 + x2 - x1;
MOTDTFrontRight.out = y1 - x2 - x1;
MOTDTBackRight.out = y1 - x2 + x1;
break;
case CTHDRIVE:
default:
break;
}
}
/*
* 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() {
int leftPower,rightPower;
setTeamName("Team1");
while (1) {
if (isJoystickConnected(CONTROLLER)){
leftPower = joystickGetAnalog(CONTROLLER,3);
rightPower = joystickGetAnalog(CONTROLLER,2);
motorSet(LEFTMOTOR,-leftPower);
motorSet(RIGHTMOTOR,-rightPower);
motorSet(ARM,-30*(joystickGetDigital(CONTROLLER,5,JOY_UP)-joystickGetDigital(CONTROLLER,5,JOY_DOWN)));
motorSet(CLAW,45*(joystickGetDigital(CONTROLLER,6,JOY_UP)-joystickGetDigital(CONTROLLER,6,JOY_DOWN)));
}
}
}
void UC_drive() {
if (joystickGetDigital(1, 8, JOY_LEFT)) {
morpheus = 1;
} else if (joystickGetDigital(1, 8, JOY_LEFT)) {
morpheus = 0;
}
// Pneumatics
digitalWrite(RightMorpheus, morpheus);
digitalWrite(LeftMorpheus, morpheus);
// Basic movement
int channel1 = cubicMap(joystickGetAnalog(1, 1));
int channel3 = cubicMap(joystickGetAnalog(1, 3));
int channel4 = cubicMap(joystickGetAnalog(1, 4));
if (morpheus == 0) {
//motor(LeftFrontMotor1) = motor(LeftBackMotor1) = motor(LeftBackMotor2) = channel3 + TURN_SPEED * channel1;
//motor(RightFrontMotor1) = motor(RightBackMotor1) = motor(RightBackMotor2) = channel3 - TURN_SPEED * channel1;
// tank drive
float morpheus1 = channel3 + TURN_SPEED * channel1;
motorSet(LeftFrontMotor1, morpheus1);
motorSet(LeftBackMotor1, morpheus1);
motorSet(LeftBackMotor2, morpheus1);
float morpheus2 = channel3 - TURN_SPEED * channel1;
motorSet(RightFrontMotor1, morpheus2);
motorSet(RightBackMotor1, morpheus2);
motorSet(RightBackMotor2, morpheus2);
} else {
// x-drive
motorSet(LeftFrontMotor1, channel3 + channel4 + channel1);
motorSet(RightFrontMotor1, channel3 - channel4 - channel1);
motorSet(LeftBackMotor1, channel3 - channel4 + channel1);
motorSet(LeftBackMotor2, channel3 - channel4 + channel1);
motorSet(RightBackMotor1, channel3 + channel4 - channel1);
motorSet(RightBackMotor2, channel3 + channel4 - channel1);
}
}
void operatorControl() {
/*TaskHandle kfWriter = taskRunLoop(writeKFValues, 20);
delay(3000);*/
/*forward(100, 500);
forward(0, 500);
forward(-100, 500);
forward(0, 1000);
fclose(file);
taskSuspend(kfWriter);*/
bool running = false;
TaskHandle kfWriter = NULL;
while(1) {
if (abs(joystickGetAnalog(1, 2) > 10)) {
//forward(joystickGetAnalog(1, 2), 20);
motorSet(frontLeft, -127);
motorSet(middleLeft, -127 * 0.7143);
motorSet(backLeft, 127);
motorSet(frontRight, 127);
motorSet(middleRight, 127 * 0.7143);
motorSet(backRight, -127);
} else {
//forward(0, 20);
motorStop(backLeft);
motorStop(middleLeft);
motorStop(frontLeft);
motorStop(backRight);
motorStop(middleRight);
motorStop(frontRight);
}
if (joystickGetDigital(1, 8, JOY_DOWN)) {
if (!running) {
kfWriter = taskRunLoop(writeKFValues, 20);
delay(3000);
running = true;
}
}
if (joystickGetDigital(1, 7, JOY_DOWN)) {
if (running) {
fclose(file);
taskSuspend(kfWriter);
running = false;
}
}
delay(20);
}
}
//If it is a button a 1 is returned for true and 0 for false
//otherwise a value between -127 and 127 is returned for the particular axis
int readJoystick(JoyInput button) {
unsigned char joy = 1;
if (button.onPartnerJoystick) {
joy = 2;
}
if (button.channel == 1 || button.channel == 2 || button.channel == 3
|| button.channel == 4) {
return joystickGetAnalog(joy, button.channel);
} else {
if (joystickGetDigital(joy, button.channel, button.btn)) {
return 1;
} else {
return 0;
}
}
}
void calibrate(){
if (bot == calib){
wind(joystickGetAnalog(1, 3));
if (joystickGetDigital(1, 7, JOY_RIGHT)){
encoderReset(encode);
}
}
int oldval = leftdraw;
//raise the left shooting value
while (joystickGetDigital(1, 7, JOY_UP)){
leftdraw = oldval - 5;
delay(25);
}
//lower left shooting value
while (joystickGetDigital(1, 7, JOY_DOWN)){
leftdraw = oldval + 5;
delay(25);
}
oldval = rightdraw;
//raise right shooting value
while (joystickGetDigital(1, 8, JOY_UP)){
rightdraw = oldval + 5;
delay(25);
}
//lower left shooting value
while (joystickGetDigital(1, 8, JOY_DOWN)){
rightdraw = oldval - 5;
delay(25);
}
char buf[16];
sprintf(buf, "R: %d L: %d", rightdraw, leftdraw);
lcdSetText(uart1, 2, buf);
}
/*
* 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);
}
int OIGetDriveRight()
{
return joystickGetAnalog(1, 2);
}
int OIGetDriveLeft()
{
return joystickGetAnalog(1, 3);
}
void operatorControl() {
//autonomous();
unsigned long prevWakeupTime = millis();
// PID controller variable
struct pid_dat arm_pid;
initPID(&arm_pid, 2, 0.5, 0.02);
while (1) {
/* Stores which joysticks are connected
* 0 - None
* 1 - Only Joystick 1
* 2 - Only Joystick 2
* 3 - Both Josticks 1 and 2
*/
int joystickStatus = 0;
if (isJoystickConnected(1))
joystickStatus |= 1;
if (isJoystickConnected(2))
joystickStatus |= 2;
// Local Variables
int moveX = 0, moveY = 0, rotate = 0, arm = 0, liftSpeed = 0;
int L, R, C;
bool liftUp = 0, liftDw = 0, supUp = 0, supDw = 0, liftCenter = 0, liftLower = 0;
// Subteam Definition
int team1 = digitalRead(1);
// Get Joystick Values based on Status
if (joystickStatus == 3) {
// Driving
moveX = joystickGetAnalog(1, JOY_LX); // Movement
moveY = joystickGetAnalog(1, JOY_LY); // Movement
rotate = joystickGetAnalog(1, JOY_RX); // Rotate
arm = joystickGetAnalog(2, JOY_RY);
// Support
supUp = joystickGetDigital(1, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(1, JOY_LBUM, JOY_UP);
// Both joysticks connected, reference as 1 or 2
liftUp = joystickGetDigital(1, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(1, JOY_RBUM, JOY_UP);
liftCenter = joystickGetDigital(2, JOY_RBUM, JOY_UP);
liftLower = joystickGetDigital(2, JOY_RBUM, JOY_DOWN);
} else {
// Driving
moveX = joystickGetAnalog(joystickStatus, JOY_LX); // Movement
moveY = joystickGetAnalog(joystickStatus, JOY_LY); // Movement
rotate = joystickGetAnalog(joystickStatus, JOY_RX); // Rotate
// Support
supUp = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_DOWN);
supDw = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_UP);
// Lift
liftUp = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_DOWN);
liftDw = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_UP);
}
// Driving
L = CAP(moveY + rotate, RANGE_MAX);
R = CAP(moveY - rotate, RANGE_MAX);
C = CAP(moveX, RANGE_MAX);
motorSet(MC_WHEEL_L, team1 == ON ? L : L*2/3);
motorSet(MC_WHEEL_R, team1 == ON ? -R : -R*2/3);
motorSet(MC_WHEEL_M, team1 == ON ? -C : -C*2/3);
// Support
if (supDw)
motorSet(MC_SUPPORT, team1 == ON ? -32 : 32);
else if (supUp)
motorSet(MC_SUPPORT, team1 == ON ? 32 : -32);
else
motorSet(MC_SUPPORT, 0);
if (team1 == ON) {
double loc = ((MAP_INPUT(arm) + 1.0) / 2.0) * 0.9 - 0.8;
if(liftCenter) {
loc = -0.35;
} else if(liftLower) {
loc = -0.45;
}
liftSpeed = -MAP_OUTPUT(computePID(loc, MAP_POT(analogRead(1)), &arm_pid ));
}
if (liftUp)
liftSpeed = LIFTSPEED;
else if (liftDw)
liftSpeed = -LIFTSPEED;
if(team1 != ON) {
liftSpeed = -liftSpeed;
}
// Lift
motorSet(MC_LIFT_BL, liftSpeed);
motorSet(MC_LIFT_ML, liftSpeed);
motorSet(MC_LIFT_TL, liftSpeed);
motorSet(MC_LIFT_BR, -liftSpeed);
motorSet(MC_LIFT_MR, -liftSpeed);
motorSet(MC_LIFT_TR, -liftSpeed);
// Motors can only be updated once every 20ms, therefore updating at twice the rate for better response time
taskDelayUntil(&prevWakeupTime, 10);
}
}
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
int OIFireAtWill()
{
return joystickGetAnalog(2, 3) > 25
|| joystickGetDigital(1, 5, JOY_UP);
}
int OISpitAtWill()
{
return joystickGetAnalog(2, 3) < -25
|| joystickGetDigital(1, 5, JOY_DOWN);
}
int OIBallStopperDown()
{
return joystickGetAnalog(2, 1) < -25;
}
/**
* Returns the output from the joystick for the magnitude of the drive.
*/
int OIGetDriveMagnitude()
{
return joystickGetAnalog(1,3);
}
void operatorControl() {
while (1) {
/*
* ================= Primary Remote =================
*/
ch1 = joystickGetAnalog( 1 , 1 ); // Horizontal Left Analog Stick
ch2 = joystickGetAnalog( 1 , 2 ); // Vertical Left Analog Stick
ch3 = joystickGetAnalog( 1 , 3 ); // Horizontal Right Analog Stick
ch4 = joystickGetAnalog( 1 , 4 ); // Vertical Right Analog Stick
ch5u = joystickGetDigital( 1 , 5 , JOY_UP ); // Left Top Bumper
ch5d = joystickGetDigital( 1 , 5 , JOY_DOWN ); // Left Bottom Bumper
ch6u = joystickGetDigital( 1 , 6 , JOY_UP ); // Right Top Bumper
ch6d = joystickGetDigital( 1 , 6 , JOY_DOWN ); // Right Bottom Bumper
ch7u = joystickGetDigital( 1 , 7 , JOY_UP ); // Left Keypad Top Button
ch7d = joystickGetDigital( 1 , 7 , JOY_DOWN ); // Left Keypad Bottom Button
ch7l = joystickGetDigital( 1 , 7 , JOY_LEFT ); // Left Keypad Left Button
ch7r = joystickGetDigital( 1 , 7 , JOY_RIGHT ); // Left Keypad Right Button
ch8u = joystickGetDigital( 1 , 8 , JOY_UP ); // Right Keypad Top Button
ch8d = joystickGetDigital( 1 , 8 , JOY_DOWN ); // Right Keypad Bottom Button
ch8l = joystickGetDigital( 1 , 8 , JOY_LEFT ); // Right Keypad Left Button
ch8r = joystickGetDigital( 1 , 8 , JOY_RIGHT ); // Right Keypad Right Button
/*
* ================= Secondary Remote =================
*/
sch1 = joystickGetAnalog( 2 , 1 ); // Horizontal Left Analog Stick
sch2 = joystickGetAnalog( 2 , 2 ); // Vertical Left Analog Stick
sch3 = joystickGetAnalog( 2 , 3 ); // Horizontal Right Analog Stick
sch4 = joystickGetAnalog( 2 , 4 ); // Vertical Right Analog Stick
sch5u = joystickGetDigital( 2 , 5 , JOY_UP ); // Left Top Bumper
sch5d = joystickGetDigital( 2 , 5 , JOY_DOWN ); // Left Bottom Bumper
sch6u = joystickGetDigital( 2 , 6 , JOY_UP ); // Right Top Bumper
sch6d = joystickGetDigital( 2 , 6 , JOY_DOWN ); // Right Bottom Bumper
sch7u = joystickGetDigital( 2 , 7 , JOY_UP ); // Left Keypad Top Button
sch7d = joystickGetDigital( 2 , 7 , JOY_DOWN ); // Left Keypad Bottom Button
sch7l = joystickGetDigital( 2 , 7 , JOY_LEFT ); // Left Keypad Left Button
sch7r = joystickGetDigital( 2 , 7 , JOY_RIGHT ); // Left Keypad Right Button
sch8u = joystickGetDigital( 2 , 8 , JOY_UP ); // Right Keypad Top Button
sch8d = joystickGetDigital( 2 , 8 , JOY_DOWN ); // Right Keypad Bottom Button
sch8l = joystickGetDigital( 2 , 8 , JOY_LEFT ); // Right Keypad Left Button
sch8r = joystickGetDigital( 2 , 8 , JOY_RIGHT ); // Right Keypad Right Button
/*
* Location ~ Movement
*/
RobotBase = baseSetQuad(RobotBase, ch4, ch3, ch5u, false, ch6u, false, 80);
delay(TIME_DELAY);
}
}
void operatorControl() {
lcdInit(uart1);
lcdClear(uart1);
lcdSetBacklight(uart1, true);
liftTaskCreate();
bool hookButtonPressed = false;
int imeLeft;
int imeRight;
while (true) {
imeGet(0, &imeLeft);
imeGet(1, &imeRight);
lcdPrint(uart1, 1, "IME Left %d", imeLeft);
lcdPrint(uart1, 2, "IME Right %d", imeRight);
// toggle hook
bool hookButtonPressedNow = joystickGetDigital(1, 7, JOY_UP);
if (hookButtonPressedNow && hookButtonPressedNow != hookButtonPressed) {
if (isHookDeployed()) {
retractHook();
} else {
deployHook();
}
}
hookButtonPressed = hookButtonPressedNow;
if (joystickGetDigital(1, 8, JOY_UP)) {
hang();
}
if (joystickGetDigital(1, 8, JOY_DOWN)) {
unhang();
}
// Drive
int leftY = ramp(joystickGetAnalog(1, 3));
int rightY = ramp(joystickGetAnalog(1, 2));
motorSet(driveFrontRight, -rightY);
motorSet(driveMiddleRight, -rightY);
motorSet(driveBackRight, -rightY);
motorSet(driveFrontLeft, leftY);
motorSet(driveMiddleLeft, leftY);
motorSet(driveBackLeft, leftY);
// Lift
int liftUp = joystickGetDigital(1, 6, JOY_UP);
int liftDown = joystickGetDigital(1, 6, JOY_DOWN);
if (liftUp && liftDown) {
//motorSet(liftLeft, 40);
//motorSet(liftRight, -40);
} else if (liftUp) {
liftManual(LIFT_SPEED);
//motorSet(liftLeft, 127);
//motorSet(liftRight, -127);
} else if (liftDown) {
liftManual(-LIFT_SPEED);
//motorSet(liftLeft, -127);
//motorSet(liftRight, 127);
} else {
//motorSet(liftLeft, 0);
//motorSet(liftRight, 0);
}
// Manipulator
int manipulatorIn = joystickGetDigital(1, 5, JOY_DOWN);
int manipulatorOut = joystickGetDigital(1, 5, JOY_UP);
if (manipulatorIn) {
motorSet(intakeLeft, -127);
motorSet(intakeRight, 127);
} else if (manipulatorOut) {
motorSet(intakeLeft, 60);
motorSet(intakeRight, -60);
} else {
motorSet(intakeLeft, 0);
motorSet(intakeRight, 0);
}
delay(50);
}
}
/**
* Returns the output from the joystick for the rotation of the drive.
*/
int OIGetDriveRotation()
{
return joystickGetAnalog(1,4);
}
int OIBallStopperUp()
{
return joystickGetAnalog(2, 1) > 25;
}
/*
* 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);
}
}
