diff --git a/First_Full_Design.c b/First_Full_Design.c index 1aa9749..e1bb5d2 100644 --- a/First_Full_Design.c +++ b/First_Full_Design.c @@ -1,5 +1,8 @@ -#pragma config(Sensor, in1, clawPotent, sensorPotentiometer) -#pragma config(Motor, port1, claw, tmotorVex393HighSpeed_HBridge, openLoop) +#pragma config(Sensor, in1, liftPosition, sensorPotentiometer) +#pragma config(Sensor, in3, clawLiftPosition, sensorPotentiometer) +#pragma config(Sensor, in5, mobileLiftPosition, sensorPotentiometer) +#pragma config(Sensor, dgtl1, coneHeight, sensorSONAR_cm) +#pragma config(Motor, port1, clawMotor, tmotorVex393HighSpeed_HBridge, openLoop) #pragma config(Motor, port2, wheelBackL, tmotorVex393HighSpeed_MC29, openLoop) #pragma config(Motor, port3, wheelBackR, tmotorVex393HighSpeed_MC29, openLoop) #pragma config(Motor, port4, wheelFrontL, tmotorVex393HighSpeed_MC29, openLoop) @@ -8,15 +11,7 @@ #pragma config(Motor, port7, armLiftR, tmotorVex393HighSpeed_MC29, openLoop) #pragma config(Motor, port8, clawLiftL, tmotorVex393HighSpeed_MC29, openLoop) #pragma config(Motor, port9, clawLiftR, tmotorVex393HighSpeed_MC29, openLoop) -#pragma config(Motor, port10, mobileLift, tmotorVex393HighSpeed_HBridge, openLoop) -//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// - -//#pragma config(Motor, port2, WheelRightOne, tmotorVex393HighSpeed_MC29, openLoop, reversed) -//#pragma config(Motor, port3, WheelLeftOne, tmotorVex393HighSpeed_MC29, openLoop) -//#pragma config(Motor, port4, mRight, tmotorVex393_MC29, openLoop) -//#pragma config(Motor, port5, mLeft, tmotorVex393_MC29, openLoop, reversed) -//#pragma config(Motor, port6, WheelLeftTwo, tmotorVex393HighSpeed_MC29, openLoop) -//#pragma config(Motor, port7, WheelRightTwo, tmotorVex393HighSpeed_MC29, openLoop, reversed) +#pragma config(Motor, port10, mobileLift, tmotorVex393HighSpeed_HBridge, openLoop, reversed) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// /*---------------------------------------------------------------------------*/ @@ -34,12 +29,60 @@ //Main competition background code...do not modify! #include "Vex_Competition_Includes.c" +// --------------------------------------- +// TASKS +task move(); +task lift(); +task claw(); +// END TASKS + + +// --------------------------------------- +// FUNCTIONS +// LIFT void moveLift(int power); -void stopAll(); +void moveLiftUp(int power); +void moveLiftDown(int power); + +int getClawLiftPosition(); + +void moveLiftUpToConeHeightAndStop(int power); +void moveLiftDownToConeHeightAndStop(int power); +// void moveLiftUpAndStop(int distance, int power); +void moveLiftDownAndStop(int distance, int power); + +// CLAW LIFT void moveClawLift(int power); +void moveClawLiftUpAndStop(int distance, int power); +void moveClawLiftDownAndStop(int distance, int power); + +// CLAW void moveClaw(int power); +void openClaw(int power); +void closeClaw(int power); + +// MOBILE LIFT +void moveMobileLift(int power); +void moveMobileLiftUp(int power); +void moveMobileLiftDown(int power); +void moveMobileLiftUpAndStop(int distance, int power); +void moveMobileLiftDownAndStop(int distance, int power); +void moveMobileLiftDownToRelease(); + +// MACRO +void moveLiftUpToConeHeightAndRelease(int powerLift, int distanceClawLift, int powerClawLift); +void moveClawLiftDownAndPickUp(int distanceLift, int powerLift, int distanceClawLift, int powerClawLift); + +// void moveLiftUpAndRelease(int distanceLift, int powerLift, int distanceClawLift, int powerClawLift); + +// HELPERS void dropCone(); -task move(); + +void stopAll(); +void stopClawLift(); +void stopClaw(); + +// END FUNCTIONS /*---------------------------------------------------------------------------*/ /* Pre-Autonomous Functions */ @@ -97,9 +140,106 @@ task autonomous() /* You must modify the code to add your own robot specific commands here. */ /*---------------------------------------------------------------------------*/ +task move() +{ + while (true) + { + motor[wheelBackL] = -(vexRT[Ch3] + vexRT[Ch1]); + motor[wheelFrontL] = -(vexRT[Ch3] + vexRT[Ch1]); + //motor[wheelBackR] = -(vexRT[Ch3] - vexRT[Ch1]); + //motor[wheelFrontR] = -(vexRT[Ch3] + vexRT[Ch1]); + } +} + + +task lift() +{ + const int powerliftUp = 80; + const int powerliftDown = 40; + const int powerClawLiftUp = 80; + const int powerClawLiftDown = 80; + const int powerMobileLift = 120; + + while (true) + { + if (vexRT[Btn8R] == 1) { + if (vexRT[Btn7U] == 1) { //Move Lift up with btn 7U + moveLift(powerliftUp); //80 + if (vexRT[Btn7U] == 0) { + moveLift(5); //holding power + } + } + else if (vexRT[Btn7D] == 1){ //Move Lift down with btn 7D + moveLift(-40); //-40 + if (vexRT[Btn7D] == 0) { + moveLift(-15); //holding power + } + } + else if (vexRT[Btn5U] == 1){ + moveClawLift(powerClawLiftUp); + } + else if (vexRT[Btn5D] == 1){ + moveClawLift(powerClawLiftDown); + } + else if (vexRT[Btn5D] == 0 && vexRT[Btn5D] == 0){ + stopClawLift(); + } + } + else if (vexRT[Btn8R] == 0) { + if (vexRT[Btn7U] == 1) { + moveLiftUpToConeHeightAndRelease(powerliftUp, 3220, powerClawLiftUp); + } + else if (vexRT[Btn7D] == 1) { + moveClawLiftDownAndPickUp(230, powerliftDown, 1900, powerClawLiftDown); + // moveLiftDownAndStop(230, powerliftDown); + } + else if (vexRT[Btn5U] == 1){ + moveClawLiftUpAndStop(3150, powerClawLiftUp); + } + else if (vexRT[Btn5D] == 1){ + moveClawLiftDownAndStop(1900, powerClawLiftDown); + } + // else if (vexRT[Btn5U] == 0 && vexRT[Btn5D] == 0){ + // stopClawLift(); + // } + else if (vexRT[Btn7R] == 1){ + moveMobileLiftDownToRelease(); + // moveMobileLiftDownAndStop(750, 80); + // moveMobileLiftDown(30); + } + else if (vexRT[Btn7L] == 1){ + moveMobileLiftUpAndStop(2100, powerMobileLift); + // moveMobileLiftUp(30); + } + } + + } +} + +task claw() +{ + while (true) + { + if (vexRT[Btn6U] == 1){ + closeClaw(50); + if (vexRT[Btn6U] == 0) { + closeClaw(10); + } + } + else if (vexRT[Btn6D] == 1){ + openClaw(50); + wait1Msec(225); + stopClaw(); + } + } +} + + task usercontrol() { startTask(move); + startTask(lift); + startTask(claw); // User control code here, inside the loop //int on = 0; while (true) @@ -118,49 +258,47 @@ task usercontrol() else if (on != 0){ moveLift(0); }*/ - - while(vexRT[Btn7U] == 1){ //Move Lift up with btn 7U - moveLift(50); //80 - if (vexRT[Btn7U] == 0) { - moveLift(5); //holding power - } - } - while(vexRT[Btn7D] == 1){ //Move Lift down with btn 7D - moveLift(-40); //-40 - if (vexRT[Btn7D] == 0) { - moveLift(-15); //holding power - } - } while(vexRT[Btn8L] == 1){ stopAll(); } while (vexRT[Btn8R] == 1) { dropCone(); } - while(vexRT[Btn6U] == 1){ - moveClaw(-50); - if(vexRT[Btn6U] == 0) { - moveClaw(-10); - } - } - while(vexRT[Btn6D] == 1){ - moveClaw(50); - wait1Msec(225); - moveClaw(0); - } - while(vexRT[Btn5U] == 1){ - moveClawLift(50); - } - while(vexRT[Btn5D] == 1){ - moveClawLift(-70); - } - moveClawLift(0); + + } } + +// ------------------------------------- +// SENSORS +int getConeHeight() +{ + return SensorValue(coneHeight); +} +int getLiftPosition() +{ + return SensorValue(liftPosition); +} + +int getClawLiftPosition() +{ + return SensorValue(clawLiftPosition); +} + +int getMobileLiftPosition() +{ + return SensorValue(mobileLiftPosition); +} + +// END SENSORS + + +// ------------------------------------- +// STOPS void stopAll(){ motor[armLiftL] = 0; motor[armLiftR] = 0; - motor[claw] = 0; + motor[clawMotor] = 0; motor[clawLiftL] = 0; motor[clawLiftR] = 0; motor[mobileLift] = 0; @@ -169,26 +307,234 @@ void stopAll(){ motor[wheelFrontL] = 0; motor[wheelFrontR] = 0; } +// END STOPS + +// ------------------------------------- +// LIFT +void stopLift(){ + moveLift(0); +} + +void moveLiftUp(int power) { + moveLift(power); +} + +void moveLiftDown(int power) { + moveLift(-power); +} + void moveLift(int power){ motor[armLiftL] = power; - motor[armLiftR] = power; + motor[armLiftR] = power; +} + +// HELPER +void moveLiftUpToConeHeightAndRelease(int powerLift, int distanceClawLift, int powerClawLift) { + moveLiftUpToConeHeightAndStop(powerLift); + wait1Msec(200); + moveClawLiftUpAndStop(distanceClawLift, powerClawLift); + wait1Msec(200); + wait1Msec(200); + moveLiftDownToConeHeightAndStop(50); + wait1Msec(200); + openClaw(50); + wait1Msec(225); + stopClaw(); +} + +void moveLiftUpToConeHeightAndStop(int power) { + int height = getConeHeight(); + while (height > 0 && height < 20){ + moveLiftUp(power); + if (vexRT[Btn8L] == 1) + { + break; + } + + height = getConeHeight(); + } + + // wait1Msec(100); + stopLift(); +} + +void moveLiftDownToConeHeightAndStop(int power) { + while (getConeHeight() > 20){ + moveLiftDown(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + + // wait1Msec(100); + stopLift(); +} + +// void moveLiftUpAndStop(int distance, int power) { + +// while (getLiftPosition() < distance){ +// moveLiftUp(power); +// if (vexRT[Btn8L] == 1) +// { +// break; +// } +// } +// stopLift(); +// } + +void moveClawLiftDownAndPickUp(int distanceLift, int powerLift, int distanceClawLift, int powerClawLift) { + moveLiftUpToConeHeightAndStop(powerLift + 10); + wait1Msec(200); + moveClawLiftDownAndStop(distanceClawLift, powerClawLift); + wait1Msec(200); + moveLiftDownAndStop(distanceLift, powerLift); + wait1Msec(200); +} + +void moveLiftDownAndStop(int distance, int power) { + + while (getLiftPosition() > distance){ + moveLiftDown(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + wait1Msec(200); + stopLift(); +} +// END MOVES + +// ------------------------------------- +// CLAW LIFT +void stopClawLift(){ + moveClawLift(0); +} + +void moveClawLiftUp(int power){ + moveClawLift(power); +} + +void moveClawLiftDown(int power){ + moveClawLift(-power); } + void moveClawLift(int power){ motor[clawLiftL] = power; motor[clawLiftR] = -power; } + +void moveClawLiftUpAndStop(int distance, int power) { + + while (getClawLiftPosition() < distance){ + moveClawLiftUp(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + stopClawLift(); +} + +void moveClawLiftDownAndStop(int distance, int power) { + + while (getClawLiftPosition() > distance){ + moveClawLiftDown(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + stopClawLift(); +} +// END CLAW LIFT + + +// ------------------------------------- +// CLAW +void stopClaw(){ + moveClaw(0); +} + +void openClaw(int power){ + moveClaw(power); +} + +void closeClaw(int power){ + moveClaw(-power); +} + void moveClaw(int power){ - motor[claw] = power; + motor[clawMotor] = power; } + void dropCone() { } -task move () -{ - while (true) - { - motor[wheelBackL] = -(vexRT[Ch3] + vexRT[Ch1]); - motor[wheelFrontL] = -(vexRT[Ch3] + vexRT[Ch1]); - //motor[wheelBackR] = -(vexRT[Ch3] - vexRT[Ch1]); - //motor[wheelFrontR] = -(vexRT[Ch3] + vexRT[Ch1]); +// END CLAW + + +// ------------------------------------- +// MOBILE LIFT +void stopMobileLift(){ + moveMobileLift(0); +} + +void moveMobileLiftUp(int power){ + moveMobileLift(power); +} + +void moveMobileLiftDown(int power){ + moveMobileLift(-power); +} + +void moveMobileLift(int power){ + motor[mobileLift] = power; +} + +void moveMobileLiftUpAndStop(int distance, int power) { + + while (getMobileLiftPosition() < distance){ + moveMobileLiftUp(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + stopMobileLift(); +} + +void moveMobileLiftDownAndStop(int distance, int power) { + + while (getMobileLiftPosition() > distance){ + moveMobileLiftDown(power); + if (vexRT[Btn8L] == 1) + { + break; + } + } + stopMobileLift(); +} + +void moveMobileLiftDownToRelease() { + + while (getMobileLiftPosition() > 2100){ + moveMobileLiftDown(120); + if (vexRT[Btn8L] == 1) + { + break; + } } + + while (getMobileLiftPosition() > 750){ + moveMobileLiftDown(60); + if (vexRT[Btn8L] == 1) + { + break; + } + } + + + stopMobileLift(); } +// END MOBILE LIFT