libZEPHYR began

8 files changed, 311 insertions, 378 deletions

diff --git a/include/API.h b/include/API.h
index 7de7889..32d8686 100644
--- a/include/API.h
+++ b/include/API.h
@@ -504,6 +504,7 @@ unsigned int imeInitializeAll();
  * * \c 240.448 for the 269 IME

  * * \c 627.2 for the 393 IME in high torque mode (factory default)

  * * \c 392 for the 393 IME in high speed mode

+ * * \c 261.333 for the 393 IME in turbo speed mode

  *

  * If the IME address is invalid, or the IME has not been reset or initialized, the value

  * stored in *value is undefined.

@@ -527,6 +528,7 @@ bool imeGet(unsigned char address, int *value);
  * * \c 30.056 for the 269 IME

  * * \c 39.2 for the 393 IME in high torque mode (factory default)

  * * \c 24.5 for the 393 IME in high speed mode

+ * * \c 16.3333125 for the 393 IME in turbo speed mode

  *

  * If the IME address is invalid, or the IME has not been reset or initialized, the value

  * stored in *value is undefined.

diff --git a/include/main.h b/include/main.h
index d1fcdb4..7022f11 100644
--- a/include/main.h
+++ b/include/main.h
@@ -2,44 +2,12 @@
 #define MAIN_H_

 

 #include <API.h>

-

-#define LCD_PORT uart1

-

-#define ANALOG_PORT(x) (x+13)

+#include <zephyr.h>

 

 #ifdef __cplusplus

 extern "C" {

 #endif

 

-/*

- *	Aliases for all the motors, stored in an enum for convenience.

-*/

-

-enum MOTOR_PORT_MAP {

-	UNUSED = 0,

-	CANNON1,

-	CANNON2,

-	CANNON3,

-	CANNON4,

-	INTAKE,

-	DRIVER,

-	DRIVEL,

-	LIFT1,

-	LIFT2,

-	ROTATER,

-};

-

-enum IME_PORT_MAP {

-	IDRIVER = 0,

-	IDRIVEL,

-	IROTATER,

-	ILIFT1,

-	ILIFT2,

-	ICANNONL,

-	ICANNONR

-};

-

-

 void autonomous();

 void initializeIO();

 void initialize();

diff --git a/include/zephyr.h b/include/zephyr.h
new file mode 100644
index 0000000..844416a
--- /dev/null
+++ b/include/zephyr.h
@@ -0,0 +1,90 @@
+#ifndef ZEPHYR_H_
+#define ZEPHYR_H_
+
+#include <main.h>
+
+#define APPLY_THRESH(n,t)	if(n < t && n > -t){ n = 0;}
+
+/*
+ *	Comment to disable LCD support.
+*/
+
+#define LCD_PORT	uart1
+
+#define LCD_RATE	500
+
+#ifdef LCD_PORT
+
+void zLCDInit(void);
+void zLCDStart(void);
+
+void zLCDWrite(unsigned char,	// 1-based Line Number
+			   const char *,	// Text
+			   ...);
+
+void zLCDSetUpdateFunc(void (*)(void *));	// Function Pointer
+void zLCDClearUpdateFunc(void);
+
+#endif // LCD_PORT
+
+/*
+ *	Comment to disable gyro support.
+*/
+
+#define GYRO_PORT	2
+
+#ifdef GYRO_PORT
+
+void zGyroInit(void);
+
+#endif // GYRO_PORT
+
+/*
+ *	Comment to disable IME support.
+*/
+
+#define IME_ENABLE
+
+#ifdef IME_ENABLE
+
+void zIMEInit(void);
+
+#endif // IME_ENABLE
+
+/*
+ *	DRIVE_NORMAL will override tank drive options.
+*/
+
+#define DRIVE_JOY			1
+#define DRIVE_THRESHOLD		10
+
+//#define DRIVE_NORMAL		3
+
+#define DRIVE_TANK_LEFT		3
+#define DRIVE_TANK_RIGHT	2
+
+#define zJoyDigital(j,g,b)	joystickGetDigital(j,g,b)
+#define zJoyAnalog(j,a)		joystickGetAnalog(j,a)
+
+void zMotorSet(const char *,	// Motor Name
+			   char				// Desired Speed
+			   );
+char zMotorGet(const char *);	// Motor Name
+
+#ifdef IME_ENABLE
+
+int zMotorIMEGet(const char *);			// Motor Name
+int zMotorIMEGetVelocity(const char *);	// Motor Name
+
+#endif // IME_ENABLE
+
+void zDriveUpdate(void);
+
+char zGetDigitalMotorSpeed(unsigned char,	// Joystick No.
+				   	   	   unsigned char,	// Button Group
+						   unsigned char,	// Positive Button
+						   unsigned char,	// Negative Button
+						   char				// Desired Speed
+				   	   	   );
+
+#endif // ZEPHYR_H_
diff --git a/src/auto.c b/src/auto.c
index 3c8f2ef..2332428 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,16 +1,16 @@
 #include <main.h>

 

-#define TARGET_SPEED 65

+/*#define TARGET_SPEED 65

 

 static unsigned int time;

 

 void autoDelay(unsigned int ms){

 	delay(ms);

 	time-=ms;

-}

+}*/

 

 void autonomous(){

-	static char speed = 0;

+	/*static char speed = 0;

 

 	time = 15000;

 

@@ -36,6 +36,6 @@ void autonomous(){
 	//while(1);

 	delay(time);

 

-	motorStopAll();

+	motorStopAll();*/

 

 }

diff --git a/src/auton.c b/src/auton.c
deleted file mode 100644
index 8af90eb..0000000
--- a/src/auton.c
+++ /dev/null
@@ -1,153 +0,0 @@
-#include <main.h>
-
-extern Gyro gyro;
-
-#ifndef PI
-#define PI 3.14159265L
-#endif // PI
-
-#define RPM_DEFAULT_MAX		160				//	Max RPM of high-speed motor
-#define RPM_CANNON_MAX		4000			//	Max RPM of cannon (motor + gears)
-#define RPM_CANNON_INC		31.49606299L	//	Expected change in RPM per joystick increment
-
-#define RPM_THRESHOLD		100				//	A margin of error for matching the target RPM
-
-#define INC_FAULT			2				//	Boost given to motors when left and right don't match RPM
-#define INC_NORMAL			2				//	Used to match actual velocity to the target
-
-#define ROT_THRESHOLD		(PI/8)			//	Margin of error for auto-aiming
-#define ROT_SPEED			30				//	How fast to rotate the cannon for aiming (for linear aiming only)
-
-static double rpmTarget = 0;	// Contains the desired RPM
-
-static double rpmCannL = 0;		// Left side's RPM
-static double rpmCannR = 0;		// Right side's RPM
-static double rpmCannA = 0;		// Average of the two above
-
-static double rotCannon = 0;	// Cumulative rotation of the cannon in radians
-
-/**
- * Calculates RPMs from cannon motors.
- *
- * There are two IMEs on the cannon, one per each side. This function converts the raw
- * velocity reading from the motors and converts it to rotations per minute (RPM).
- * Results are stored in global variables defined above, as well as an average of the
- * two.
- */
-
-void cannUpdate(void){
-	int cl,cr,rc;
-
-	/*
-	 *	Read raw velocity values.
-	*/
-
-	imeGetVelocity(ICANNONL,&cl);
-	imeGetVelocity(ICANNONR,&cr);
-
-	/*
-	 *	Divide by a divisor given in API.h and multiply the result by the cannon's gear ratio.
-	*/
-
-	rpmCannL = cl / 24.5L * 25.0L;
-	rpmCannR = cr / 24.5L * 25.0L;
-
-	/*
-	 *	Calculate an average of the two results from above.
-	*/
-
-	rpmCannA = (rpmCannL + rpmCannR) / 2;
-
-	/*
-	 *	Find the cumulative rotation of the cannon for auto-aiming.
-	*/
-
-	imeGet(IROTATER,&rc);
-
-	rotCannon = rc / 627.2L * 5;
-	rotCannon *= 2*PI;				// convert to radians
-
-}
-
-/**
- * Sets a target RPM for the cannon.
- *
- * This calculates a target RPM by simply multiplying the current position of a jostick axis
- * by a predetermined increment that is 1/127th of the highest possible RPM of the cannon.
- *
- * @param a value returned from a call to joystickGetAnalog()
- */
-
-void cannTarget(char jpos){
-	rpmTarget = jpos * RPM_CANNON_INC;
-}
-
-/**
- * Runs/Updates the actual speed of the cannon motors.
- *
- * Speeds for each side of the cannon are kept inside the scope of this function. Two checks
- * are made before applying the motor speeds. The first one will increase the speed of one
- * side of the cannon if the RPMs of the two sides aren't reasonably equal. The second check
- * will bring the overall speeds up or down depending on where the actual average RPM is
- * compared to the target. Following these checks is the setting of the motor speeds.
- */
-
-void cannRun(void){
-	static char speedCannL = 0;
-	static char speedCannR = 0;
-
-	/*
-	 *	Make sure cannon motors are up to about the same speed (as in RPM).
-	*/
-
-		 if(rpmCannR < rpmTarget + RPM_THRESHOLD && rpmCannL < rpmCannR - RPM_THRESHOLD) speedCannL += INC_FAULT;
-	else if(rpmCannL < rpmTarget + RPM_THRESHOLD &&	rpmCannR < rpmCannL - RPM_THRESHOLD) speedCannR += INC_FAULT;
-
-	/*
-	 *	Adjust motor velocities to match the target.
-	*/
-
-	if(rpmTarget > rpmCannA + RPM_THRESHOLD){
-		speedCannL += INC_NORMAL;
-		speedCannR += INC_NORMAL;
-	}else if(rpmTarget < rpmCannA - RPM_THRESHOLD){
-		speedCannL -= INC_NORMAL;
-		speedCannR -= INC_NORMAL;
-	}
-
-	/*
-	 *	Apply the calculated motor speeds.
-	*/
-
-	motorSet(CANNON1,speedCannL);
-	motorSet(CANNON2,speedCannR);
-	motorSet(CANNON3,speedCannL);
-	motorSet(CANNON4,speedCannR);
-}
-
-/**
- * Aims the cannon towards a specified location(?)
- *
- * ... . Also sets the motor speed.
- *
- * @param a rotation to base calculations of off (preferably that of the robot's body)
- */
-
-void cannAim(double r){
-	static char speed = 0;
-
-	/*
-	 *	Do simple linear adjustments to have okay-ish aim. Once self-location is accomplished
-	 *	a more accurate function could be used to adjust aim.
-	*/
-
-		 if(rotCannon > r + ROT_THRESHOLD) speed =  ROT_SPEED;
-	else if(rotCannon < r - ROT_THRESHOLD) speed = -ROT_SPEED;
-	else								   speed =  0;
-
-	/*
-	 *	Apply the resulting speed to the motor.
-	*/
-
-	motorSet(ROTATER,speed);
-}
diff --git a/src/init.c b/src/init.c
index a59dcba..3c1a7d2 100644
--- a/src/init.c
+++ b/src/init.c
@@ -1,32 +1,14 @@
 #include <main.h>

 

-Gyro gyro;

-unsigned int imeCount;

-

 void initializeIO(){

-	pinMode(ANALOG_PORT(1),INPUT_ANALOG);	// Power expander status port

-	pinMode(1,OUTPUT);						// LED

-	pinMode(2,INPUT);

-	pinMode(3,INPUT);

-	pinMode(4,INPUT);

 }

 

 void initialize(){

 

-	//	Initialize the LCDs.

-

-	lcdInit(uart1);

-	lcdClear(uart1);

-	lcdSetBacklight(uart1,1);

-

-	//	Setup sensors.

+	zLCDInit();

+	zGyroInit();

+	zIMEInit();

 

-	imeCount = imeInitializeAll();

-	gyro=gyroInit(2,0);

-

-	lcdPrint(uart1,1,"%u IMEs :)",imeCount);

-	delay(1000);

-	lcdPrint(uart1,1,"ready...");

 	delay(1000);

 

 }

diff --git a/src/opcontrol.c b/src/opcontrol.c
index 8e410fa..e5955eb 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,193 +1,45 @@
 #include <main.h>

-#include <stdint.h>

-#include <math.h>

 

-#define PI 3.1415926535L

-

-extern Gyro gyro;	// Originally defined in init.c

-

-static double Vl,	// left wheel velocity

-			  Vr,	// right wheel velocity

-			  Vc,	// center/average velocity

-			  Va,	// angular velocity

-			  Vra;	// velocity ratio

-

-static double R;	// radius of current circular path

-

-static double Px=0,	// X position

-			  Py=0;	// Y position

-

-void operatorLCD(void *start){

-	while(1){

-

-		lcdPrint(uart1,1,"%.3lf, %.3lf",R,Va);

-

-		delay(100);

-	}

-}

-

-static unsigned int START;

+void lcdUpdateFunc(void *);

 

 void operatorControl(){

 

-	static char uiinc = 0;	// See below

-	static char in=0,lift;	// Used to set the multiple cannon motors to the same joy-stick reading.

-

-	static char joy;

+	static char lift;

 

-	/*

-	 *	Start other tasks.

-	*/

-

-	START = ((!digitalRead(3)) | (!digitalRead(4)<<1)) + 1;

-	taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,&START,TASK_PRIORITY_DEFAULT);

+	zLCDStart();

+	zLCDSetUpdateFunc(lcdUpdateFunc);

 

 	while(1){

 

-		//	Set the drive motors speeds.

+		/*

+		 *	Handle drive controls and update drive motor speeds.

+		*/

 

-		joy=joystickGetAnalog(1,3);

-		if(joy < 10 && joy > -10)joy=0;

-		motorSet(DRIVEL,joy);

-		joy=joystickGetAnalog(1,2);

-		if(joy < 10 && joy > -10)joy=0;

-		motorSet(DRIVER,joy);

+		zDriveUpdate();

 

 		//	Set the rotating motor speed.

 

-		motorSet(ROTATER,-joystickGetAnalog(2,1)/4);

-

-		in=joystickGetAnalog(2,3);

-

-		motorSet(CANNON1,in);

-		motorSet(CANNON2,in);

-		motorSet(CANNON3,in);

-		motorSet(CANNON4,in);

+		zMotorSet("ROTATER",-zJoyAnalog(2,1)/4);

 

 		// Set the intake's speed.

 

-		motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP  )?  127 :

-						joystickGetDigital(1,6,JOY_DOWN)? -127 :

-						0 );

+		zMotorSet("INTAKE",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));

 

 		// Set the lift's speed.

 

-		lift=joystickGetDigital(2,6,JOY_UP  )?  127 :

-			 joystickGetDigital(2,6,JOY_DOWN)? -127 :

-			 0 ;

+		lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127);

 

-		motorSet(LIFT1,lift);

-		motorSet(LIFT2,lift);

-

-		/*

-		 *	Miscellaneous operation handlers.

-		 */

-

-		if(++uiinc==20){	// Equates to every 200ms

-			uiinc=0;

-

-			// Run autonomous (for testing wo/ competition switch).

-

-			if(joystickGetDigital(1,7,JOY_RIGHT))

-				autonomous();

-

-			// Test

-

-			if(joystickGetDigital(1,7,JOY_LEFT)){

-

-			}

-

-		}

+		zMotorSet("LIFT1",lift);

+		zMotorSet("LIFT2",lift);

 

 		delay(10);	// Short delay to allow task switching

+	}

+}

 

-		/*

-		 *	Get the velocity of the two (IME'd) drive motors.

-		*/

-

-		static int vl,vr;

-

-		imeGetVelocity(1,&vl);

-		imeGetVelocity(0,&vr);

-

-		/*

-		 *	Convert the raw reading to rotations a minute, then inches per minute, then inches

-		 *	per second.

-		*/

-

-		Vl =  vl /* RPM Divisor         */ / 39.2L

-				 /* Wheel Circumference */ * 8

-				 /* Minutes to seconds  */ / 60;

-

-		Vr = -vr / 39.2L * 8 / 60;	// Right wheel IME is inverted

-

-		Vc = (Vl + Vr) / 2;			// Average/Center velocity

-

-		/*

-		 *	Round down the results to the nearest inch, and enforce a 2 in/s threshold.

-		*/

-

-		Vl = floor(Vl);

-		Vr = floor(Vr);

-

-		if(Vl < 2 && Vl > -2) Vl = 0;

-		if(Vr < 2 && Vr > -2) Vr = 0;

-

-		/*

-		 *	Calculate the ratio of the higher velocity to the lowest, for determining the

-		 *	radius of the circle the robot is forming.

-		*/

-

-		if(((!Vr) & (Vl!=0)) ||

-		   ((!Vl) & (Vr!=0)) ){

-

-			/*

-			 *	One wheel isn't moving, the radius is the distance between the wheels.

-			*/

-

-			R = 15;

-			goto CONT;

-

-		}else if((Vr > Vl) & (Vl > 0) ||	// Curve to forwards right

-				 (Vl > Vr) & (Vl < 0) ){	// Curve to backwards right

-

-			Vra = Vr / Vl;

-

-		}else if((Vl > Vr) & (Vr > 0) ||	// Curve to forwards left

-				 (Vr > Vl) & (Vr < 0) ){	// Curve to backwards left

-

-			Vra = Vl / Vr;

-

-		}else Vra = 0;						// No movement?

-

-		if(Vra<0) Vra *= -1;				// get absolute value of the ratio

-

-		/*

-		 *	"Plug-n'-chug" for a radius, assuming that the radius will increase by 7.5 (the

-		 *	halfway between the wheels on the robot) when multiplied by the ratio.

-		*/

-

-		for(R = 0; R < 200; R++){

-

-			if(R * Vra > R + 7 &&		// Allow a one inch margin of error

-			   R * Vra < R + 8)break;	//

-		}

-

-CONT:

-

-		/*

-		 *	Calculate the anglular velocity of the robot.

-		*/

-

-		Va = Vc / R;

-

-		/*

-		 *	Determine the increase in X and Y position based on the angular velocity and the

-		 *	average total movement.

-		*/

+void lcdUpdateFunc(void *unused_param){

+	static double liftIME;

 

-		Px += cos(Va) * Vc * .01;	// Convert per second velocity to per 10ms, as the motors

-		Py += sin(Va) * Vc * .01;	// have only been at this velocity for that time (above).

+	liftIME = (zMotorIMEGetVelocity("LIFT1") - zMotorIMEGetVelocity("LIFT2")) / 2 / 16.3333125L;

 

-	}

+	zLCDWrite(1,"%.3lf",liftIME);

 }

diff --git a/src/zephyr.c b/src/zephyr.c
new file mode 100644
index 0000000..847f95a
--- /dev/null
+++ b/src/zephyr.c
@@ -0,0 +1,192 @@
+#include <zephyr.h>
+#include <main.h>
+
+#include <string.h>
+
+#ifdef LCD_PORT
+
+static void (*_lcdUpdateFunc)(void *);
+static char lcdBuffer[2][16];
+
+void zLCDHandler(void *unused_param){
+	while(1){
+		lcdSetText(LCD_PORT,1,lcdBuffer[0]);
+		lcdSetText(LCD_PORT,2,lcdBuffer[1]);
+
+		if(_lcdUpdateFunc)
+			_lcdUpdateFunc(unused_param);
+
+		delay(LCD_RATE);
+	}
+}
+
+void zLCDInit(void){
+	lcdInit(LCD_PORT);
+	lcdClear(LCD_PORT);
+	lcdSetBacklight(LCD_PORT,true);
+}
+
+void zLCDStart(void){
+	taskCreate(zLCDHandler,
+			   TASK_DEFAULT_STACK_SIZE,
+			   NULL,
+			   TASK_PRIORITY_DEFAULT
+			   );
+
+	memset(&lcdBuffer,0,32);
+	strcpy(lcdBuffer[0]," libZEPHYR v1.0 ");
+}
+
+void zLCDWrite(unsigned char line,const char *text,...){
+	va_list args;
+	char buf[16];
+	va_start(args,text);
+	sprintf(buf,text,args);
+	va_end(args);
+	strcpy(lcdBuffer[line-1],buf);
+}
+
+void zLCDSetUpdateFunc(void (*func)(void *)){
+	_lcdUpdateFunc = func;
+}
+
+void zLCDClearUpdateFunc(void){
+	_lcdUpdateFunc = 0;
+}
+
+#endif // LCD_PORT
+
+#ifdef GYRO_PORT
+
+static Gyro gyro;
+static bool gyroEnabled = false;
+
+void zGyroInit(void){
+	if((gyro=gyroInit(2,0))){
+		gyroEnabled = true;
+	}
+}
+
+#endif // GYRO_PORT
+
+/*
+ *	A map of what's plugged into the motor ports.
+ *	Expected declarations:
+ *
+ *	DRIVEL	-	Left drive port
+ *	DRIVER	-	Right drive port
+ *
+*/
+
+#define MOTOR_PORT_COUNT	10
+
+#ifdef IME_ENABLE
+#define MOTOR_IME_COUNT		5
+#endif // IME_ENABLE
+
+const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
+	"UNUSED1",
+	"UNUSED2",
+	"UNUSED3",
+	"UNUSED4",
+	"INTAKE",
+	"DRIVER",
+	"DRIVEL",
+	"LIFT1",
+	"LIFT2",
+	"ROTATER"
+};
+
+#ifdef IME_ENABLE
+
+const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = {
+	"DRIVER",
+	"DRIVEL",
+	"ROTATER",
+	"LIFT1",
+	"LIFT2"
+};
+
+static unsigned int imeCount = 0;
+
+void zIMEInit(void){
+	imeCount = imeInitializeAll();
+}
+
+#endif // IME_ENABLE
+
+void zMotorSet(const char *motor,char speed){
+	for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
+		if(!strcmp(MOTOR_PORT_MAP[i],motor)){
+			motorSet(i+1,speed);
+			return;
+		}
+	}
+}
+
+char zMotorGet(const char *motor){
+	for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
+		if(!strcmp(MOTOR_PORT_MAP[i],motor)){
+			return motorGet(i+1);
+		}
+	}
+	return 0;
+}
+
+#ifdef IME_ENABLE
+
+int zMotorIMEGet(const char *motor){
+	int IMEValue = 0;
+	for(unsigned char i=0;i<imeCount;i++){
+		if(!strcmp(MOTOR_IME_MAP[i],motor)){
+			imeGet(i,&IMEValue);
+			break;
+		}
+	}
+	return IMEValue;
+}
+
+int zMotorIMEGetVelocity(const char *motor){
+	int IMEValue = 0;
+	for(unsigned char i=0;i<imeCount;i++){
+		if(!strcmp(MOTOR_IME_MAP[i],motor)){
+			imeGetVelocity(i,&IMEValue);
+			break;
+		}
+	}
+	return IMEValue;
+}
+
+#endif // IME_ENABLE
+
+void zDriveUpdate(void){
+
+#ifdef DRIVE_NORMAL
+	char s = joystickGetAnalog(DRIVE_JOY,DRIVE_NORMAL);
+
+	APPLY_THRESH(s,DRIVE_THRESHOLD);
+
+	motorSet(DRIVEL,s);
+	motorSet(DRIVER,s);
+
+#else
+
+	char l,r;
+
+	l = joystickGetAnalog(DRIVE_JOY,DRIVE_TANK_LEFT);
+	r = joystickGetAnalog(DRIVE_JOY,DRIVE_TANK_RIGHT);
+
+	APPLY_THRESH(l,DRIVE_THRESHOLD);
+	APPLY_THRESH(r,DRIVE_THRESHOLD);
+
+	zMotorSet("DRIVEL",l);
+	zMotorSet("DRIVER",r);
+
+#endif // DRIVE_NORMAL
+
+}
+
+char zGetDigitalMotorSpeed(unsigned char joy,unsigned char btn_group,unsigned char btn_pos,unsigned char btn_neg,char speed){
+	return joystickGetDigital(joy,btn_group,btn_pos) ?  speed :
+		   joystickGetDigital(joy,btn_group,btn_neg) ? -speed : 0;
+}