tasking improvements

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