* * \c 240.448 for the 269 IME\r
* * \c 627.2 for the 393 IME in high torque mode (factory default)\r
* * \c 392 for the 393 IME in high speed mode\r
+ * * \c 261.333 for the 393 IME in turbo speed mode\r
*\r
* If the IME address is invalid, or the IME has not been reset or initialized, the value\r
* stored in *value is undefined.\r
* * \c 30.056 for the 269 IME\r
* * \c 39.2 for the 393 IME in high torque mode (factory default)\r
* * \c 24.5 for the 393 IME in high speed mode\r
+ * * \c 16.3333125 for the 393 IME in turbo speed mode\r
*\r
* If the IME address is invalid, or the IME has not been reset or initialized, the value\r
* stored in *value is undefined.\r
#define MAIN_H_\r
\r
#include <API.h>\r
-\r
-#define LCD_PORT uart1\r
-\r
-#define ANALOG_PORT(x) (x+13)\r
+#include <zephyr.h>\r
\r
#ifdef __cplusplus\r
extern "C" {\r
#endif\r
\r
-/*\r
- * Aliases for all the motors, stored in an enum for convenience.\r
-*/\r
-\r
-enum MOTOR_PORT_MAP {\r
- UNUSED = 0,\r
- CANNON1,\r
- CANNON2,\r
- CANNON3,\r
- CANNON4,\r
- INTAKE,\r
- DRIVER,\r
- DRIVEL,\r
- LIFT1,\r
- LIFT2,\r
- ROTATER,\r
-};\r
-\r
-enum IME_PORT_MAP {\r
- IDRIVER = 0,\r
- IDRIVEL,\r
- IROTATER,\r
- ILIFT1,\r
- ILIFT2,\r
- ICANNONL,\r
- ICANNONR\r
-};\r
-\r
-\r
void autonomous();\r
void initializeIO();\r
void initialize();\r
--- /dev/null
+#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_
#include <main.h>\r
\r
-#define TARGET_SPEED 65\r
+/*#define TARGET_SPEED 65\r
\r
static unsigned int time;\r
\r
void autoDelay(unsigned int ms){\r
delay(ms);\r
time-=ms;\r
-}\r
+}*/\r
\r
void autonomous(){\r
- static char speed = 0;\r
+ /*static char speed = 0;\r
\r
time = 15000;\r
\r
//while(1);\r
delay(time);\r
\r
- motorStopAll();\r
+ motorStopAll();*/\r
\r
}\r
+++ /dev/null
-#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);
-}
#include <main.h>\r
\r
-Gyro gyro;\r
-unsigned int imeCount;\r
-\r
void initializeIO(){\r
- pinMode(ANALOG_PORT(1),INPUT_ANALOG); // Power expander status port\r
- pinMode(1,OUTPUT); // LED\r
- pinMode(2,INPUT);\r
- pinMode(3,INPUT);\r
- pinMode(4,INPUT);\r
}\r
\r
void initialize(){\r
\r
- // Initialize the LCDs.\r
-\r
- lcdInit(uart1);\r
- lcdClear(uart1);\r
- lcdSetBacklight(uart1,1);\r
-\r
- // Setup sensors.\r
+ zLCDInit();\r
+ zGyroInit();\r
+ zIMEInit();\r
\r
- imeCount = imeInitializeAll();\r
- gyro=gyroInit(2,0);\r
-\r
- lcdPrint(uart1,1,"%u IMEs :)",imeCount);\r
- delay(1000);\r
- lcdPrint(uart1,1,"ready...");\r
delay(1000);\r
\r
}\r
#include <main.h>\r
-#include <stdint.h>\r
-#include <math.h>\r
\r
-#define PI 3.1415926535L\r
-\r
-extern Gyro gyro; // Originally defined in init.c\r
-\r
-static double Vl, // left wheel velocity\r
- Vr, // right wheel velocity\r
- Vc, // center/average velocity\r
- Va, // angular velocity\r
- Vra; // velocity ratio\r
-\r
-static double R; // radius of current circular path\r
-\r
-static double Px=0, // X position\r
- Py=0; // Y position\r
-\r
-void operatorLCD(void *start){\r
- while(1){\r
-\r
- lcdPrint(uart1,1,"%.3lf, %.3lf",R,Va);\r
-\r
- delay(100);\r
- }\r
-}\r
-\r
-static unsigned int START;\r
+void lcdUpdateFunc(void *);\r
\r
void operatorControl(){\r
\r
- static char uiinc = 0; // See below\r
- static char in=0,lift; // Used to set the multiple cannon motors to the same joy-stick reading.\r
-\r
- static char joy;\r
+ static char lift;\r
\r
- /*\r
- * Start other tasks.\r
- */\r
-\r
- START = ((!digitalRead(3)) | (!digitalRead(4)<<1)) + 1;\r
- taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,&START,TASK_PRIORITY_DEFAULT);\r
+ zLCDStart();\r
+ zLCDSetUpdateFunc(lcdUpdateFunc);\r
\r
while(1){\r
\r
- // Set the drive motors speeds.\r
+ /*\r
+ * Handle drive controls and update drive motor speeds.\r
+ */\r
\r
- joy=joystickGetAnalog(1,3);\r
- if(joy < 10 && joy > -10)joy=0;\r
- motorSet(DRIVEL,joy);\r
- joy=joystickGetAnalog(1,2);\r
- if(joy < 10 && joy > -10)joy=0;\r
- motorSet(DRIVER,joy);\r
+ zDriveUpdate();\r
\r
// Set the rotating motor speed.\r
\r
- motorSet(ROTATER,-joystickGetAnalog(2,1)/4);\r
-\r
- in=joystickGetAnalog(2,3);\r
-\r
- motorSet(CANNON1,in);\r
- motorSet(CANNON2,in);\r
- motorSet(CANNON3,in);\r
- motorSet(CANNON4,in);\r
+ zMotorSet("ROTATER",-zJoyAnalog(2,1)/4);\r
\r
// Set the intake's speed.\r
\r
- motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP )? 127 :\r
- joystickGetDigital(1,6,JOY_DOWN)? -127 :\r
- 0 );\r
+ zMotorSet("INTAKE",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));\r
\r
// Set the lift's speed.\r
\r
- lift=joystickGetDigital(2,6,JOY_UP )? 127 :\r
- joystickGetDigital(2,6,JOY_DOWN)? -127 :\r
- 0 ;\r
+ lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127);\r
\r
- motorSet(LIFT1,lift);\r
- motorSet(LIFT2,lift);\r
-\r
- /*\r
- * Miscellaneous operation handlers.\r
- */\r
-\r
- if(++uiinc==20){ // Equates to every 200ms\r
- uiinc=0;\r
-\r
- // Run autonomous (for testing wo/ competition switch).\r
-\r
- if(joystickGetDigital(1,7,JOY_RIGHT))\r
- autonomous();\r
-\r
- // Test\r
-\r
- if(joystickGetDigital(1,7,JOY_LEFT)){\r
-\r
- }\r
-\r
- }\r
+ zMotorSet("LIFT1",lift);\r
+ zMotorSet("LIFT2",lift);\r
\r
delay(10); // Short delay to allow task switching\r
+ }\r
+}\r
\r
- /*\r
- * Get the velocity of the two (IME'd) drive motors.\r
- */\r
-\r
- static int vl,vr;\r
-\r
- imeGetVelocity(1,&vl);\r
- imeGetVelocity(0,&vr);\r
-\r
- /*\r
- * Convert the raw reading to rotations a minute, then inches per minute, then inches\r
- * per second.\r
- */\r
-\r
- Vl = vl /* RPM Divisor */ / 39.2L\r
- /* Wheel Circumference */ * 8\r
- /* Minutes to seconds */ / 60;\r
-\r
- Vr = -vr / 39.2L * 8 / 60; // Right wheel IME is inverted\r
-\r
- Vc = (Vl + Vr) / 2; // Average/Center velocity\r
-\r
- /*\r
- * Round down the results to the nearest inch, and enforce a 2 in/s threshold.\r
- */\r
-\r
- Vl = floor(Vl);\r
- Vr = floor(Vr);\r
-\r
- if(Vl < 2 && Vl > -2) Vl = 0;\r
- if(Vr < 2 && Vr > -2) Vr = 0;\r
-\r
- /*\r
- * Calculate the ratio of the higher velocity to the lowest, for determining the\r
- * radius of the circle the robot is forming.\r
- */\r
-\r
- if(((!Vr) & (Vl!=0)) ||\r
- ((!Vl) & (Vr!=0)) ){\r
-\r
- /*\r
- * One wheel isn't moving, the radius is the distance between the wheels.\r
- */\r
-\r
- R = 15;\r
- goto CONT;\r
-\r
- }else if((Vr > Vl) & (Vl > 0) || // Curve to forwards right\r
- (Vl > Vr) & (Vl < 0) ){ // Curve to backwards right\r
-\r
- Vra = Vr / Vl;\r
-\r
- }else if((Vl > Vr) & (Vr > 0) || // Curve to forwards left\r
- (Vr > Vl) & (Vr < 0) ){ // Curve to backwards left\r
-\r
- Vra = Vl / Vr;\r
-\r
- }else Vra = 0; // No movement?\r
-\r
- if(Vra<0) Vra *= -1; // get absolute value of the ratio\r
-\r
- /*\r
- * "Plug-n'-chug" for a radius, assuming that the radius will increase by 7.5 (the\r
- * halfway between the wheels on the robot) when multiplied by the ratio.\r
- */\r
-\r
- for(R = 0; R < 200; R++){\r
-\r
- if(R * Vra > R + 7 && // Allow a one inch margin of error\r
- R * Vra < R + 8)break; //\r
- }\r
-\r
-CONT:\r
-\r
- /*\r
- * Calculate the anglular velocity of the robot.\r
- */\r
-\r
- Va = Vc / R;\r
-\r
- /*\r
- * Determine the increase in X and Y position based on the angular velocity and the\r
- * average total movement.\r
- */\r
+void lcdUpdateFunc(void *unused_param){\r
+ static double liftIME;\r
\r
- Px += cos(Va) * Vc * .01; // Convert per second velocity to per 10ms, as the motors\r
- Py += sin(Va) * Vc * .01; // have only been at this velocity for that time (above).\r
+ liftIME = (zMotorIMEGetVelocity("LIFT1") - zMotorIMEGetVelocity("LIFT2")) / 2 / 16.3333125L;\r
\r
- }\r
+ zLCDWrite(1,"%.3lf",liftIME);\r
}\r
--- /dev/null
+#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;
+}
projects / abelleisle / vex5106z.git / commitdiff