]> code.bitgloo.com Git - abelleisle/vex5106z.git/commitdiff
Added Pseudoish Code
authorfrydaddy07 <nmfunfun@gmail.com>
Sat, 14 Nov 2015 02:31:24 +0000 (21:31 -0500)
committerfrydaddy07 <nmfunfun@gmail.com>
Sat, 14 Nov 2015 02:31:24 +0000 (21:31 -0500)
src/opcontrol.c

index b138bc09f1247a47d8e0b7a52995fe3b02d42081..b455e210a6d890fb97723948e5c578e656f7ed87 100644 (file)
@@ -6,7 +6,7 @@ extern Gyro gyro;       // Originally defined in init.c
 \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
@@ -19,7 +19,7 @@ static double gyroRotation;
 \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
@@ -28,7 +28,7 @@ void operatorLCD(void *unused){
 \r
                /*\r
                 *      Display information according to the current 'window'.\r
-               */\r
+                */\r
 \r
                switch(ctty){\r
                default:\r
@@ -36,7 +36,7 @@ void operatorLCD(void *unused){
                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
@@ -76,7 +76,7 @@ void operatorPoll(){
 \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
@@ -140,21 +140,21 @@ void operatorControl(){
                // 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
@@ -162,14 +162,14 @@ void operatorControl(){
                        // 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
@@ -201,3 +201,51 @@ void operatorControl(){
                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