From da41ab424f45eb92406eeafc4298f8965c881a06 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Wed, 10 Feb 2016 16:46:20 -0500 Subject: tasking improvements --- include/main.h | 14 +++++- src/opcontrol.c | 144 +++++++++++++++++++++++++++++++------------------------- src/reset.c | 2 +- 3 files changed, 92 insertions(+), 68 deletions(-) diff --git a/include/main.h b/include/main.h index cc616d8..d0ee41b 100644 --- a/include/main.h +++ b/include/main.h @@ -94,9 +94,16 @@ extern unsigned char MOTOR_USE_MAP[10]; #define motorTake(p,k) MOTOR_USE_MAP[p-1] = k; #define motorFree(p) MOTOR_USE_MAP[p-1] = 0; + #define motorSetK(p,s,k) if(!MOTOR_USE_MAP[p-1] || MOTOR_USE_MAP[p-1] == k){ motorSet(p,s); } #define motorSetN(p,s) motorSetK(p,s,0) +#define motorSetBK(p,s,k,b) motorSetK(p,b.u ? s : b.d ? -s : 0,k) +#define motorSetBN(p,s,b) motorSetN(p,b.u ? s : b.d ? -s : 0) + +#define motorCopyK(p1,p2,k) motorSetK(p1,motorGet(p2),k) +#define motorCopyN(p1,p2) motorSetN(p1,motorGet(p2)) + int getIMEPort(unsigned int port); int getIME(unsigned int port); int getIMEVelocity(unsigned int port); @@ -108,6 +115,9 @@ int getIMEVelocity(unsigned int port); #define keyUp(b) (b == KEY_UP) #define keyDown(b) (b == DOWN) +#define onKeyUp(b) if(keyUp(b)) +#define onKeyDown(b) if(keyDown(b)) + void setEvent(Controller *c); /** @@ -115,7 +125,7 @@ void setEvent(Controller *c); */ #define LIGHT_THRESH_DEFAULT 50 -#define SONIC_THRESH_DEFAULT 8 +#define SONIC_THRESH_DEFAULT 5 #define initUltrasonic(p1,p2) initSensor((p2<<16)|p1,ULTRASONIC) Sensor initSensor(uint32_t port,unsigned char type); @@ -131,7 +141,7 @@ int readSensor(Sensor *s); * Process library functions */ -#define taskInit(h,p) if(!h) h = taskCreate(h##Code,TASK_DEFAULT_STACK_SIZE,p,TASK_PRIORITY_DEFAULT); +#define taskInit(h,p) h = h ? h : taskCreate(h##Code,TASK_DEFAULT_STACK_SIZE,p,TASK_PRIORITY_DEFAULT) /** * Main function declarations diff --git a/src/opcontrol.c b/src/opcontrol.c index 7338883..1efcb10 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -32,19 +32,21 @@ static Controller c[2]; static Button No = UP; -void taskLCDCode(void *); -void taskPosCode(void *); -void taskCanCode(void *); -void taskLiftCode(void *); -void taskArmCode(void *); -void taskAimCode(void *); +void taskPosCode(void *); // 20ms +// operatorControl(); // 50ms +void taskCanCode(void *); // 100ms +void taskLiftCode(void *); // 100ms +void taskArmCode(void *); // 100ms +void taskAimCode(void *); // 100ms +void taskLCDCode(void *); // 500ms void operatorControl(void){ - static bool invert = false; + static bool invert; DEFAULT_TRPM; c[0].num = 1; c[1].num = 2; + invert = false; /** * Insure that the initialization functions were executed. @@ -59,8 +61,8 @@ void operatorControl(void){ * Get initial readings from each sensor. */ - intakeFrontLeft.initial = readSensor(&intakeFrontLeft); intakeFrontRight.initial = readSensor(&intakeFrontRight); + intakeFrontLeft.initial = readSensor(&intakeFrontLeft); intakeLiftBase.initial = readSensor(&intakeLiftBase); intakeLiftTop.initial = readSensor(&intakeLiftTop); //robotGyro.initial = readSensor(&robotGyro); @@ -90,42 +92,33 @@ void operatorControl(void){ setEvent(&c[0]); setEvent(&c[1]); - if(keyUp(c[1].left.front.l)){ - taskInit(taskCan,&c[1].left.front.l); - }else if(keyUp(c[1].left.front.u)){ - /*if(!taskLift){ - c[1].left.front.u = DOWN; - taskLift = taskCreate(taskLiftCode,TASK_DEFAULT_STACK_SIZE,&c[1].left.front.u,TASK_PRIORITY_DEFAULT); - }*/ - taskInit(taskAim,&c[1].left.front.u); - }else if(keyUp(c[1].left.front.d)){ - taskInit(taskArm,&c[1].left.front.d); - } + onKeyUp(c[1].left.front.l) taskInit(taskCan ,&c[1].left.front.l); + onKeyUp(c[1].left.front.u) taskInit(taskLift,&c[1].left.front.u); + onKeyUp(c[1].left.front.d) taskInit(taskArm ,&c[1].left.front.d); + onKeyUp(c[1].left.front.r) taskInit(taskAim ,&c[1].left.front.r); - if(keyUp(c[1].right.front.u)){ - arpm += 50; - }else if(keyUp(c[1].right.front.d)){ - arpm -= 50; - }else if(keyUp(c[1].right.front.l) | keyDown(c[0].right.front.l)){ - softwareReset(); - } + onKeyUp(c[1].right.front.u) arpm += 50; + onKeyUp(c[1].right.front.d) arpm -= 50; + onKeyUp(c[1].right.front.l) softwareReset(); - if(keyUp(c[0].right.front.r) | keyUp(c[1].right.front.r)){ - invert ^= true; - } + onKeyUp(c[0].right.front.r) invert ^= true; + onKeyUp(c[1].right.front.r) invert ^= true; motorSetN(DRIVE_LEFT ,c[0].left.stick.y); motorSetN(DRIVE_RIGHT,c[0].right.stick.y); - 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); - motorSetN(INTAKE_1,invert ? -motorGet(INTAKE_2) : motorGet(INTAKE_2)); + motorSetBN(INTAKE_2,127,c[1].left.back); + motorCopyN(INTAKE_1,INTAKE_2); + + if(invert) + motorSetN(INTAKE_1,-motorGet(INTAKE_1)); - motorSetN(LIFT_1,c[1].right.back.u ? 127 : c[1].right.back.d ? -127 : 0); - motorSetN(LIFT_2,motorGet(LIFT_1)); + motorSetBN(LIFT_1,127,c[1].right.back); + motorCopyN(LIFT_2,LIFT_1); motorSetN(LIFT_ROTATER,-c[1].right.stick.x / 4); - delay(20); + delay(50); } } @@ -134,41 +127,67 @@ static unsigned int ballPos = 0; void taskLiftCode(void *unused){ Button *kill = (Button *)unused; + static bool turned,loaded; + + turned = loaded = false; + + motorTake(LIFT_1,1); + motorTake(LIFT_2,1); motorTake(INTAKE_1,1); motorTake(INTAKE_2,1); + motorSetK(INTAKE_1,127,1); + motorSetK(INTAKE_2,127,1); + do{ - if(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){ - if(cangle < 20 && cangle > -20){ - motorTake(LIFT_1,1); - motorTake(LIFT_2,1); - motorSetK(LIFT_1,127,1); - motorSetK(LIFT_2,127,1); - }else{ - motorSetK(LIFT_1,0,1); - motorSetK(LIFT_2,0,1); - motorFree(LIFT_1); - motorFree(LIFT_2); - - if(!underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) && - !underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT) ){ - motorSetK(INTAKE_1,127,1); - motorSetK(INTAKE_2,127,1); - }else{ - motorSetK(INTAKE_1,0,1); + turned = (cangle > 30) | (cangle < -30); + loaded = underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT); + + /* + * Stop the lift if a ball is ready to be shot. + */ + + if(loaded){ + motorSetK(LIFT_1,0,1); + motorSetK(LIFT_2,0,1); + + /* + * Kill the middle intake motor if there's no room. + */ + + if(turned || underSensor(intakeLiftBase,SONIC_THRESH_DEFAULT)){ + motorSetK(INTAKE_1,0,1); + if(underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) || underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT)){ motorSetK(INTAKE_2,0,1); + }else{ + motorSetK(INTAKE_2,127,1); } + }else{ + motorSetK(INTAKE_1,127,1); + motorSetK(INTAKE_2,127,1); } }else{ - motorSetK(INTAKE_1,0,1); - motorSetK(INTAKE_2,0,1); - motorSetN(LIFT_1,0); - motorSetN(LIFT_2,0); + motorSetK(LIFT_1,127,1); + motorSetK(LIFT_2,127,1); + if(!turned){ + motorSetK(INTAKE_1,127,1); + motorSetK(INTAKE_2,127,1); + }else{ + motorSetK(INTAKE_1,0,1); + if(underSensor(intakeFrontLeft,LIGHT_THRESH_DEFAULT) || underSensor(intakeFrontRight,LIGHT_THRESH_DEFAULT)){ + motorSetK(INTAKE_2,0,1); + } + } } + + delay(100); + }while(!keyDown(*kill)); motorFree(INTAKE_1); motorFree(INTAKE_2); + motorFree(LIFT_1); + motorFree(LIFT_2); while(keyDown(*kill)) delay(100); @@ -186,7 +205,6 @@ void taskAimCode(void *unused){ target = cangle; do{ - if(cangle > target){ motorSetK(LIFT_ROTATER,30,4); }else if(cangle < target){ @@ -391,7 +409,6 @@ PUSH: void taskLCDCode(void *unused){ static unsigned int pos = 1; unsigned int lcdInput; - static double cangle = 0; while(1){ @@ -405,16 +422,13 @@ void taskLCDCode(void *unused){ lcdPrint(LCD_PORT,2," "); break; case 2: - lcdPrint(LCD_PORT,1,"%u %u %u %u", - taskGetState(taskPos), - taskGetState(taskCan), - taskGetState(taskArm), - taskGetState(taskLift)); - break; - case 3: lcdPrint(LCD_PORT,1,"%lf",cangle); lcdPrint(LCD_PORT,2," "); break; + case 3: + lcdPrint(LCD_PORT,1,"%d | %d",getSensor(intakeLiftTop),getSensor(intakeLiftBase)); + lcdPrint(LCD_PORT,2,"%d | %d",getSensor(intakeFrontLeft),getSensor(intakeFrontRight)); + break; } lcdInput = lcdReadButtons(LCD_PORT); diff --git a/src/reset.c b/src/reset.c index 60b5b2b..b90b610 100644 --- a/src/reset.c +++ b/src/reset.c @@ -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 -- cgit v1.2.3