push

diff --git a/include/zephyr.h b/include/zephyr.h
index cdd2910898d2d68a008d54c776f388461ff6f1f1..11f922f3da4ca4999ecdd6eea44cb2b59e920c50 100644 (file)
--- a/include/zephyr.h
+++ b/include/zephyr.h
@@ -51,10 +51,10 @@ void zIMEInit(void);
 #define DRIVE_JOY                      1
 #define DRIVE_THRESHOLD                10
 
-//#define DRIVE_NORMAL         3
+#define DRIVE_NORMAL           3
 
-#define DRIVE_TANK_LEFT                3
-#define DRIVE_TANK_RIGHT       2
+//#define DRIVE_TANK_LEFT              3
+//#define DRIVE_TANK_RIGHT     2
 
 #define zJoyDigital(j,g,b)     joystickGetDigital(j,g,b)
 #define zJoyAnalog(j,a)                joystickGetAnalog(j,a)

diff --git a/src/auto.c b/src/auto.c
index 1be01efb57de705601c42454da64f3884e0fdff3..5cd39b51c83b762404ae9a87a960853d08e1e6ec 100644 (file)
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,5 +1,5 @@
 #include <main.h>\r
-#include <zephyr.c>\r
+#include <zephyr.h>\r
 \r
 #define TARGET_RPM 1700\r
 \r

diff --git a/src/init.c b/src/init.c
index 1d54cce41c5dd4dcd64e1c17a643fd263b87abeb..e85313d1c6d08e3e54c3c12f5ed565e206950af5 100644 (file)
--- a/src/init.c
+++ b/src/init.c
@@ -6,6 +6,7 @@ void initializeIO(){
 void initialize(){\r
 \r
        pinMode(20,INPUT_ANALOG);\r
+       pinMode(13,INPUT_ANALOG);\r
 \r
        lcdInit(LCD_PORT);\r
        lcdClear(LCD_PORT);\r

diff --git a/src/opcontrol.c b/src/opcontrol.c
index 93edc1390e0e93b1ef1f8196eb978ce248e7f2f9..1d9a0a71cfb245f689f211c6610f3863d5c6a1c2 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -2,9 +2,13 @@
 #include <stdint.h>\r
 #include <math.h>\r
 \r
+/**\r
+ * The distance in inches the robot starts from the goal.\r
+ */\r
+\r
 #define GOAL_DISTANCE 120\r
 \r
-/*\r
+/**\r
  * RTTTL songs for speaker usage.\r
  */\r
 \r
@@ -14,33 +18,69 @@ const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5";
 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
 \r
-/*\r
- * Contains how many milliseconds it took to enter operatorControl().\r
+/**\r
+ * Contains how many milliseconds it took to enter operatorControl(), used to\r
+ * measure how long we've been in 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
+ * Contains a light sensor value collected at init time for object detection.\r
  */\r
 \r
 static int lightThresh = 0;\r
 \r
+/**\r
+ * The value passed to motorSet() for the cannons. This variable is declared\r
+ * in a global scope so that the 'Speed Adjust' buttons can modify raw speed\r
+ * when not in RPM Tracking Mode.\r
+ */\r
+\r
 static char cann = 0;\r
-static double rpm  = 0,trpm = 1750, arpm = 0;\r
+\r
+/**\r
+ * 'rpm' is the current RPM of the cannon motors when in RPM Tracking Mode.\r
+ * 'trpm' contains the target RPM for the cannon to reach, which is adjusted\r
+ * when the robot moves.\r
+ * 'arpm' stands for Adjust RPM, and contains a value modifiable by the\r
+ * operator and added to the target RPM.\r
+ */\r
+\r
+static double rpm  = 0,\r
+                         trpm = 1850,\r
+                         arpm = 0;\r
+\r
+/**\r
+ * Contains the current X and Y position in inches. The X axis extends from\r
+ * the front of the robot, with the Y axis perpendicular to the X axis.\r
+ */\r
 \r
 static double xpos=0,ypos=0;\r
 \r
+/**\r
+ * These bools are set by their corresponding tasks when those tasks are ran or\r
+ * stopped. These prevent multiple instances of the same task.\r
+ */\r
+\r
 static bool cannonProcRun = false,\r
                        armProcRun = false,\r
                        aimProcRun = false;\r
 \r
+/**\r
+ * Task handles for the tasks, should they be needed.\r
+ */\r
+\r
 TaskHandle taskLCD,\r
                   taskCannon,\r
                   taskArm,\r
                   taskMove,\r
                   taskAim;\r
 \r
+/**\r
+ * Task function prototypes so that they can be spawned from operatorControl().\r
+ */\r
+\r
 void lcdUpdateFunc(void *);\r
 void cannonProc(void *);\r
 void armProc(void *);\r
@@ -79,7 +119,7 @@ void softwareReset(void){
         * reset flags.\r
         */\r
 \r
-       AIRCR = (AIRCR & 0xFFFF) | (VECT_KEY << 16) | SYSRESETREQ | VECTRESET;\r
+       AIRCR = (AIRCR & 0xFFFF) | (VECTKEY << 16) | SYSRESETREQ | VECTRESET;\r
 \r
        // Update the AIRCR.\r
 \r
@@ -103,13 +143,32 @@ void softwareReset(void){
  ******************************************************************************/\r
 \r
 void operatorControl(){\r
+\r
+       /**\r
+        * 'ui_inc' is used to have button reads happen at a certain interval.\r
+        * 'cyc' contains what song to play next off of the speaker.\r
+        */\r
+\r
        static unsigned char ui_inc = 0;\r
        static unsigned char cyc = 0;\r
+\r
+       /**\r
+        * The raw speed to set the lift motors to.\r
+        */\r
+\r
        static char lift;\r
 \r
+       /**\r
+        * Collected init-time variables.\r
+        */\r
+\r
        opmillis = millis();\r
        lightThresh = analogRead(8) - 60;\r
 \r
+       /**\r
+        * Spawn the LCD task and the position-tracking task.\r
+        */\r
+\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
@@ -129,7 +188,8 @@ void operatorControl(){
                // Set the intake's speed.\r
 \r
                zMotorSet("Intake",\r
-                                 zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127),\r
+                                 zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127)/*|\r
+                                 zGetDigitalMotorSpeed(2,5,JOY_UP,JOY_DOWN,127)*/,\r
                                  0\r
                                  );\r
 \r
@@ -151,10 +211,14 @@ void operatorControl(){
                if(++ui_inc == 50){\r
                        ui_inc = 0;\r
 \r
+                       // 1-5          CPU reset\r
+\r
                        if(zJoyDigital(1,5,JOY_UP) && zJoyDigital(1,5,JOY_DOWN))\r
-                               cpu_reset();\r
+                               softwareReset();\r
+\r
+                       // 1-7L         Speaker function\r
 \r
-                       if(zJoyDigital(1,7,JOY_LEFT)){\r
+                       /*if(zJoyDigital(1,7,JOY_LEFT)){\r
                                speakerInit();\r
                                switch(cyc){\r
                                case 0:speakerPlayRtttl(song );break;\r
@@ -164,28 +228,47 @@ void operatorControl(){
                                }\r
                                if(++cyc == 4) cyc = 0;\r
                                speakerShutdown();\r
-                       }else if(zJoyDigital(1,7,JOY_RIGHT)){\r
+\r
+                       // 2-8R         Gyroscope reset\r
+\r
+                       }else if(zJoyDigital(2,8,JOY_RIGHT)){\r
                                zGyroReset();\r
                                zMotorIMEReset("Rotater");\r
-                       }else if(zJoyDigital(1,7,JOY_UP))\r
+\r
+                       // 2-8U         Auto-Aim start\r
+\r
+                       }else if(zJoyDigital(2,8,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
+                       // 2-8D         Auto-Aim kill\r
+\r
+                       else if(zJoyDigital(2,8,JOY_DOWN))\r
+                               aimProcRun = false;*/\r
+\r
+                       // 2-7U         Increase cannon (speed/target RPM)\r
+\r
+                       if(zJoyDigital(1,7,JOY_UP)){\r
                                if(cannonProcRun) arpm += 50;\r
                                else if(cann < 120)cann += 10;\r
-                       }else if(zJoyDigital(1,8,JOY_DOWN)){\r
+\r
+                       // 2-7D         Decrease cannon\r
+\r
+                       }else if(zJoyDigital(1,7,JOY_DOWN)){\r
                                if(cannonProcRun) arpm -= 50;\r
                                else if(cann > -120)cann -= 10;\r
-                       }else if(zJoyDigital(1,8,JOY_LEFT)){\r
+\r
+                       // 2-7L         Toggle RPM Tracking task.\r
+\r
+                       }else if(zJoyDigital(1,7,JOY_LEFT)){\r
                                if(!cannonProcRun)\r
                                        taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
                                else\r
                                        cannonProcRun = false;\r
                        }\r
 \r
-                       if(zJoyDigital(1,8,JOY_RIGHT))\r
+                       // 2-7R         Start Ball Pusher task.\r
+\r
+                       if(zJoyDigital(1,7,JOY_RIGHT))\r
                                taskArm = taskCreate(armProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
                }\r
 \r
@@ -202,34 +285,71 @@ void operatorControl(){
  */\r
 \r
 void moveProc(void *unused_param){\r
+\r
+       /**\r
+        * 'l' and 'r' contain IMEGet() values, 'lv' and 'rv' contain\r
+        * IMEGetVelocity() values.\r
+        */\r
+\r
        static double l,r,lv,rv;\r
+\r
+       /**\r
+        * 'dist' might contain the total distance traveled in the past 10\r
+        * milliseconds.\r
+        * 'head' contains the angle the robot is facing at relative to its origin.\r
+        * 'dpos' is the delta position.?\r
+        */\r
+\r
        static double dist,head;\r
        static double dpos;\r
+\r
        while(1){\r
 \r
+               /**\r
+                * Get IMEGet values.\r
+                */\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
+                * Get IMEGetVelocity values and convert to inches per millisecond:\r
+                *\r
+                * random # ->\r
+                * motor RPM ->\r
+                * wheel RPM (inches per minute) ->\r
+                * inches per millisecond\r
+                */\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
+               /**\r
+                * Get the distance thing.\r
+                */\r
+\r
                dist = (l - r) * 8.64L;\r
 \r
+               /**\r
+                * Calculate heading, then trash the result in favor of the gyroscope.\r
+                */\r
+\r
                head = fmod(dist / 15,360.0L) / 2 * 90;\r
                head = /*(head +*/ zGyroGet()/*) / 2*/;\r
 \r
+               /**\r
+                * Calculate the delta position thing.\r
+                */\r
+\r
                dpos = (lv+rv) / 2 * 20;\r
                dpos *= 1000;\r
                dpos = floor(dpos) / 1000;\r
 \r
+               // Adjust position values.\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
@@ -243,42 +363,36 @@ void moveProc(void *unused_param){
  */\r
 \r
 void aimProc(void *procPtr){\r
+\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
+                                 rangle;       // 'Robot' angle (target angle\r
+\r
+       // Tell others we're running.\r
 \r
        aimProcRun = true;\r
 \r
-       /*\r
-        * Claim necessary motors.\r
+       /**\r
+        * Claim necessary motors to prevent others from using them.\r
         */\r
 \r
        zMotorTake("Rotater",1);\r
 \r
-       /*\r
+       /**\r
         * Run until a stop is requested.\r
         */\r
 \r
        while(aimProcRun){\r
 \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
+               rangle = zGyroGet() - (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
 \r
-               lcdPrint(uart1,2,"%lf",angle);\r
+               lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);\r
 \r
-               /*\r
+               /**\r
                 * Adjust aim if necessary.\r
                 */\r
 \r
@@ -292,7 +406,7 @@ void aimProc(void *procPtr){
                delay(100);\r
        }\r
 \r
-       /*\r
+       /**\r
         * Free motors.\r
         */\r
 \r
@@ -337,13 +451,13 @@ void cannonProc(void *procPtr){
        do{\r
 \r
                /*\r
-                *      Bring up the speed, --exiting if we go too high.-- (crossed out)\r
+                *      Bring up the speed ...\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
+               speed += 10;\r
                if(speed > 120)\r
                        speed = 127;\r
 \r
@@ -354,7 +468,7 @@ void cannonProc(void *procPtr){
                zMotorSet("Left cannon" ,-speed,2);\r
                zMotorSet("Right cannon", speed,2);\r
 \r
-               delay(400);\r
+               delay(300);\r
 \r
                /*\r
                 *      Calculate the average RPM, and continue if our target is met.\r
@@ -362,10 +476,7 @@ void cannonProc(void *procPtr){
 \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
+               rpm = ca = cr;//(cl + cr) / 2;\r
 \r
        }while(cannonProcRun && ca < trpm);\r
 \r
@@ -386,8 +497,7 @@ void cannonProc(void *procPtr){
 \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
+               rpm = ca = cr;//(cl + cr) / 2;\r
 \r
                /*\r
                 *      Guess an RPM.\r
@@ -411,18 +521,19 @@ void cannonProc(void *procPtr){
                 */\r
 \r
                if(ca < trpm - 40){\r
-                       speed++;\r
+                       speed += 2;\r
                        zMotorSet("Left cannon" ,-speed,2);\r
                        zMotorSet("Right cannon", speed,2);\r
-               }else if(ca > trpm + 50){\r
-                       speed--;\r
+               }else if(ca > trpm + 40){\r
+                       speed -= 2;\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
+               lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);\r
+               //printf("%.0lf,%.3lf,%d\n",trpm,rpm,speed);\r
 \r
-               delay(200);\r
+               delay(100);\r
        }\r
 \r
        zMotorSet("Left cannon" ,0,2);\r
@@ -464,7 +575,7 @@ void armProc(void *procPtr){
         * Just run the pusher if it was requested by the user.\r
         */\r
 \r
-       if(zJoyDigital(1,7,JOY_DOWN))\r
+       if(zJoyDigital(1,8,JOY_DOWN))\r
                goto PUSH;\r
 \r
        /*\r
@@ -492,9 +603,9 @@ void armProc(void *procPtr){
 PUSH:\r
 \r
        zMotorSet("Misc",127,3);\r
-       delay(1000);\r
+       delay(500);\r
        zMotorSet("Misc",-127,3);\r
-       delay(1000);\r
+       delay(500);\r
        zMotorSet("Misc",0,3);\r
 \r
        /*\r
@@ -528,9 +639,11 @@ void lcdUpdateFunc(void *unused_param){
                 * 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
+               //elapsed = millis() - opmillis;\r
+               //lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+               //lcdPrint(uart1,1,"%.3lf V",(float)analogRead(1)/70/4);\r
 \r
+               //lcdPrint(uart1,2,"%4.0lf/%4.0lf",trpm,rpm);\r
                delay(LCD_RATE);\r
        }\r
 }\r

diff --git a/src/zephyr.c b/src/zephyr.c
index 53d712d957a06acb3ff27581a5435cbee8a533f9..6767dd86074e2fa45750576974741828ebc03a58 100644 (file)
--- a/src/zephyr.c
+++ b/src/zephyr.c
@@ -1,8 +1,6 @@
 #include <zephyr.h>
 #include <main.h>
 
-#define PI =3.14159
-
 #include <string.h>
 
 #ifdef LCD_PORT