\r
/*\r
* These keep track of the current LCD 'window' and the number of 'windows' that exist.\r
-*/\r
+ */\r
\r
static signed int ctty=0;\r
const unsigned int mtty=4;\r
\r
/*\r
* This runs as a separate task to update the LCD.\r
-*/\r
+ */\r
\r
void operatorLCD(void *unused){\r
static int ime;\r
\r
/*\r
* Display information according to the current 'window'.\r
- */\r
+ */\r
\r
switch(ctty){\r
default:\r
case 0: // Welcome screen\r
\r
lcdSetText(uart1,1," JOHN CENA ");\r
- lcdPrint(uart1,2," ===========%3d",motorGet(CANNON1));\r
+ lcdPrint(uart1,2," ===========%3d",motorGet(CANNON1));\r
break;\r
\r
case 1: // Battery levels in volts\r
\r
/*\r
* Read in rotations of motors and the current angle of the robot.\r
- */\r
+ */\r
\r
imeGet(DRIVEL,&idl); // Get reading\r
imeDriveLeft = idl / 627.0L; // Convert to wheel rotations\r
// Set the intake's speed.\r
\r
motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP )? 127 :\r
- joystickGetDigital(1,6,JOY_DOWN)? -127 :\r
- 0 );\r
+ joystickGetDigital(1,6,JOY_DOWN)? -127 :\r
+ 0 );\r
\r
// Set the lift's speed.\r
\r
lift=joystickGetDigital(2,6,JOY_UP )? 127 :\r
- joystickGetDigital(2,6,JOY_DOWN)? -127 :\r
- 0 ;\r
+ joystickGetDigital(2,6,JOY_DOWN)? -127 :\r
+ 0 ;\r
\r
motorSet(LIFT1,lift);\r
motorSet(LIFT2,lift);\r
\r
/*\r
* Miscellaneous operation handlers.\r
- */\r
+ */\r
\r
if(++uiinc==20){ // Equates to every 200ms\r
uiinc=0;\r
// Goto next 'window'.\r
\r
if(joystickGetDigital(1,7,JOY_UP) ||\r
- joystickGetDigital(2,7,JOY_UP) ){\r
+ joystickGetDigital(2,7,JOY_UP) ){\r
if(++ctty==mtty)ctty=0;\r
}\r
\r
// Goto previous 'window'.\r
\r
if(joystickGetDigital(1,7,JOY_DOWN) ||\r
- joystickGetDigital(2,7,JOY_DOWN) ){\r
+ joystickGetDigital(2,7,JOY_DOWN) ){\r
if(--ctty==-1)ctty=mtty-1;\r
}\r
\r
delay(10); // Short delay to allow task switching.\r
}\r
}\r
+//Totally Theoretical PseudoCode for AutoShoot\r
+//Based on IME data:\r
+double lVel;//=velocity of left side\r
+double rVel;//=velocity of right side\r
+double cVel;//=velocity of center (calculated below)\r
+double aVel;//=angular velocity (calculated below)\r
+\r
+//Used to find rectangular vectors * double aVel=angular velocity of robot\r
+//Used to store the rectangular vectors:\r
+double xVel1;\r
+double xVel2;\r
+double yVel1;\r
+double yVel2;\r
+\r
+//Final Position Vectors\r
+double xPos;\r
+double yPos;\r
+\r
+//Vector Assignments:\r
+void Vectors(){\r
+while(1){//something in the brackets\r
+ int i;\r
+ i++;\r
+ cVel=(lVel+rVel)/2;\r
+ if(lVel>rVel){\r
+ aVel=cVel/(16*lVel/(rVel-lVel));\r
+ }\r
+ if(rVel>lVel){\r
+ aVel=cVel/(16*rVel/(lVel-rVel));\r
+ }\r
+ else{\r
+ aVel=0;\r
+ if(i%2==0){\r
+ xVel1=cos(aVel)*cVel;\r
+ yVel1=sin(aVel)*cVel;\r
+ }\r
+ else{\r
+ xVel2=cos(aVel)*cVel;\r
+ yVel2=sin(aVel)*cVel;\r
+ }\r
+\r
+ xPos+=((xVel1+xVel2)/2);//*time elapsed between cycles\r
+ yPos+=((yVel1+yVel2)/2);//*time elapsed between cycles\r
+ delay(20);\r
+ }\r
+}\r
+}\r
+\r