Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //Quad Values
- int fireStop = 0;
- int loadStop = 0;
- int gateOpen = 0;
- int gateClosed = 0;
- /*
- Catapult motor power for certain distances
- Short = 1/2 Court (0)
- Mid = 3/4 Court (1)
- Long = Full Court (2)
- */
- int distance = 0;
- int shortPower = 65;
- int midPower = 70;
- int longPower = 75;
- int holdPower = -15;
- int loadPower = -127;
- bool intakeRunning = true;
- bool isGate = true;
- int armStatus = 0;
- int gateStatus = 0;
- /*
- Maintains desired arm position using Quad
- Three States:
- -Loaded (0)
- -Firing (1)
- */
- task armPosition(){
- while(true){
- if(armStatus == 1 && SensorValue[ballSonic] < 70){
- //Open gate to start loading of another ball
- gateStatus = 1;
- int speed;
- switch(distance){
- case 0:
- speed = shortPower;
- break;
- case 1:
- speed = midPower;
- break;
- case 2:
- speed = longPower;
- break;
- default:
- speed = 0;
- break;
- }
- //Fire currently loaded ball
- while(SensorValue[armQuad] < fireStop){
- if(speed == 0){
- break;
- }
- motor[yOne] = speed;
- motor[yTwo] = speed;
- motor[yThree] = speed;
- }
- motor[yOne] = 0;
- motor[yTwo] = 0;
- motor[yThree] = 0;
- //Bring arm back down
- while(SensorValue[armQuad] > loadStop){
- motor[yOne] = loadPower;
- motor[yTwo] = loadPower;
- motor[yThree] = loadPower;
- }
- motor[yOne] = holdPower;
- motor[yTwo] = holdPower;
- motor[yThree] = holdPower;
- //Set arm to LOADED state
- armStatus = 0;
- }
- }
- }
- /*
- Maintains desired gate position using Quad
- Three States:
- -Closed (0)
- -Open (1)
- */
- task gatePosition(){
- while(true){
- int lastStatus = gateStatus;
- if(intakeRunning){
- motor[intake] = 127;
- }else{
- motor[intake] = 0;
- }
- if(SensorValue[ballSonic] < 70 && armStatus = 0){
- ballSeen = true;
- gateStatus = 0;
- }
- if(lastStatus != gateStatus){
- if(gateStatus == 1){
- while(SensorValue[gateQuad] > gateOpen){
- motor[gate] = 127;
- }
- motor[gate] = -127;
- wait1Msec(2);
- motor[gate] = 0;
- }
- if(gateStatus == 0){
- intakeRunning = false;
- while(SensorValue[gateQuad] < gateClosed){
- motor[gate] = -127;
- }
- motor[gate] = 127;
- wait1Msec(2);
- motor[gate] = 0;
- }
- }
- }
- }
- //Monitors how many balls are in the bot (will shut off intake after seeing 4 balls)
- task ballCounter(){
- }
- task autonomous(){
- }
- task usercontrol(){
- /*
- ***** MOVE THESE INTIALIZATIONS TO BEGINNING OF AUTONOMOUS WHEN READY *****
- */
- gateStatus = 0;
- armStatus = 0;
- SensorValue[armQuad] = 0;
- SensorValue[gateQuad] = 0;
- startTask(armPosition);
- startTask(gatePosition);
- while(true){
- //Firing Controls
- if(vexRT[Btn7U] == 1){
- distance = 2;
- armStatus = 1;
- }else{
- if(vexRT[Btn7L] == 1){
- distance = 1;
- armStatus = 1;
- }else{
- if(vexRT[Btn7D] == 1){
- distance = 0;
- armStatus = 1;
- }else{
- if(vexRT[Btn7R] == 1){
- gateStatus = 1;
- }
- }
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement