Last active
November 15, 2015 21:24
-
-
Save bradllj/9d1d2356ed82f9b4fb50 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #pragma config(Sensor, dgtl1, button1, sensorTouch) | |
| #pragma config(Motor, port1, rightback, tmotorVex393_HBridge, openLoop, reversed) | |
| #pragma config(Motor, port2, rightfront, tmotorVex393_MC29, openLoop) | |
| #pragma config(Motor, port4, arm, tmotorVex393_MC29, openLoop, reversed) | |
| #pragma config(Motor, port6, arm2, tmotorVex393_MC29, openLoop) | |
| #pragma config(Motor, port7, claw, tmotorVex393_MC29, openLoop) | |
| #pragma config(Motor, port9, leftfront, tmotorVex393_MC29, openLoop) | |
| #pragma config(Motor, port10, leftback, tmotorVex393_HBridge, openLoop) | |
| #pragma platform(VEX) | |
| //Competition Control and Duration Settings | |
| #pragma competitionControl(Competition) | |
| #pragma autonomousDuration(20) | |
| #pragma userControlDuration(120) | |
| #include "Vex_Competition_Includes.c" //Main competition background code...do not modify! | |
| ///////////////////////////////////////////////////////////////////////////////////////// | |
| // | |
| // Pre-Autonomous Functions | |
| // | |
| // You may want to perform some actions before the competition starts. Do them in the | |
| // following function. | |
| // | |
| ///////////////////////////////////////////////////////////////////////////////////////// | |
| void pre_auton() | |
| { | |
| // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between | |
| // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false. | |
| bStopTasksBetweenModes = true; | |
| // All activities that occur before the competition starts | |
| // Example: clearing encoders, setting servo positions, ... | |
| } | |
| void driveforward (int time) | |
| { | |
| motor[leftback]=80; | |
| motor[rightback]=80; | |
| motor[leftfront]=80; | |
| motor[rightfront]=80; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[rightback]=0; | |
| motor[leftfront]=0; | |
| motor[rightfront]=0; | |
| } | |
| void turnleft(int time) //function to turn left | |
| { | |
| motor[leftback]=-80; | |
| motor[leftfront]=-80; | |
| motor[rightback]=80; | |
| motor[rightfront]=80; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| } | |
| void swingright(int time) | |
| { | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=80; | |
| motor[rightfront]=80; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| } | |
| void swingleft(int time) | |
| { | |
| motor[leftback]=80; | |
| motor[leftfront]=80; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| } | |
| void turnright (int time) | |
| { | |
| motor[leftback]=80; | |
| motor[leftfront]=80; | |
| motor[rightback]=-80; | |
| motor[rightfront]=-80; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| } | |
| void drivebackwards (int time) | |
| { | |
| motor[leftback]=-80; | |
| motor[leftfront]=-80; | |
| motor[rightback]=-80; | |
| motor[rightfront]=-80; | |
| wait1Msec(time); | |
| motor[leftback]=0; | |
| motor[leftfront]=0; | |
| motor[rightback]=0; | |
| motor[rightfront]=0; | |
| } | |
| void clawopen (int time) | |
| { | |
| motor[claw]=80; | |
| wait1Msec(time); | |
| motor[claw]=0; | |
| } | |
| void clawclose (int time) | |
| { | |
| motor[claw]=-127; | |
| wait1Msec(time); | |
| motor[claw]=0; | |
| } | |
| void armup (int time) | |
| { | |
| motor[arm]=80; | |
| motor[arm2]=80; | |
| wait1Msec(time); | |
| motor[arm]=0; | |
| motor[arm2]=0; | |
| } | |
| void armdown (int time) | |
| { | |
| motor[arm]=-80; | |
| motor[arm2]=-80; | |
| wait1Msec(time); | |
| motor[arm]=0; | |
| motor[arm2]=0; | |
| } | |
| task autonomous() | |
| { | |
| if(SensorValue(button1)==1) | |
| { | |
| // turnright(200);//red | |
| driveforward (400); | |
| clawclose(700); | |
| armup (1600); | |
| drivebackwards(890); | |
| turnleft(400); | |
| driveforward(500); | |
| armdown(2200); | |
| wait1Msec(500); | |
| clawopen (1000); | |
| drivebackwards(1000); | |
| } | |
| else | |
| { | |
| swingleft(200);//blue | |
| clawclose(700); | |
| armup (1600); | |
| drivebackwards(800); | |
| turnright(550); | |
| driveforward(500); | |
| armdown(2200); | |
| wait1Msec(500); | |
| clawopen (1000); | |
| drivebackwards(1000); | |
| } | |
| } | |
| ///////////////////////////////////////////////////////////////////////////////////////// | |
| // | |
| // User Control Task | |
| // | |
| // This task is used to control your robot during the user control phase of a VEX Competition. | |
| // You must modify the code to add your own robot specific commands here. | |
| // | |
| ///////////////////////////////////////////////////////////////////////////////////////// | |
| task usercontrol() | |
| { | |
| while (true) | |
| { | |
| motor(leftfront)=vexRT[Ch3]; | |
| motor(leftback)=vexRT[Ch3]; | |
| motor(rightfront)=vexRT[Ch2]; | |
| motor(rightback)=vexRT[Ch2]; | |
| motor(arm)=vexRT[Ch2Xmtr2]; | |
| motor(arm2)=vexRT[Ch2Xmtr2]; | |
| if(vexRT[Btn5UXmtr2]==1) // button 6 up opens claw | |
| { | |
| motor[claw]= 80; | |
| } | |
| else if(vexRT[Btn5DXmtr2]==1) // button 6 down closes claw | |
| { | |
| motor[claw]=-80; | |
| } | |
| else | |
| { | |
| motor[claw]=0; | |
| } | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment