\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
#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
*/\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
* 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
\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
* 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
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
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
target = cangle;\r
\r
do{\r
-\r
if(cangle > target){\r
motorSetK(LIFT_ROTATER,30,4);\r
}else if(cangle < target){\r
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
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