everything

diff --git a/include/main.h b/include/main.h
index ebe72a65a14ed08cceb46ff1f42c494f2db653e8..6d369738aad189fee4f2ee74abf43c35fc240e58 100644 (file)
--- a/include/main.h
+++ b/include/main.h
@@ -11,6 +11,34 @@
 extern "C" {\r
 #endif\r
 \r
+/*\r
+ *     Aliases for all the motors, stored in an enum for convenience.\r
+*/\r
+\r
+enum MOTOR_PORT_MAP {\r
+       UNUSED = 0,\r
+       CANNON1,\r
+       CANNON2,\r
+       CANNON3,\r
+       CANNON4,\r
+       INTAKE,\r
+       DRIVER,\r
+       DRIVEL,\r
+       LIFT1,\r
+       LIFT2,\r
+       ROTATER,\r
+};\r
+\r
+enum IME_PORT_MAP {\r
+       IDRIVER,\r
+       IDRIVEL,\r
+       IROTATER,\r
+       ILIFT1,\r
+       ILIFT2,\r
+       ICANNON3\r
+};\r
+\r
+\r
 void autonomous();\r
 void initializeIO();\r
 void initialize();\r

diff --git a/src/auto.c b/src/auto.c
index 50fda48ad72e8dd1125b0bd92bd242f391a65a22..33204f9be4ee0cc164c9df100102e84e0cfa98e4 100644 (file)
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,4 +1,46 @@
 #include <main.h>\r
 \r
+#define TARGET_RPM 1970\r
+\r
+static unsigned int time;\r
+\r
+void autoDelay(unsigned int ms){\r
+       delay(ms);\r
+       time-=ms;\r
+}\r
+\r
 void autonomous(){\r
+       static int    trpm;\r
+       static double rpm;\r
+       static char cs;\r
+\r
+       time = 15000;\r
+       cs = rpm = trpm = 0;\r
+\r
+       do{\r
+\r
+               cs+=cs<100?10:0;\r
+\r
+               motorSet(CANNON1,cs);\r
+               motorSet(CANNON2,cs);\r
+               motorSet(CANNON3,cs);\r
+               motorSet(CANNON4,cs);\r
+\r
+               autoDelay(800);\r
+\r
+               imeGetVelocity(ICANNON3,&trpm);\r
+               rpm=abs(trpm)/24.5L;\r
+               rpm*=25.0L;\r
+\r
+       }while(rpm<TARGET_RPM);\r
+\r
+       motorSet(LIFT1,127);\r
+       motorSet(LIFT2,127);\r
+\r
+       delay(time);\r
+\r
+       // Stop                                                                         4000ms remaining\r
+\r
+       motorStopAll();\r
+\r
 }\r

diff --git a/src/init.c b/src/init.c
index e2145b4dfa02cae72914a3fc148cb8b853b76cd0..3ffa429836a418795151195c0603af8703123772 100644 (file)
--- a/src/init.c
+++ b/src/init.c
@@ -4,13 +4,38 @@ Gyro gyro;
 unsigned int imeCount;\r
 \r
 void initializeIO(){\r
-       pinMode(ANALOG_PORT(1),INPUT_ANALOG);\r
+       pinMode(ANALOG_PORT(1),INPUT_ANALOG);   // Power expander status port\r
+       pinMode(1,OUTPUT);                                              // LED\r
 }\r
 \r
 void initialize(){\r
-       imeCount = imeInitializeAll();\r
-       gyro=gyroInit(2,0);\r
+\r
+       //      Initialize the LCDs.\r
+\r
        lcdInit(uart1);\r
        lcdClear(uart1);\r
        lcdSetBacklight(uart1,1);\r
+\r
+       lcdInit(uart2);\r
+       lcdClear(uart2);\r
+       lcdSetBacklight(uart2,1);\r
+\r
+       //      Setup sensors.\r
+\r
+       imeCount = imeInitializeAll();\r
+       gyro=gyroInit(2,0);\r
+\r
+       lcdPrint(uart1,1,"%u IMEs :)",imeCount);\r
+\r
+       lcdPrint(uart2,1,"ERR: Dark theme ");\r
+       lcdPrint(uart2,2,"     not found! ");\r
+\r
+       delay(2000);\r
+\r
+       lcdPrint(uart1,1,"ready...");\r
+       lcdPrint(uart2,1,"...yeah.        ");\r
+       lcdPrint(uart2,2,"                ");\r
+\r
+       delay(1000);\r
+\r
 }\r

diff --git a/src/opcontrol.c b/src/opcontrol.c
index bd3118a8f1299ff794f90aa2c5783f2a6f1fbd0c..b138bc09f1247a47d8e0b7a52995fe3b02d42081 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,102 +1,203 @@
 #include <main.h>\r
 #include <stdint.h>\r
-#include <string.h>\r
-\r
-enum MOTOR_PORT_MAP {\r
-       UNUSED = 0,\r
-       INTAKE1,\r
-       DRIVEL ,\r
-       DRIVER ,\r
-       LIFT1  ,\r
-       LIFT2  ,\r
-       UNUSED6,\r
-       INTAKE2,\r
-       INTAKE3,\r
-       INTAKE4,\r
-       ROTATER,\r
-};\r
-\r
-extern Gyro gyro;\r
+#include <math.h>\r
+\r
+extern Gyro gyro;      // Originally defined in init.c\r
+\r
+/*\r
+ *     These keep track of the current LCD 'window' and the number of 'windows' that exist.\r
+*/\r
 \r
 static   signed int ctty=0;\r
-const  unsigned int mtty=3;\r
+const  unsigned int mtty=4;\r
+\r
+static double imeDriveLeft;\r
+static double imeDriveRight;\r
+static double imeRotater;\r
+\r
+static double gyroRotation;\r
+\r
+/*\r
+ *     This runs as a separate task to update the LCD.\r
+*/\r
 \r
 void operatorLCD(void *unused){\r
+       static int ime;\r
+\r
        while(1){\r
+\r
+               /*\r
+                *      Display information according to the current 'window'.\r
+               */\r
+\r
                switch(ctty){\r
                default:\r
-               case 0:\r
+\r
+               case 0: //      Welcome screen\r
+\r
                        lcdSetText(uart1,1,"   JOHN  CENA   ");\r
-                       lcdSetText(uart1,2,"  ============  ");\r
+                         lcdPrint(uart1,2,"  ===========%3d",motorGet(CANNON1));\r
                        break;\r
-               case 1:\r
+\r
+               case 1: //      Battery levels in volts\r
+\r
                        lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f));\r
-                       lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1)   / 210.0f));\r
+                       lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1)   / 280.0f));\r
+                       break;\r
+\r
+               case 2: //      Gyroscope readings\r
+\r
+                       imeGet(IROTATER,&ime);\r
+\r
+                       double rot = ime / 3.0L / 627.2L * 10;\r
+\r
+                       rot = floor(rot) / 10;\r
+\r
+                       lcdPrint(uart1,1,"Gyro: %d",gyroGet(gyro));\r
+                       lcdPrint(uart1,2,"Rot.: %.1lf",rot);\r
+\r
+                       break;\r
+\r
+               case 3:\r
+\r
+                       lcdPrint(uart1,1,"%.3lf",imeDriveLeft);\r
+                       lcdPrint(uart1,2,"%.3lf",imeDriveRight);\r
+\r
                        break;\r
-               case 2:\r
-                       lcdPrint(uart1,1,"Gyro: %u",gyroGet(gyro));\r
                }\r
-               delay(2000);\r
+\r
+               delay(500);\r
+       }\r
+}\r
+\r
+void operatorPoll(){\r
+       static int idl,idr,ir,gr;\r
+       while(1){\r
+\r
+               /*\r
+                *      Read in rotations of motors and the current angle of the robot.\r
+               */\r
+\r
+               imeGet(DRIVEL,&idl);                    // Get reading\r
+               imeDriveLeft  = idl / 627.0L;   // Convert to wheel rotations\r
+               imeDriveLeft *=  8.64L;                 // Convert to distance in inches\r
+\r
+               imeGet(DRIVER,&idr);                    // Get reading\r
+               imeDriveRight  = idr / 627.0L;  // Convert to wheel rotations\r
+               imeDriveRight *=  8.64L;                // Convert to distance in inches\r
+\r
+               imeGet(ROTATER,&ir);\r
+               imeRotater = ir / 627.0L;\r
+\r
+               gyroRotation = gyroGet(gyro);\r
+\r
+               delay(100);\r
        }\r
 }\r
 \r
 void operatorControl(){\r
 \r
-#define getJoy(n) joy[n-1]\r
+       static bool selfAim = false;\r
 \r
-       static int8_t joy[8],\r
-                                 lift   = 0,\r
-                                 rotate = 0,\r
-                                 intake = 0,\r
-                                 uiinc = 0;\r
+       static char uiinc = 0;  // See below\r
 \r
-       // Start the LCD task\r
-       taskCreate(operatorLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+       static char in,lift;    // Used to set the multiple cannon motors to the same joy-stick reading.\r
+\r
+       taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+       taskCreate(operatorPoll,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
 \r
        while(1){\r
 \r
-               joy[0]=joystickGetAnalog(1,1);\r
-               joy[1]=joystickGetAnalog(1,2);\r
-               joy[2]=joystickGetAnalog(1,3);\r
-               joy[3]=joystickGetAnalog(1,4);\r
-               joy[4]=joystickGetAnalog(2,1);\r
-               joy[5]=joystickGetAnalog(2,2);\r
-               joy[6]=joystickGetAnalog(2,3);\r
-               joy[7]=joystickGetAnalog(2,4);\r
-\r
-               intake = getJoy(7);\r
-               rotate =-getJoy(5);\r
-               lift   = joystickGetDigital(2,6,JOY_UP  ) ?  127 :\r
-                                joystickGetDigital(2,6,JOY_DOWN) ? -127 :\r
-                                0;\r
-\r
-               motorSet(INTAKE1,intake);\r
-               motorSet(INTAKE2,intake);\r
-               motorSet(INTAKE3,intake);\r
-               motorSet(INTAKE4,intake);\r
-               motorSet(ROTATER,rotate);\r
+               //      Set the drive motors speeds.\r
+\r
+               motorSet(DRIVEL,joystickGetAnalog(1,3));\r
+               motorSet(DRIVER,joystickGetAnalog(1,2));\r
+\r
+               //      Set the rotating motor speed.\r
+\r
+               if(!selfAim){\r
+\r
+                       motorSet(ROTATER,-joystickGetAnalog(2,1)/2);\r
+\r
+               }else{\r
+\r
+                       //static int gc=0,gl=0;\r
+\r
+                       //gl = gc;\r
+                       //gc = gyroGet(gyro);\r
+\r
+               }\r
+\r
+               //      Set the cannon's speed.\r
+\r
+               in=joystickGetAnalog(2,3);\r
+\r
+               motorSet(CANNON1,in);\r
+               motorSet(CANNON2,in);\r
+               motorSet(CANNON3,in);\r
+               motorSet(CANNON4,in);\r
+\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
+\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
+\r
                motorSet(LIFT1,lift);\r
                motorSet(LIFT2,lift);\r
 \r
-               motorSet(DRIVEL,-getJoy(2));\r
-               motorSet(DRIVER, getJoy(3));\r
-\r
-               /*motorSet(PULLER,joystickGetDigital(2,6,JOY_UP  ) ?  127 :\r
-                                               joystickGetDigital(2,6,JOY_DOWN) ? -127 :\r
-                                               0);*/\r
+               /*\r
+                *      Miscellaneous operation handlers.\r
+               */\r
 \r
-               if(++uiinc==20){\r
+               if(++uiinc==20){        // Equates to every 200ms\r
                        uiinc=0;\r
+\r
+                       // Goto next 'window'.\r
+\r
                        if(joystickGetDigital(1,7,JOY_UP) ||\r
                           joystickGetDigital(2,7,JOY_UP) ){\r
                                if(++ctty==mtty)ctty=0;\r
                        }\r
-          else if(joystickGetDigital(1,7,JOY_DOWN) ||\r
+\r
+                       // Goto previous 'window'.\r
+\r
+                       if(joystickGetDigital(1,7,JOY_DOWN) ||\r
                           joystickGetDigital(2,7,JOY_DOWN) ){\r
                                if(--ctty==-1)ctty=mtty-1;\r
                        }\r
+\r
+                       // Run autonomous (for testing wo/ competition switch).\r
+\r
+                       if(joystickGetDigital(1,7,JOY_RIGHT))\r
+                               autonomous();\r
+\r
+                       // Goto test area.\r
+\r
+                       if(joystickGetDigital(1,7,JOY_LEFT)){\r
+                               static double target = 0;\r
+\r
+                               if(!target) target = (imeDriveLeft+imeDriveRight) / 2 + 13;\r
+\r
+                               motorSet(DRIVEL,60);\r
+                               motorSet(DRIVER,60);\r
+\r
+                               while((imeDriveLeft+imeDriveRight) / 2 < target)\r
+                                       delay(10);\r
+\r
+                               motorSet(DRIVEL,0);\r
+                               motorSet(DRIVER,0);\r
+\r
+                       }\r
+\r
                }\r
 \r
-               delay(10);\r
+               delay(10);      // Short delay to allow task switching.\r
        }\r
 }\r