autonomizing robot

diff --git a/include/zephyr.h b/include/zephyr.h
index 1ba234c801dd58c90942764dc5f805274dfead38..cdd2910898d2d68a008d54c776f388461ff6f1f1 100644 (file)
--- a/include/zephyr.h
+++ b/include/zephyr.h
@@ -16,17 +16,6 @@
 #define LCD_RATE       500
 
 #ifdef LCD_PORT
-
-void zLCDInit(void);
-void zLCDStart(void);
-
-void zLCDWrite(unsigned char,  // 1-based Line Number
-                          const char *,        // Text
-                          ...);
-
-void zLCDSetUpdateFunc(void (*)(void *));      // Function Pointer
-void zLCDClearUpdateFunc(void);
-
 #endif // LCD_PORT
 
 /*
@@ -38,6 +27,8 @@ void zLCDClearUpdateFunc(void);
 #ifdef GYRO_PORT
 
 void zGyroInit(void);
+int  zGyroGet(void);
+void zGyroReset(void);
 
 #endif // GYRO_PORT
 
@@ -69,14 +60,19 @@ void zIMEInit(void);
 #define zJoyAnalog(j,a)                joystickGetAnalog(j,a)
 
 void zMotorSet(const char *,   // Motor Name
-                          char                         // Desired Speed
+                          int,                         // Desired Speed
+                          unsigned int         // Caller ID
                           );
 char zMotorGet(const char *);  // Motor Name
 
+void zMotorTake(const char *,unsigned int);
+void zMotorReturn(const char *);
+
 #ifdef IME_ENABLE
 
 int zMotorIMEGet(const char *);                        // Motor Name
 int zMotorIMEGetVelocity(const char *);        // Motor Name
+bool zMotorIMEReset(const char *motor);
 
 #endif // IME_ENABLE
 

diff --git a/src/auto.c b/src/auto.c
index 23324284dd2e0b8b3de18b522351ac6ff0374c29..a5bbb5da5fc54e89912e056bec37fa60577d2783 100644 (file)
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,41 +1,28 @@
 #include <main.h>\r
 \r
-/*#define TARGET_SPEED 65\r
-\r
-static unsigned int time;\r
-\r
-void autoDelay(unsigned int ms){\r
-       delay(ms);\r
-       time-=ms;\r
-}*/\r
+#define TARGET_RPM 1700\r
 \r
 void autonomous(){\r
-       /*static char speed = 0;\r
+       /*static double cl,cr,ca;\r
+       static char speed;\r
 \r
-       time = 15000;\r
+       speed = 30;\r
 \r
        do{\r
-\r
-               speed+=10;\r
-\r
-               motorSet(CANNON1,speed);\r
-               motorSet(CANNON2,speed);\r
-               motorSet(CANNON3,speed);\r
-               motorSet(CANNON4,speed);\r
-\r
-               autoDelay(200);\r
-\r
-       }while(speed<TARGET_SPEED);\r
-\r
-       autoDelay(800);\r
-\r
-       motorSet(LIFT1 ,127);\r
-       motorSet(LIFT2 ,127);\r
-       motorSet(INTAKE,127);\r
-\r
-       //while(1);\r
-       delay(time);\r
+               speed += 10;\r
+               zMotorSet("Left cannon",-speed);\r
+               zMotorSet("Right cannon",speed);\r
+               delay(800);\r
+               cl =  zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;\r
+               cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+               ca = (cl + cr) / 2;\r
+       }while(ca < TARGET_RPM);\r
+\r
+       zMotorSet("Misc",127);\r
+       delay(900);\r
+       zMotorSet("Misc",-127);\r
+       delay(900);\r
+       zMotorSet("Misc",0);\r
 \r
        motorStopAll();*/\r
-\r
 }\r

diff --git a/src/init.c b/src/init.c
index 3c1a7d2dc6f68f887af150f64215984a90a61977..1d54cce41c5dd4dcd64e1c17a643fd263b87abeb 100644 (file)
--- a/src/init.c
+++ b/src/init.c
@@ -5,7 +5,12 @@ void initializeIO(){
 \r
 void initialize(){\r
 \r
-       zLCDInit();\r
+       pinMode(20,INPUT_ANALOG);\r
+\r
+       lcdInit(LCD_PORT);\r
+       lcdClear(LCD_PORT);\r
+       lcdSetBacklight(LCD_PORT,true);\r
+\r
        zGyroInit();\r
        zIMEInit();\r
 \r

diff --git a/src/opcontrol.c b/src/opcontrol.c
index 9eefe951195b61f283ecb2894736ac5266104064..93edc1390e0e93b1ef1f8196eb978ce248e7f2f9 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,91 +1,536 @@
 #include <main.h>\r
+#include <stdint.h>\r
 #include <math.h>\r
 \r
+#define GOAL_DISTANCE 120\r
+\r
+/*\r
+ * RTTTL songs for speaker usage.\r
+ */\r
+\r
 const char *song  = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5";\r
 const char *xpst  = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5";\r
 const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5";\r
-const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5.,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\\r
+const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5,4p,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\\r
 4a#5,8g5,8f5,4d#5";\r
-static unsigned char cyc = 0;\r
+\r
+/*\r
+ * Contains how many milliseconds it took to enter operatorControl().\r
+ */\r
+\r
+static unsigned long opmillis = 0;\r
+\r
+/*\r
+ * Contains a light sensor value calculated at init time for object detection.\r
+ */\r
+\r
+static int lightThresh = 0;\r
+\r
+static char cann = 0;\r
+static double rpm  = 0,trpm = 1750, arpm = 0;\r
+\r
+static double xpos=0,ypos=0;\r
+\r
+static bool cannonProcRun = false,\r
+                       armProcRun = false,\r
+                       aimProcRun = false;\r
+\r
+TaskHandle taskLCD,\r
+                  taskCannon,\r
+                  taskArm,\r
+                  taskMove,\r
+                  taskAim;\r
 \r
 void lcdUpdateFunc(void *);\r
+void cannonProc(void *);\r
+void armProc(void *);\r
+void moveProc(void *);\r
+void aimProc(void *);\r
+\r
+/**\r
+ * Cause the Cortex to reset, which results in the Cortex restarting the\r
+ * operator control code.\r
+ *\r
+ * This reset is accomplished through setting two bits, SYSRESETREQ and\r
+ * VECTRESET, in the Application Interrupt and Reset Control Register (AIRCR),\r
+ * which is at a known location in memory. SYSRESETREQ will actually request\r
+ * the system reset, while VECTRESET is 'reserved for debugging'. This second\r
+ * bit may not need to be set; it's only set here because others have found it\r
+ * necessary.\r
+ */\r
+\r
+#define AIRCR_ADDR             0xE000ED0C\r
+#define VECTKEY                        0x05FA\r
+#define SYSRESETREQ            (1<<2)\r
+#define VECTRESET              (1<<0)\r
+\r
+void softwareReset(void){\r
+\r
+       /*\r
+        * Read the current value of the AIRCR, since some flags currently set in\r
+        * the register need to remain the same in order for the reset to work.\r
+        */\r
+\r
+       uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR);\r
+\r
+       /*\r
+        * Here we save the lower 16 bits of the register, write a special key to\r
+        * the higher 16 bits that'll allow the write to occur, and then set the\r
+        * reset flags.\r
+        */\r
+\r
+       AIRCR = (AIRCR & 0xFFFF) | (VECT_KEY << 16) | SYSRESETREQ | VECTRESET;\r
+\r
+       // Update the AIRCR.\r
+\r
+       *((uint32_t *)0xE000ED0C) = AIRCR;\r
+\r
+       /*\r
+        * This instruction causes the program to wait until the previous memory\r
+        * write is complete, ensuring it is taken into effect and the reset\r
+        * request is made.\r
+        */\r
+\r
+       asm("DSB");\r
+\r
+       // Wait until the system reset completes.\r
+\r
+       while(1);\r
+}\r
+\r
+/******************************************************************************\r
+ * OPERATOR CONTROL                                                           *\r
+ ******************************************************************************/\r
 \r
 void operatorControl(){\r
+       static unsigned char ui_inc = 0;\r
+       static unsigned char cyc = 0;\r
+       static char lift;\r
 \r
-       static char lift,cann;\r
+       opmillis = millis();\r
+       lightThresh = analogRead(8) - 60;\r
 \r
-       zLCDStart();\r
-       zLCDSetUpdateFunc(lcdUpdateFunc);\r
+       taskLCD=taskCreate(lcdUpdateFunc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+       taskMove=taskCreate(moveProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT+1);\r
 \r
        while(1){\r
 \r
                /*\r
                 *      Handle drive controls and update drive motor speeds.\r
-               */\r
+                */\r
 \r
                zDriveUpdate();\r
 \r
                //      Set the rotating motor speed.\r
 \r
-               zMotorSet("Rotater",-zJoyAnalog(2,1)/4);\r
+               if(!aimProcRun)\r
+                       zMotorSet("Rotater",-zJoyAnalog(1,1) / 4,0);\r
 \r
                // Set the intake's speed.\r
 \r
-               zMotorSet("Intake",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));\r
+               zMotorSet("Intake",\r
+                                 zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127),\r
+                                 0\r
+                                 );\r
 \r
                // Set the lift's speed.\r
 \r
-               lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127);\r
+               if(!armProcRun){\r
+                       lift = zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127);\r
+                       zMotorSet("Lift 1",lift,0);\r
+                       zMotorSet("Lift 2",lift,0);\r
+               }\r
 \r
-               zMotorSet("Lift 1",lift);\r
-               zMotorSet("Lift 2",lift);\r
+               // Set the cannon's speed.\r
 \r
-               cann=zJoyAnalog(2,3);//zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127);\r
+               if(!cannonProcRun){\r
+                       zMotorSet("Left cannon" ,-cann,0);\r
+                       zMotorSet("Right cannon", cann,0);\r
+               }\r
 \r
-               zMotorSet("Left cannon" ,-cann);\r
-               zMotorSet("Right cannon", cann);\r
+               if(++ui_inc == 50){\r
+                       ui_inc = 0;\r
 \r
-               zMotorSet("Misc",zGetDigitalMotorSpeed(2,7,JOY_UP,JOY_DOWN,127));\r
+                       if(zJoyDigital(1,5,JOY_UP) && zJoyDigital(1,5,JOY_DOWN))\r
+                               cpu_reset();\r
 \r
-               if(zJoyDigital(1,7,JOY_LEFT)){\r
-                       speakerInit();\r
-                       switch(cyc){\r
-                       case 0:speakerPlayRtttl(song );break;\r
-                       case 1:speakerPlayRtttl(xpst );break;\r
-                       case 2:speakerPlayRtttl(xpst2);break;\r
-                       case 3:speakerPlayRtttl(bound);break;\r
+                       if(zJoyDigital(1,7,JOY_LEFT)){\r
+                               speakerInit();\r
+                               switch(cyc){\r
+                               case 0:speakerPlayRtttl(song );break;\r
+                               case 1:speakerPlayRtttl(xpst );break;\r
+                               case 2:speakerPlayRtttl(xpst2);break;\r
+                               case 3:speakerPlayRtttl(bound);break;\r
+                               }\r
+                               if(++cyc == 4) cyc = 0;\r
+                               speakerShutdown();\r
+                       }else if(zJoyDigital(1,7,JOY_RIGHT)){\r
+                               zGyroReset();\r
+                               zMotorIMEReset("Rotater");\r
+                       }else if(zJoyDigital(1,7,JOY_UP))\r
+                               taskAim = taskCreate(aimProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+                       else if(zJoyDigital(1,7,JOY_DOWN))\r
+                               aimProcRun = false;\r
+\r
+                       if(zJoyDigital(1,8,JOY_UP)){\r
+                               if(cannonProcRun) arpm += 50;\r
+                               else if(cann < 120)cann += 10;\r
+                       }else if(zJoyDigital(1,8,JOY_DOWN)){\r
+                               if(cannonProcRun) arpm -= 50;\r
+                               else if(cann > -120)cann -= 10;\r
+                       }else if(zJoyDigital(1,8,JOY_LEFT)){\r
+                               if(!cannonProcRun)\r
+                                       taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+                               else\r
+                                       cannonProcRun = false;\r
                        }\r
-                       if(++cyc == 4) cyc = 0;\r
-                       speakerShutdown();\r
+\r
+                       if(zJoyDigital(1,8,JOY_RIGHT))\r
+                               taskArm = taskCreate(armProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
                }\r
 \r
                delay(10);      // Short delay to allow task switching\r
        }\r
 }\r
 \r
-void lcdUpdateFunc(void *unused_param){\r
-       static double l,r,dist,heading;\r
+/**\r
+ * The Position-Tracker process.\r
+ *\r
+ * This process tries to track the position of the robot, using a combination\r
+ * of motor encoders and the gyroscope. This process is created directly, and\r
+ * is expected to run at a higher priority than other tasks.\r
+ */\r
+\r
+void moveProc(void *unused_param){\r
+       static double l,r,lv,rv;\r
+       static double dist,head;\r
+       static double dpos;\r
+       while(1){\r
+\r
+               l =  zMotorIMEGet("Left drive")  / 627.2L;\r
+               r = -zMotorIMEGet("Right drive") / 627.2L;\r
+\r
+               // random # -> motor RPM -> wheel RPM (inches per minute) -> inches per millisecond\r
+\r
+               lv =  zMotorIMEGetVelocity("Left drive")  / 39.2L * 8.64L / 60000;\r
+               rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 8.64L / 60000;\r
+\r
+               // total distance traveled since init\r
+               dist = (l - r) * 8.64L;\r
+\r
+               head = fmod(dist / 15,360.0L) / 2 * 90;\r
+               head = /*(head +*/ zGyroGet()/*) / 2*/;\r
+\r
+               dpos = (lv+rv) / 2 * 20;\r
+               dpos *= 1000;\r
+               dpos = floor(dpos) / 1000;\r
+\r
+               ypos += sin(head * PI / 180) * dpos;\r
+               xpos += cos(head * PI / 180) * dpos;\r
+\r
+               //lcdPrint(uart1,1,"%.3lf,%.3lf",xpos,ypos);\r
+\r
+               delay(20);\r
+       }\r
+}\r
+\r
+/**\r
+ * The Auto-Aim process.\r
+ *\r
+ * This function will attempt to keep the cannon aimed at a constant angle,\r
+ * adjusting its position when the robot's orientation changes by using the\r
+ * gyroscope and the encoder build into the rotater motor.\r
+ */\r
+\r
+void aimProc(void *procPtr){\r
+       static double cangle,   // Current angle (of rotater)\r
+                                 rangle,       // 'Robot' angle (target angle\r
+                                 angle;        // Angle between x-axis and line from robot to goal\r
+       static double dist;             // Distance of robot from goal\r
+       double xpos2,goal2;\r
+\r
+       aimProcRun = true;\r
+\r
+       /*\r
+        * Claim necessary motors.\r
+        */\r
+\r
+       zMotorTake("Rotater",1);\r
+\r
+       /*\r
+        * Run until a stop is requested.\r
+        */\r
+\r
+       while(aimProcRun){\r
+\r
+               /*\r
+                * Read orientation sensors.\r
+                */\r
+\r
+               cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);\r
+               rangle = zGyroGet();\r
+\r
+               xpos2 = pow(xpos,2);\r
+               goal2 = pow(GOAL_DISTANCE - ypos,2);\r
+               dist = sqrt(xpos2 + goal2);\r
+               angle = acos((xpos2 + pow(dist,2) - goal2) / (2 * xpos * dist));\r
+               rangle = (angle - PI / 2) * 180 / PI;\r
+\r
+               lcdPrint(uart1,2,"%lf",angle);\r
+\r
+               /*\r
+                * Adjust aim if necessary.\r
+                */\r
+\r
+               if(cangle > rangle + 3)\r
+                       zMotorSet("Rotater",30,1);\r
+               else if(cangle < rangle - 3)\r
+                       zMotorSet("Rotater",-30,1);\r
+               else\r
+                       zMotorSet("Rotater",0,1);\r
+\r
+               delay(100);\r
+       }\r
+\r
+       /*\r
+        * Free motors.\r
+        */\r
+\r
+       zMotorReturn("Rotater");\r
+}\r
+\r
+/**\r
+ * The RPM-Targeter process.\r
+ *\r
+ * This will attempt to keep the cannon motors running at a constant speed,\r
+ * determined through a target RPM set by the user.\r
+ */\r
+\r
+void cannonProc(void *procPtr){\r
        static double cl,cr,ca;\r
+       static int speed;\r
+\r
+       cannonProcRun = true;\r
 \r
        /*\r
-        *      Positioning code.\r
-       */\r
+        *      Initialize variables.\r
+        *      These need to be static so that their values are preserved through\r
+        *      task switchs but will retain their values when the function is\r
+        *      re-called, so here we reset them.\r
+        */\r
 \r
-       l =  zMotorIMEGet("Left drive")  / 627.2L;\r
-       r = -zMotorIMEGet("Right drive") / 627.2L;\r
+       speed = 20;\r
+       rpm = cl = cr = ca = 0;\r
 \r
-       dist=(l - r) * 8.64L;\r
-       heading = fmod(round(dist / 15),360.0L);\r
+       /*\r
+        * Reserve needed motors.\r
+        */\r
+\r
+       zMotorTake("Left cannon",2);\r
+       zMotorTake("Right cannon",2);\r
+\r
+       /*\r
+        *      Here we increase the power provided to the motors until our target\r
+        *      RPM is reached.\r
+        */\r
+\r
+       do{\r
+\r
+               /*\r
+                *      Bring up the speed, --exiting if we go too high.-- (crossed out)\r
+                *      The only reasonable explanation for an error such as the speed\r
+                *      getting too high is a hardware fault in the robot (bad motor,\r
+                *      bad IME, ...).\r
+                */\r
+\r
+               speed += 5;\r
+               if(speed > 120)\r
+                       speed = 127;\r
+\r
+               /*\r
+                *      Set the power levels, and allow the motors to adjust.\r
+                */\r
+\r
+               zMotorSet("Left cannon" ,-speed,2);\r
+               zMotorSet("Right cannon", speed,2);\r
+\r
+               delay(400);\r
+\r
+               /*\r
+                *      Calculate the average RPM, and continue if our target is met.\r
+                */\r
+\r
+               cl =  zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;\r
+               cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+               ca = /*(cl +*/ cr/*) / 2*/;\r
+               rpm = ca;\r
+\r
+               lcdPrint(uart1,2,"%.3lf/%.3lf",cl,cr);\r
+\r
+       }while(cannonProcRun && ca < trpm);\r
+\r
+       if(!cannonProcRun)\r
+               return;\r
+\r
+       /*\r
+        *      Once we reach our target, we 'idle' at the sped and adjust as\r
+        *      necessary. `cannInUse` will be cleared by any user attempt to use the\r
+        *      motor.\r
+        */\r
+\r
+       while(cannonProcRun){\r
+\r
+               /*\r
+                *      Update RPM values.\r
+                */\r
+\r
+               cl =  zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;\r
+               cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+               ca = /*(cl +*/ cr/*) / 2*/;\r
+               rpm = ca;\r
+\r
+               /*\r
+                *      Guess an RPM.\r
+                */\r
+\r
+               if(xpos < 20)\r
+                       trpm = 1850;\r
+               else if(xpos < 40)\r
+                       trpm = 1750;\r
+               else if(xpos < 60)\r
+                       trpm = 1650;\r
+               else\r
+                       trpm = 1550;\r
+\r
+               trpm += arpm;\r
+\r
+               /*\r
+                *      Handle fluxuations in RPM by adjusting the power level if the\r
+                *      motor RPMs go out of range after three 'tries' (over a course\r
+                *      of 600ms).\r
+                */\r
+\r
+               if(ca < trpm - 40){\r
+                       speed++;\r
+                       zMotorSet("Left cannon" ,-speed,2);\r
+                       zMotorSet("Right cannon", speed,2);\r
+               }else if(ca > trpm + 50){\r
+                       speed--;\r
+                       zMotorSet("Left cannon" ,-speed,2);\r
+                       zMotorSet("Right cannon", speed,2);\r
+               }\r
+\r
+               lcdPrint(uart1,2,"%4.0lf/%4.0lf",trpm,rpm);\r
+\r
+               delay(200);\r
+       }\r
+\r
+       zMotorSet("Left cannon" ,0,2);\r
+       zMotorSet("Right cannon",0,2);\r
 \r
-       zLCDWrite(1,"%.3lf %.3lf",heading,dist);\r
-       //zLCDWrite(2,"%.3lf %.3lf",l,r);\r
+       zMotorReturn("Left cannon");\r
+       zMotorReturn("Right cannon");\r
+}\r
+\r
+/**\r
+ * The Ball-Pusher process.\r
+ *\r
+ * This process will handle the pusher that actually sends the ball into the\r
+ * cannon through either an instant push or a detected one, in which the lift\r
+ * is ran until a ball is seen via a light sensor.\r
+ */\r
+\r
+void armProc(void *procPtr){\r
+       static unsigned int start;\r
+\r
+       armProcRun = true;\r
+\r
+       /*\r
+        * Claim necessary motors.\r
+        */\r
+\r
+       zMotorTake("Lift 1",3);\r
+       zMotorTake("Lift 2",3);\r
+       zMotorTake("Misc",3);\r
+\r
+       /*\r
+        * Collect the time we started this operation, but negate it so we can add\r
+        * the finish millis() call to collect a difference in milliseconds.\r
+        */\r
+\r
+       start = -millis();\r
+\r
+       /*\r
+        * Just run the pusher if it was requested by the user.\r
+        */\r
+\r
+       if(zJoyDigital(1,7,JOY_DOWN))\r
+               goto PUSH;\r
+\r
+       /*\r
+        * Run the lift and wait until a ball is detected by the light sensor. Once\r
+        * a ball is seen, stop the lift so that the pusher can do its job.\r
+        */\r
+\r
+       zMotorSet("Lift 1",127,3);\r
+       zMotorSet("Lift 2",127,3);\r
+\r
+       while(armProcRun && analogRead(8) > lightThresh)\r
+               delay(10);\r
+\r
+       if(!armProcRun)\r
+               return;\r
+\r
+       delay(300);\r
+\r
+       zMotorSet("Lift 1",0,3);\r
+       zMotorSet("Lift 2",0,3);\r
+\r
+       /*\r
+        * Push a ball into the cannon.\r
+        */\r
+PUSH:\r
+\r
+       zMotorSet("Misc",127,3);\r
+       delay(1000);\r
+       zMotorSet("Misc",-127,3);\r
+       delay(1000);\r
+       zMotorSet("Misc",0,3);\r
 \r
        /*\r
-        *      RPM control code.\r
-       */\r
+        * 'Stop' the timer and print the result. Do this on the first line in case\r
+        * the RPM tracker or another process is using the second.\r
+        */\r
+\r
+       start += millis();\r
+       lcdPrint(uart1,1,"%ums",start);\r
+\r
+       /*\r
+        * Release the used motors.\r
+        */\r
+\r
+       zMotorReturn("Lift 1");\r
+       zMotorReturn("Lift 2");\r
+       zMotorReturn("Misc");\r
 \r
-       cl = -zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;\r
-       cr =  zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
-       ca = (cl + cr) / 2;\r
-       zLCDWrite(2,"RPM: %.3lf",ca);\r
+       armProcRun = false;\r
+}\r
+\r
+/**\r
+ * The LCD update function, registered at init time to be called by libZephyr's\r
+ * LCD code.\r
+ */\r
+\r
+void lcdUpdateFunc(void *unused_param){\r
+       unsigned long elapsed;\r
+       while(1){\r
+               /*\r
+                * Track elapsed time since operatorControl() entrance.\r
+                */\r
+\r
+               elapsed = millis() - opmillis;\r
+               lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+\r
+               delay(LCD_RATE);\r
+       }\r
 }\r

diff --git a/src/zephyr.c b/src/zephyr.c
index 7bfd76ed0f3c144dd3245029650d5c1cd7c64560..6767dd86074e2fa45750576974741828ebc03a58 100644 (file)
--- a/src/zephyr.c
+++ b/src/zephyr.c
@@ -4,56 +4,6 @@
 #include <string.h>
 
 #ifdef LCD_PORT
-
-static void (*_lcdUpdateFunc)(void *);
-static char lcdBuffer[2][16];
-
-void zLCDHandler(void *unused_param){
-       while(1){
-               lcdSetText(LCD_PORT,1,lcdBuffer[0]);
-               lcdSetText(LCD_PORT,2,lcdBuffer[1]);
-
-               if(_lcdUpdateFunc)
-                       _lcdUpdateFunc(unused_param);
-
-               delay(LCD_RATE);
-       }
-}
-
-void zLCDInit(void){
-       lcdInit(LCD_PORT);
-       lcdClear(LCD_PORT);
-       lcdSetBacklight(LCD_PORT,true);
-}
-
-void zLCDStart(void){
-       taskCreate(zLCDHandler,
-                          TASK_DEFAULT_STACK_SIZE,
-                          NULL,
-                          TASK_PRIORITY_DEFAULT
-                          );
-
-       memset(&lcdBuffer,0,32);
-       strcpy(lcdBuffer[0]," libZEPHYR v1.0 ");
-}
-
-void zLCDWrite(unsigned char line,const char *text,...){
-       va_list args;
-       char buf[16];
-       va_start(args,text);
-       sprintf(buf,text,args);
-       va_end(args);
-       strcpy(lcdBuffer[line-1],buf);
-}
-
-void zLCDSetUpdateFunc(void (*func)(void *)){
-       _lcdUpdateFunc = func;
-}
-
-void zLCDClearUpdateFunc(void){
-       _lcdUpdateFunc = 0;
-}
-
 #endif // LCD_PORT
 
 #ifdef GYRO_PORT
@@ -67,6 +17,14 @@ void zGyroInit(void){
        }
 }
 
+int zGyroGet(void){
+       return gyroGet(gyro);
+}
+
+void zGyroReset(void){
+       gyroReset(gyro);
+}
+
 #endif // GYRO_PORT
 
 /*
@@ -88,7 +46,7 @@ const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
        "Left cannon",
        "Right cannon",
        "Misc",
-       "Port 4",
+       "Intake",
        "Intake",
        "Right drive",
        "Left drive",
@@ -97,6 +55,10 @@ const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
        "Rotater"
 };
 
+static unsigned int mInUse[10]={
+       0,0,0,0,0,0,0,0,0,0
+};
+
 #ifdef IME_ENABLE
 
 const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = {
@@ -117,11 +79,13 @@ void zIMEInit(void){
 
 #endif // IME_ENABLE
 
-void zMotorSet(const char *motor,char speed){
+void zMotorSet(const char *motor,int speed,unsigned int id){
        for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
-               if(!strcmp(MOTOR_PORT_MAP[i],motor)){
+               if(!strcmp(MOTOR_PORT_MAP[i],motor) && mInUse[i] == id){
+                       if(speed >  127)speed = 127;
+                       if(speed < -127)speed = -127;
                        motorSet(i+1,speed);
-                       return;
+                       //return;
                }
        }
 }
@@ -135,6 +99,21 @@ char zMotorGet(const char *motor){
        return 0;
 }
 
+void zMotorTake(const char *motor,unsigned int id){
+       for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
+               if(!strcmp(MOTOR_PORT_MAP[i],motor)){
+                       mInUse[i] = id;
+               }
+       }
+}
+void zMotorReturn(const char *motor){
+       for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
+               if(!strcmp(MOTOR_PORT_MAP[i],motor)){
+                       mInUse[i] = 0;
+               }
+       }
+}
+
 #ifdef IME_ENABLE
 
 int zMotorIMEGet(const char *motor){
@@ -159,17 +138,28 @@ int zMotorIMEGetVelocity(const char *motor){
        return IMEValue;
 }
 
+bool zMotorIMEReset(const char *motor){
+       for(unsigned char i=0;i<imeCount;i++){
+               if(!strcmp(MOTOR_IME_MAP[i],motor)){
+                       return imeReset(i);
+               }
+       }
+       return false;
+}
+
 #endif // IME_ENABLE
 
 void zDriveUpdate(void){
 
 #ifdef DRIVE_NORMAL
-       char s = joystickGetAnalog(DRIVE_JOY,DRIVE_NORMAL);
+       char y = joystickGetAnalog(DRIVE_JOY,DRIVE_NORMAL);
+       char x = joystickGetAnalog(DRIVE_JOY,4);
 
-       APPLY_THRESH(s,DRIVE_THRESHOLD);
+       APPLY_THRESH(x,DRIVE_THRESHOLD);
+       APPLY_THRESH(y,DRIVE_THRESHOLD);
 
-       motorSet(DRIVEL,s);
-       motorSet(DRIVER,s);
+       zMotorSet("Left drive" ,y+x,0);
+       zMotorSet("Right drive",y-x,0);
 
 #else
 
@@ -181,8 +171,8 @@ void zDriveUpdate(void){
        APPLY_THRESH(l,DRIVE_THRESHOLD);
        APPLY_THRESH(r,DRIVE_THRESHOLD);
 
-       zMotorSet("Left drive",l);
-       zMotorSet("Right drive",r);
+       zMotorSet("Left drive",l,0);
+       zMotorSet("Right drive",r,0);
 
 #endif // DRIVE_NORMAL