]> code.bitgloo.com Git - abelleisle/vex5106z.git/commitdiff
tasking improvements
authorClyne Sullivan <tullivan99@gmail.com>
Wed, 10 Feb 2016 21:46:20 +0000 (16:46 -0500)
committerClyne Sullivan <tullivan99@gmail.com>
Wed, 10 Feb 2016 21:46:20 +0000 (16:46 -0500)
include/main.h
src/opcontrol.c
src/reset.c

index cc616d864212b7bcc941d1d71462dea084feb779..d0ee41b691bfb33068e6b7e694b5276e508a5702 100644 (file)
@@ -94,9 +94,16 @@ extern unsigned char MOTOR_USE_MAP[10];
 \r
 #define motorTake(p,k)   MOTOR_USE_MAP[p-1] = k;\r
 #define motorFree(p)     MOTOR_USE_MAP[p-1] = 0;\r
+\r
 #define motorSetK(p,s,k) if(!MOTOR_USE_MAP[p-1] || MOTOR_USE_MAP[p-1] == k){ motorSet(p,s); }\r
 #define motorSetN(p,s)   motorSetK(p,s,0)\r
 \r
+#define motorSetBK(p,s,k,b) motorSetK(p,b.u ? s : b.d ? -s : 0,k)\r
+#define motorSetBN(p,s,b)   motorSetN(p,b.u ? s : b.d ? -s : 0)\r
+\r
+#define motorCopyK(p1,p2,k) motorSetK(p1,motorGet(p2),k)\r
+#define motorCopyN(p1,p2)   motorSetN(p1,motorGet(p2))\r
+\r
 int getIMEPort(unsigned int port);\r
 int getIME(unsigned int port);\r
 int getIMEVelocity(unsigned int port);\r
@@ -108,6 +115,9 @@ int getIMEVelocity(unsigned int port);
 #define keyUp(b)   (b == KEY_UP)\r
 #define keyDown(b) (b == DOWN)\r
 \r
+#define onKeyUp(b)   if(keyUp(b))\r
+#define onKeyDown(b) if(keyDown(b))\r
+\r
 void setEvent(Controller *c);\r
 \r
 /**\r
@@ -115,7 +125,7 @@ void setEvent(Controller *c);
  */\r
 \r
 #define LIGHT_THRESH_DEFAULT 50\r
-#define SONIC_THRESH_DEFAULT 8\r
+#define SONIC_THRESH_DEFAULT 5\r
 \r
 #define initUltrasonic(p1,p2) initSensor((p2<<16)|p1,ULTRASONIC)\r
 Sensor initSensor(uint32_t port,unsigned char type);\r
@@ -131,7 +141,7 @@ int readSensor(Sensor *s);
  * Process library functions\r
  */\r
 \r
-#define taskInit(h,p) if(!h) h = taskCreate(h##Code,TASK_DEFAULT_STACK_SIZE,p,TASK_PRIORITY_DEFAULT);\r
+#define taskInit(h,p) h = h ? h : taskCreate(h##Code,TASK_DEFAULT_STACK_SIZE,p,TASK_PRIORITY_DEFAULT)\r
 \r
 /**\r
  * Main function declarations\r
index 73388831d40a77b996ce33527c2daaca11adface..1efcb101e4893babf6e695582effc83f48ee7d45 100644 (file)
@@ -32,19 +32,21 @@ static Controller c[2];
 \r
 static Button No = UP;\r
 \r
-void taskLCDCode(void *);\r
-void taskPosCode(void *);\r
-void taskCanCode(void *);\r
-void taskLiftCode(void *);\r
-void taskArmCode(void *);\r
-void taskAimCode(void *);\r
+void taskPosCode(void *);      //      20ms\r
+//      operatorControl();             //      50ms\r
+void taskCanCode(void *);      //      100ms\r
+void taskLiftCode(void *);     //      100ms\r
+void taskArmCode(void *);      //      100ms\r
+void taskAimCode(void *);      //      100ms\r
+void taskLCDCode(void *);      //      500ms\r
 \r
 void operatorControl(void){\r
-       static bool invert = false;\r
+       static bool invert;\r
 \r
        DEFAULT_TRPM;\r
        c[0].num = 1;\r
        c[1].num = 2;\r
+       invert = false;\r
 \r
        /**\r
         * Insure that the initialization functions were executed.\r
@@ -59,8 +61,8 @@ void operatorControl(void){
         * Get initial readings from each sensor.\r
         */\r
 \r
-       intakeFrontLeft.initial = readSensor(&intakeFrontLeft);\r
        intakeFrontRight.initial = readSensor(&intakeFrontRight);\r
+       intakeFrontLeft.initial = readSensor(&intakeFrontLeft);\r
        intakeLiftBase.initial = readSensor(&intakeLiftBase);\r
        intakeLiftTop.initial = readSensor(&intakeLiftTop);\r
        //robotGyro.initial = readSensor(&robotGyro);\r
@@ -90,42 +92,33 @@ void operatorControl(void){
                setEvent(&c[0]);\r
                setEvent(&c[1]);\r
 \r
-               if(keyUp(c[1].left.front.l)){\r
-                       taskInit(taskCan,&c[1].left.front.l);\r
-               }else if(keyUp(c[1].left.front.u)){\r
-                       /*if(!taskLift){\r
-                               c[1].left.front.u = DOWN;\r
-                               taskLift = taskCreate(taskLiftCode,TASK_DEFAULT_STACK_SIZE,&c[1].left.front.u,TASK_PRIORITY_DEFAULT);\r
-                       }*/\r
-                       taskInit(taskAim,&c[1].left.front.u);\r
-               }else if(keyUp(c[1].left.front.d)){\r
-                       taskInit(taskArm,&c[1].left.front.d);\r
-               }\r
+               onKeyUp(c[1].left.front.l) taskInit(taskCan ,&c[1].left.front.l);\r
+               onKeyUp(c[1].left.front.u) taskInit(taskLift,&c[1].left.front.u);\r
+               onKeyUp(c[1].left.front.d) taskInit(taskArm ,&c[1].left.front.d);\r
+               onKeyUp(c[1].left.front.r) taskInit(taskAim ,&c[1].left.front.r);\r
 \r
-               if(keyUp(c[1].right.front.u)){\r
-                       arpm += 50;\r
-               }else if(keyUp(c[1].right.front.d)){\r
-                       arpm -= 50;\r
-               }else if(keyUp(c[1].right.front.l) | keyDown(c[0].right.front.l)){\r
-                       softwareReset();\r
-               }\r
+               onKeyUp(c[1].right.front.u) arpm += 50;\r
+               onKeyUp(c[1].right.front.d) arpm -= 50;\r
+               onKeyUp(c[1].right.front.l) softwareReset();\r
 \r
-               if(keyUp(c[0].right.front.r) | keyUp(c[1].right.front.r)){\r
-                       invert ^= true;\r
-               }\r
+               onKeyUp(c[0].right.front.r) invert ^= true;\r
+               onKeyUp(c[1].right.front.r) invert ^= true;\r
 \r
                motorSetN(DRIVE_LEFT ,c[0].left.stick.y);\r
                motorSetN(DRIVE_RIGHT,c[0].right.stick.y);\r
 \r
-               motorSetN(INTAKE_2,(c[0].left.back.u | c[1].left.back.u) ? 127 : (c[0].left.back.d | c[1].left.back.d) ? -127 : 0);\r
-               motorSetN(INTAKE_1,invert ? -motorGet(INTAKE_2) : motorGet(INTAKE_2));\r
+               motorSetBN(INTAKE_2,127,c[1].left.back);\r
+               motorCopyN(INTAKE_1,INTAKE_2);\r
+\r
+               if(invert)\r
+                       motorSetN(INTAKE_1,-motorGet(INTAKE_1));\r
 \r
-               motorSetN(LIFT_1,c[1].right.back.u ? 127 : c[1].right.back.d ? -127 : 0);\r
-               motorSetN(LIFT_2,motorGet(LIFT_1));\r
+               motorSetBN(LIFT_1,127,c[1].right.back);\r
+               motorCopyN(LIFT_2,LIFT_1);\r
 \r
                motorSetN(LIFT_ROTATER,-c[1].right.stick.x / 4);\r
 \r
-               delay(20);\r
+               delay(50);\r
        }\r
 }\r
 \r
@@ -134,41 +127,67 @@ static unsigned int ballPos = 0;
 void taskLiftCode(void *unused){\r
        Button *kill = (Button *)unused;\r
 \r
+       static bool turned,loaded;\r
+\r
+       turned = loaded = false;\r
+\r
+       motorTake(LIFT_1,1);\r
+       motorTake(LIFT_2,1);\r
        motorTake(INTAKE_1,1);\r
        motorTake(INTAKE_2,1);\r
 \r
+       motorSetK(INTAKE_1,127,1);\r
+       motorSetK(INTAKE_2,127,1);\r
+\r
        do{\r
-               if(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){\r
-                       if(cangle < 20 && cangle > -20){\r
-                               motorTake(LIFT_1,1);\r
-                               motorTake(LIFT_2,1);\r
-                               motorSetK(LIFT_1,127,1);\r
-                               motorSetK(LIFT_2,127,1);\r
-                       }else{\r
-                               motorSetK(LIFT_1,0,1);\r
-                               motorSetK(LIFT_2,0,1);\r
-                               motorFree(LIFT_1);\r
-                               motorFree(LIFT_2);\r
-\r
-                               if(!underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT)  &&\r
-                                  !underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT) ){\r
-                                       motorSetK(INTAKE_1,127,1);\r
-                                       motorSetK(INTAKE_2,127,1);\r
-                               }else{\r
-                                       motorSetK(INTAKE_1,0,1);\r
+               turned = (cangle > 30) | (cangle < -30);\r
+               loaded = underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT);\r
+\r
+               /*\r
+                * Stop the lift if a ball is ready to be shot.\r
+                */\r
+\r
+               if(loaded){\r
+                       motorSetK(LIFT_1,0,1);\r
+                       motorSetK(LIFT_2,0,1);\r
+\r
+                       /*\r
+                        * Kill the middle intake motor if there's no room.\r
+                        */\r
+\r
+                       if(turned || underSensor(intakeLiftBase,SONIC_THRESH_DEFAULT)){\r
+                               motorSetK(INTAKE_1,0,1);\r
+                               if(underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) || underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT)){\r
                                        motorSetK(INTAKE_2,0,1);\r
+                               }else{\r
+                                       motorSetK(INTAKE_2,127,1);\r
                                }\r
+                       }else{\r
+                               motorSetK(INTAKE_1,127,1);\r
+                               motorSetK(INTAKE_2,127,1);\r
                        }\r
                }else{\r
-                       motorSetK(INTAKE_1,0,1);\r
-                       motorSetK(INTAKE_2,0,1);\r
-                       motorSetN(LIFT_1,0);\r
-                       motorSetN(LIFT_2,0);\r
+                       motorSetK(LIFT_1,127,1);\r
+                       motorSetK(LIFT_2,127,1);\r
+                       if(!turned){\r
+                               motorSetK(INTAKE_1,127,1);\r
+                               motorSetK(INTAKE_2,127,1);\r
+                       }else{\r
+                               motorSetK(INTAKE_1,0,1);\r
+                               if(underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) || underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT)){\r
+                                       motorSetK(INTAKE_2,0,1);\r
+                               }\r
+                       }\r
                }\r
+\r
+               delay(100);\r
+\r
        }while(!keyDown(*kill));\r
 \r
        motorFree(INTAKE_1);\r
        motorFree(INTAKE_2);\r
+       motorFree(LIFT_1);\r
+       motorFree(LIFT_2);\r
 \r
        while(keyDown(*kill))\r
                delay(100);\r
@@ -186,7 +205,6 @@ void taskAimCode(void *unused){
        target = cangle;\r
 \r
        do{\r
-\r
                if(cangle > target){\r
                        motorSetK(LIFT_ROTATER,30,4);\r
                }else if(cangle < target){\r
@@ -391,7 +409,6 @@ PUSH:
 void taskLCDCode(void *unused){\r
        static unsigned int pos = 1;\r
        unsigned int lcdInput;\r
-       static double cangle = 0;\r
 \r
        while(1){\r
 \r
@@ -405,16 +422,13 @@ void taskLCDCode(void *unused){
                        lcdPrint(LCD_PORT,2,"                ");\r
                        break;\r
                case 2:\r
-                       lcdPrint(LCD_PORT,1,"%u %u %u %u",\r
-                                                               taskGetState(taskPos),\r
-                                                               taskGetState(taskCan),\r
-                                                               taskGetState(taskArm),\r
-                                                               taskGetState(taskLift));\r
-                       break;\r
-               case 3:\r
                        lcdPrint(LCD_PORT,1,"%lf",cangle);\r
                        lcdPrint(LCD_PORT,2,"                ");\r
                        break;\r
+               case 3:\r
+                       lcdPrint(LCD_PORT,1,"%d | %d",getSensor(intakeLiftTop),getSensor(intakeLiftBase));\r
+                       lcdPrint(LCD_PORT,2,"%d | %d",getSensor(intakeFrontLeft),getSensor(intakeFrontRight));\r
+                       break;\r
                }\r
 \r
                lcdInput = lcdReadButtons(LCD_PORT);\r
index 60b5b2bea2554b1f8aba0ba4f8e2ca531a2a0f2c..b90b610dabdbf9680469abaaa1d022fb88d1779b 100644 (file)
@@ -36,7 +36,7 @@ void softwareReset(void){
 
        // Update the AIRCR.
 
-       *((uint32_t *)0xE000ED0C) = AIRCR;
+       *((volatile uint32_t *)0xE000ED0C) = AIRCR;
 
        /*
         * This instruction causes the program to wait until the previous memory