/*
* 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);
}
}
void pincers(){
if (joystickGetDigital(1, 5, JOY_UP)){// close claw
//if ((analogRead(pince_pot_left) + analogRead(pince_pot_right))/2 < 4096){
// we want to change speeds at a certain angle
// that angle is 224
// if less than 224 degrees, then go full speed
motorSet(pincer_right, -127);
motorSet(pincer_left, -127);
pinMode(led_Pincer,OUTPUT);
// }
// else{
// otherwise go at a lower speed
// will help with motor stalling when trying to hold stars and cubes
// motorSet(pincer_right, -50);
// motorSet(pincer_left, -50);
//}
}
else if (joystickGetDigital(1, 5, JOY_DOWN)){// open claw
motorSet(pincer_right, 127);
motorSet(pincer_left, 127);
pinMode(led_Pincer,OUTPUT);
}
else {
motorSet(pincer_right, 0);
motorSet(pincer_left, 0);
pinMode(led_Pincer,INPUT);
}
}
void UC_intake() {
if (joystickGetDigital(1, 6, JOY_UP)) {
motorSet(Intake, 127);
} else if (joystickGetDigital(1, 6, JOY_DOWN)) {
motorSet(Intake, -127);
} else {
motorSet(Intake, 0);
}
}
int OIGetIntake2Out()
{
if(numJoysticks == 2)
{
return joystickGetDigital(1, 5, JOY_UP) ||
joystickGetDigital(2, 7, JOY_UP);
}
else
{
return joystickGetDigital(1, 7, JOY_UP);
}
}
int OIGetIntake1Out()
{
if(numJoysticks == 2)
{
return joystickGetDigital(1, 6, JOY_UP)
|| joystickGetDigital(2, 8, JOY_UP);
}
else
{
return joystickGetDigital(1, 8, JOY_UP);
}
}
int OIGetIntake2In()
{
if(numJoysticks == 2)
{
return joystickGetDigital(1, 5, JOY_DOWN) ||
joystickGetDigital(2, 7, JOY_DOWN);
}
else
{
return joystickGetDigital(1, 7, JOY_DOWN);
}
}
int OIGetIntake1In()
{
if(numJoysticks == 2)
{
return joystickGetDigital(1, 6, JOY_DOWN) ||
joystickGetDigital(2, 8, JOY_DOWN);
}
else
{
return joystickGetDigital(1, 8, JOY_DOWN);
}
}
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);
}
}
void operatorControl() {
while (true)
{
calibrate();
handleLocks();
handleLcdButtons();
legs();
int shouldshoot = 0;
while (joystickGetDigital(1, 7, JOY_LEFT)){
shouldshoot = 1;
}
//start the shoot loop
if (shouldshoot){
shootgo = 1;
bot = shoot;
shootLoop();
}
int shouldlift = 0;
while (joystickGetDigital(2, 8, JOY_LEFT)){
shouldlift = 1;
}
if (shouldlift){
if (bot == lift){
bot = control;
} else {
bot = lift;
}
}
if (bot == control){
if (joystickGetDigital(1, 8, JOY_LEFT)){
shootLeft();
}
if (joystickGetDigital(1, 8, JOY_RIGHT)){
shootRight();
}
}
delay(25);
}
}
void operatorControl() {
lcdInitiate();
while (true){
ch3 = joystickAxes(3);
ch1 = joystickAxes(1);
drive(ch3, ch1);
// lift code
lift();
// pincer code
pincers();
lcd();
delay(100);
if(joystickGetDigital(2,7,JOY_DOWN)){
motorSet(left_lift_Motor1, -127);
delay(2000);
motorSet(left_lift_Motor1, 0);
motorSet(left_lift_Motor2, 127);
delay(2000);
motorSet(left_lift_Motor2, 0);
motorSet(right_lift_Motor3, -127);
delay(2000);
motorSet(right_lift_Motor3, 0);
motorSet(right_lift_Motor4, 127);
delay(2000);
motorSet(right_lift_Motor4, 0);
}
}
}
/*
* 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 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() {
while (1) {
if(joystickGetDigital(1, 6, JOY_UP))
{
motorSet(1, 127);
}
else if(joystickGetDigital(1, 6, JOY_DOWN))
{
motorSet(1, -127);
}
else
{
motorSet(1, 0);
}
delay(20);
}
}
void lift(){
if (joystickGetDigital(1, 6, JOY_UP)){// lift goes up
// if(analogRead(lift_pot > 4000)){// if the lift is vertical
// motorSet(pincer_right, 127);// open the pincers
// motorSet(pincer_left, -127);
// motorSet(left_lift_Motor1, 0);// stop the lift
// motorSet(left_lift_Motor2, 0);
// motorSet(right_lift_Motor3, 0);
// motorSet(right_lift_Motor4, 0);
// }
motorSet(left_lift_Motor1, -127);// lift goes up
motorSet(left_lift_Motor2, 127);
motorSet(right_lift_Motor3, -127);
motorSet(right_lift_Motor4, 127);
pinMode(led_Lift,OUTPUT);
}
else if (joystickGetDigital(1, 6, JOY_DOWN)){
motorSet(left_lift_Motor1, 127);
motorSet(left_lift_Motor2, -127);
motorSet(right_lift_Motor3, 127);
motorSet(right_lift_Motor4, -127);
pinMode(led_Lift,OUTPUT);
}
else if (joystickGetDigital(1, 8, JOY_RIGHT)){
motorSet(left_lift_Motor1, 50);
motorSet(left_lift_Motor2, -50);
motorSet(right_lift_Motor3, 50);
motorSet(right_lift_Motor4, -50);
}
else{
motorSet(left_lift_Motor1, 0);
motorSet(left_lift_Motor2, 0);
motorSet(right_lift_Motor3, 0);
motorSet(right_lift_Motor4, 0);
pinMode(led_Lift,INPUT);
}
}
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 UC_arm(int safe) {
//if (safe) {
if (joystickGetDigital(1, 5, JOY_UP)
&& joystickGetDigital(1, 5, JOY_DOWN)) {
moveArm(0);
} else if (joystickGetDigital(1, 5, JOY_UP)) { // bring the arm up
moveArm(127);
} else if (joystickGetDigital(1, 5, JOY_DOWN)) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
/*} else {
if (vexRT[Btn5U] == 1 && vexRT[Btn5D] == 1) {
moveArm(0);
} else if (vexRT[Btn5U] == 1) { // bring the arm up
moveArm(127);
} else if (vexRT[Btn5D] == 1) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
}*/
}
//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 hang() {
liftTaskDelete();
// engage the winch
digitalWrite(winchPiston, 1);
// give it a chance to engage
taskDelay(100);
int leftTicks = analogRead(potLiftLeft);
int rightTicks = analogRead(potLiftRight);
int changesArrayLength = STALL_TIME/HANG_DT;
int changes[changesArrayLength]; // -1 indicates no data
fillArray(changes, changesArrayLength, -1);
int stallTicks = 0;
while (true) {
if (joystickGetDigital(1, 8, JOY_DOWN)) {
return;
}
int newLeftTicks = analogRead(potLiftLeft);
int newRightTicks = analogRead(potLiftRight);
if (newLeftTicks < HANG_HEIGHT && newRightTicks < HANG_HEIGHT) {
// we're at the right height, stop
break;
}
if ((stallTicks * HANG_DT) > STALL_DELAY) {
// do stall detection
int change = abs(leftTicks - newLeftTicks) + abs(rightTicks - newRightTicks);
pushArrayValue(changes, changesArrayLength, change);
int totalChanges = sumStallChanges(changes, changesArrayLength);
// if changes has data (>= 0) and it's stalled, stop motors
if (totalChanges >= 0 && totalChanges < STALL_THRESHOLD) {
// stalled, wait 3 seconds
hangMotors(0);
delay(3000);
// clear changes
fillArray(changes, changesArrayLength, -1);
continue;
}
}
leftTicks = newLeftTicks;
rightTicks = newRightTicks;
hangMotors(127);
stallTicks++;
taskDelay(HANG_DT);
}
hangMotors(0);
}
int OIBallStopperUp()
{
return joystickGetDigital(1, 8, JOY_UP);
}
int OIShooterUp()
{
return joystickGetDigital(numJoysticks, 6, JOY_UP);
}
int OIShooterOff()
{
return joystickGetDigital(numJoysticks, 5, JOY_DOWN);
}
int OIFenderShoot()
{
return joystickGetDigital(2, 7, JOY_RIGHT);
}
int OIShooterDown()
{
return joystickGetDigital(numJoysticks, 5, JOY_UP);
}
int OIHalfCourtShoot()
{
return joystickGetDigital(2, 7, JOY_LEFT);
}
int OIBallStopperDown()
{
return joystickGetDigital(1, 8, JOY_DOWN);
}
int OIReduceDrive()
{
return joystickGetDigital(1,6,JOY_DOWN);
}
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() {
//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 legs(){
if (bot == lift){
if (joystickGetDigital(2, 7, JOY_UP)){
drinv = 1;
}
if (joystickGetDigital(2, 7, JOY_DOWN)){
drinv = -1;
}
if (joystickGetDigital(2, 7, JOY_LEFT)){
oneleg_hold = 1;
otherleg_hold = 1;
}
if (joystickGetDigital(2, 7, JOY_RIGHT)){
oneleg_hold = 0;
otherleg_hold = 0;
}
int lflgo = joystickGetDigital(2, 5, JOY_UP);
int lrlgo = joystickGetDigital(2, 5, JOY_DOWN);
int rflgo = joystickGetDigital(2, 6, JOY_UP);
int rrlgo = joystickGetDigital(2, 6, JOY_DOWN);
if (oneleg_hold){
if (lflgo){
motorSet(left_front_leg, drinv * left_front_leg_speed);
} else if (joystickGetDigital(2, 8, JOY_UP)){
oneleg_hold = 0;
} else {
motorSet(left_front_leg, lflhold);
}
} else {
if (joystickGetDigital(2, 8, JOY_UP)){
motorSet(left_front_leg, -left_front_leg_speed);
} else if (lflgo){
motorSet(left_front_leg, drinv * left_front_leg_speed);
} else {
motorSet(left_front_leg, 0);
}
}
if (lrlgo){
motorSet(left_rear_leg, drinv * left_rear_leg_speed);
} else {
if (otherleg_hold){
motorSet(left_rear_leg, lrlhold);
} else {
motorSet(left_rear_leg, 0);
}
}
if (rflgo){
motorSet(right_front_leg, drinv * right_front_leg_speed);
} else {
if (otherleg_hold){
motorSet(right_front_leg, rflhold);
} else {
motorSet(right_front_leg, 0);
}
}
if (rrlgo){
motorSet(right_rear_leg, drinv * right_rear_leg_speed);
} else if (joystickGetDigital(2, 8, JOY_RIGHT)){
motorSet(right_rear_leg, -right_rear_leg_speed);
} else {
if (otherleg_hold){
motorSet(right_rear_leg, rrlhold);
} else {
motorSet(right_rear_leg, 0);
}
}
} else {
stopLift();
}
}
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
