prog skills

diff --git a/src/auto.c b/src/auto.c
index 5cd39b51c83b762404ae9a87a960853d08e1e6ec..c26bfd94beed9a0db53f55ff6aa1db1184d168cc 100644 (file)
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,10 +1,10 @@
 #include <main.h>\r
 #include <zephyr.h>\r
 \r
-#define TARGET_RPM 1700\r
+//#define TARGET_RPM 1700\r
 \r
-void autonomous(){\r
-       /*static double cl,cr,ca;\r
+/*void autonomous(){\r
+       static double cl,cr,ca;\r
        static char speed;\r
 \r
        speed = 30;\r
@@ -25,5 +25,5 @@ void autonomous(){
        delay(900);\r
        zMotorSet("Misc",0);\r
 \r
-       motorStopAll();*/\r
-}\r
+       motorStopAll();\r
+}*/\r

diff --git a/src/opcontrol.c b/src/opcontrol.c
index 1d9a0a71cfb245f689f211c6610f3863d5c6a1c2..41da2088c8f6e5c8a7400aeaa864d35ab2c16f01 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -50,6 +50,7 @@ static char cann = 0;
 static double rpm  = 0,\r
                          trpm = 1850,\r
                          arpm = 0;\r
+static bool cannReady = false;\r
 \r
 /**\r
  * Contains the current X and Y position in inches. The X axis extends from\r
@@ -321,8 +322,8 @@ void moveProc(void *unused_param){
                 * 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
+               lv =  zMotorIMEGetVelocity("Left drive")  / 39.2L * 12.566L / 60000;\r
+               rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000;\r
 \r
                /**\r
                 * Get the distance thing.\r
@@ -388,7 +389,7 @@ void aimProc(void *procPtr){
                 */\r
 \r
                cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);\r
-               rangle = zGyroGet() - (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
+               rangle = zGyroGet() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
 \r
                lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);\r
 \r
@@ -422,7 +423,7 @@ void aimProc(void *procPtr){
 \r
 void cannonProc(void *procPtr){\r
        static double cl,cr,ca;\r
-       static int speed;\r
+       static int speed;//,ispeed = 0;\r
 \r
        cannonProcRun = true;\r
 \r
@@ -524,10 +525,16 @@ void cannonProc(void *procPtr){
                        speed += 2;\r
                        zMotorSet("Left cannon" ,-speed,2);\r
                        zMotorSet("Right cannon", speed,2);\r
+                       cannReady = false;\r
                }else if(ca > trpm + 40){\r
                        speed -= 2;\r
+                       //if(speed < ispeed) speed = ispeed;\r
                        zMotorSet("Left cannon" ,-speed,2);\r
                        zMotorSet("Right cannon", speed,2);\r
+                       cannReady = false;\r
+               }else{\r
+                       cannReady = true;\r
+                       //ispeed = speed;\r
                }\r
 \r
                lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);\r
@@ -536,7 +543,6 @@ void cannonProc(void *procPtr){
                delay(100);\r
        }\r
 \r
-       zMotorSet("Left cannon" ,0,2);\r
        zMotorSet("Right cannon",0,2);\r
 \r
        zMotorReturn("Left cannon");\r
@@ -633,7 +639,6 @@ PUSH:
  */\r
 \r
 void lcdUpdateFunc(void *unused_param){\r
-       unsigned long elapsed;\r
        while(1){\r
                /*\r
                 * Track elapsed time since operatorControl() entrance.\r
@@ -647,3 +652,22 @@ void lcdUpdateFunc(void *unused_param){
                delay(LCD_RATE);\r
        }\r
 }\r
+\r
+void autonomous(){\r
+       static unsigned long elapsed = 0;\r
+       opmillis = millis();\r
+       taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+       while(1){\r
+               elapsed = millis() - opmillis;\r
+               lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+\r
+               if(cannReady){\r
+                       zMotorSet("Misc",127,0);\r
+                       delay(500);\r
+                       zMotorSet("Misc",-127,0);\r
+                       delay(500);\r
+                       zMotorSet("Misc",0,0);\r
+               }\r
+               delay(10);\r
+       }\r
+}\r