From 5d3e755cc37dea817b08e88f4697275b927c56b8 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Tue, 15 Dec 2015 16:28:26 -0500 Subject: autonomizing robot --- include/zephyr.h | 20 +-- src/auto.c | 49 ++---- src/init.c | 7 +- src/opcontrol.c | 525 ++++++++++++++++++++++++++++++++++++++++++++++++++----- src/zephyr.c | 110 ++++++------ 5 files changed, 567 insertions(+), 144 deletions(-) diff --git a/include/zephyr.h b/include/zephyr.h index 1ba234c..cdd2910 100644 --- a/include/zephyr.h +++ b/include/zephyr.h @@ -16,17 +16,6 @@ #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 /* @@ -38,6 +27,8 @@ void zLCDClearUpdateFunc(void); #ifdef GYRO_PORT void zGyroInit(void); +int zGyroGet(void); +void zGyroReset(void); #endif // GYRO_PORT @@ -69,14 +60,19 @@ void zIMEInit(void); #define zJoyAnalog(j,a) joystickGetAnalog(j,a) void zMotorSet(const char *, // Motor Name - char // Desired Speed + int, // Desired Speed + unsigned int // Caller ID ); char zMotorGet(const char *); // Motor Name +void zMotorTake(const char *,unsigned int); +void zMotorReturn(const char *); + #ifdef IME_ENABLE int zMotorIMEGet(const char *); // Motor Name int zMotorIMEGetVelocity(const char *); // Motor Name +bool zMotorIMEReset(const char *motor); #endif // IME_ENABLE diff --git a/src/auto.c b/src/auto.c index 2332428..a5bbb5d 100644 --- a/src/auto.c +++ b/src/auto.c @@ -1,41 +1,28 @@ #include -/*#define TARGET_SPEED 65 - -static unsigned int time; - -void autoDelay(unsigned int ms){ - delay(ms); - time-=ms; -}*/ +#define TARGET_RPM 1700 void autonomous(){ - /*static char speed = 0; + /*static double cl,cr,ca; + static char speed; - time = 15000; + speed = 30; do{ - - speed+=10; - - motorSet(CANNON1,speed); - motorSet(CANNON2,speed); - motorSet(CANNON3,speed); - motorSet(CANNON4,speed); - - autoDelay(200); - - }while(speed +#include #include +#define GOAL_DISTANCE 120 + +/* + * RTTTL songs for speaker usage. + */ + const char *song = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5"; const char *xpst = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5"; const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5"; -const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5.,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\ +const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5,4p,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\ 4a#5,8g5,8f5,4d#5"; -static unsigned char cyc = 0; + +/* + * Contains how many milliseconds it took to enter operatorControl(). + */ + +static unsigned long opmillis = 0; + +/* + * Contains a light sensor value calculated at init time for object detection. + */ + +static int lightThresh = 0; + +static char cann = 0; +static double rpm = 0,trpm = 1750, arpm = 0; + +static double xpos=0,ypos=0; + +static bool cannonProcRun = false, + armProcRun = false, + aimProcRun = false; + +TaskHandle taskLCD, + taskCannon, + taskArm, + taskMove, + taskAim; void lcdUpdateFunc(void *); +void cannonProc(void *); +void armProc(void *); +void moveProc(void *); +void aimProc(void *); + +/** + * Cause the Cortex to reset, which results in the Cortex restarting the + * operator control code. + * + * This reset is accomplished through setting two bits, SYSRESETREQ and + * VECTRESET, in the Application Interrupt and Reset Control Register (AIRCR), + * which is at a known location in memory. SYSRESETREQ will actually request + * the system reset, while VECTRESET is 'reserved for debugging'. This second + * bit may not need to be set; it's only set here because others have found it + * necessary. + */ + +#define AIRCR_ADDR 0xE000ED0C +#define VECTKEY 0x05FA +#define SYSRESETREQ (1<<2) +#define VECTRESET (1<<0) + +void softwareReset(void){ + + /* + * Read the current value of the AIRCR, since some flags currently set in + * the register need to remain the same in order for the reset to work. + */ + + uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR); + + /* + * Here we save the lower 16 bits of the register, write a special key to + * the higher 16 bits that'll allow the write to occur, and then set the + * reset flags. + */ + + AIRCR = (AIRCR & 0xFFFF) | (VECT_KEY << 16) | SYSRESETREQ | VECTRESET; + + // Update the AIRCR. + + *((uint32_t *)0xE000ED0C) = AIRCR; + + /* + * This instruction causes the program to wait until the previous memory + * write is complete, ensuring it is taken into effect and the reset + * request is made. + */ + + asm("DSB"); + + // Wait until the system reset completes. + + while(1); +} + +/****************************************************************************** + * OPERATOR CONTROL * + ******************************************************************************/ void operatorControl(){ + static unsigned char ui_inc = 0; + static unsigned char cyc = 0; + static char lift; - static char lift,cann; + opmillis = millis(); + lightThresh = analogRead(8) - 60; - zLCDStart(); - zLCDSetUpdateFunc(lcdUpdateFunc); + taskLCD=taskCreate(lcdUpdateFunc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + taskMove=taskCreate(moveProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT+1); while(1){ /* * Handle drive controls and update drive motor speeds. - */ + */ zDriveUpdate(); // Set the rotating motor speed. - zMotorSet("Rotater",-zJoyAnalog(2,1)/4); + if(!aimProcRun) + zMotorSet("Rotater",-zJoyAnalog(1,1) / 4,0); // Set the intake's speed. - zMotorSet("Intake",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127)); + zMotorSet("Intake", + zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127), + 0 + ); // Set the lift's speed. - lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127); + if(!armProcRun){ + lift = zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127); + zMotorSet("Lift 1",lift,0); + zMotorSet("Lift 2",lift,0); + } - zMotorSet("Lift 1",lift); - zMotorSet("Lift 2",lift); + // Set the cannon's speed. - cann=zJoyAnalog(2,3);//zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127); + if(!cannonProcRun){ + zMotorSet("Left cannon" ,-cann,0); + zMotorSet("Right cannon", cann,0); + } - zMotorSet("Left cannon" ,-cann); - zMotorSet("Right cannon", cann); + if(++ui_inc == 50){ + ui_inc = 0; - zMotorSet("Misc",zGetDigitalMotorSpeed(2,7,JOY_UP,JOY_DOWN,127)); + if(zJoyDigital(1,5,JOY_UP) && zJoyDigital(1,5,JOY_DOWN)) + cpu_reset(); - if(zJoyDigital(1,7,JOY_LEFT)){ - speakerInit(); - switch(cyc){ - case 0:speakerPlayRtttl(song );break; - case 1:speakerPlayRtttl(xpst );break; - case 2:speakerPlayRtttl(xpst2);break; - case 3:speakerPlayRtttl(bound);break; + if(zJoyDigital(1,7,JOY_LEFT)){ + speakerInit(); + switch(cyc){ + case 0:speakerPlayRtttl(song );break; + case 1:speakerPlayRtttl(xpst );break; + case 2:speakerPlayRtttl(xpst2);break; + case 3:speakerPlayRtttl(bound);break; + } + if(++cyc == 4) cyc = 0; + speakerShutdown(); + }else if(zJoyDigital(1,7,JOY_RIGHT)){ + zGyroReset(); + zMotorIMEReset("Rotater"); + }else if(zJoyDigital(1,7,JOY_UP)) + taskAim = taskCreate(aimProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + else if(zJoyDigital(1,7,JOY_DOWN)) + aimProcRun = false; + + if(zJoyDigital(1,8,JOY_UP)){ + if(cannonProcRun) arpm += 50; + else if(cann < 120)cann += 10; + }else if(zJoyDigital(1,8,JOY_DOWN)){ + if(cannonProcRun) arpm -= 50; + else if(cann > -120)cann -= 10; + }else if(zJoyDigital(1,8,JOY_LEFT)){ + if(!cannonProcRun) + taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + else + cannonProcRun = false; } - if(++cyc == 4) cyc = 0; - speakerShutdown(); + + if(zJoyDigital(1,8,JOY_RIGHT)) + taskArm = taskCreate(armProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); } delay(10); // Short delay to allow task switching } } -void lcdUpdateFunc(void *unused_param){ - static double l,r,dist,heading; +/** + * The Position-Tracker process. + * + * This process tries to track the position of the robot, using a combination + * of motor encoders and the gyroscope. This process is created directly, and + * is expected to run at a higher priority than other tasks. + */ + +void moveProc(void *unused_param){ + static double l,r,lv,rv; + static double dist,head; + static double dpos; + while(1){ + + l = zMotorIMEGet("Left drive") / 627.2L; + r = -zMotorIMEGet("Right drive") / 627.2L; + + // random # -> motor RPM -> wheel RPM (inches per minute) -> inches per millisecond + + lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 8.64L / 60000; + rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 8.64L / 60000; + + // total distance traveled since init + dist = (l - r) * 8.64L; + + head = fmod(dist / 15,360.0L) / 2 * 90; + head = /*(head +*/ zGyroGet()/*) / 2*/; + + dpos = (lv+rv) / 2 * 20; + dpos *= 1000; + dpos = floor(dpos) / 1000; + + ypos += sin(head * PI / 180) * dpos; + xpos += cos(head * PI / 180) * dpos; + + //lcdPrint(uart1,1,"%.3lf,%.3lf",xpos,ypos); + + delay(20); + } +} + +/** + * The Auto-Aim process. + * + * This function will attempt to keep the cannon aimed at a constant angle, + * adjusting its position when the robot's orientation changes by using the + * gyroscope and the encoder build into the rotater motor. + */ + +void aimProc(void *procPtr){ + static double cangle, // Current angle (of rotater) + rangle, // 'Robot' angle (target angle + angle; // Angle between x-axis and line from robot to goal + static double dist; // Distance of robot from goal + double xpos2,goal2; + + aimProcRun = true; + + /* + * Claim necessary motors. + */ + + zMotorTake("Rotater",1); + + /* + * Run until a stop is requested. + */ + + while(aimProcRun){ + + /* + * Read orientation sensors. + */ + + cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5); + rangle = zGyroGet(); + + xpos2 = pow(xpos,2); + goal2 = pow(GOAL_DISTANCE - ypos,2); + dist = sqrt(xpos2 + goal2); + angle = acos((xpos2 + pow(dist,2) - goal2) / (2 * xpos * dist)); + rangle = (angle - PI / 2) * 180 / PI; + + lcdPrint(uart1,2,"%lf",angle); + + /* + * Adjust aim if necessary. + */ + + if(cangle > rangle + 3) + zMotorSet("Rotater",30,1); + else if(cangle < rangle - 3) + zMotorSet("Rotater",-30,1); + else + zMotorSet("Rotater",0,1); + + delay(100); + } + + /* + * Free motors. + */ + + zMotorReturn("Rotater"); +} + +/** + * The RPM-Targeter process. + * + * This will attempt to keep the cannon motors running at a constant speed, + * determined through a target RPM set by the user. + */ + +void cannonProc(void *procPtr){ static double cl,cr,ca; + static int speed; + + cannonProcRun = true; /* - * Positioning code. - */ + * Initialize variables. + * These need to be static so that their values are preserved through + * task switchs but will retain their values when the function is + * re-called, so here we reset them. + */ - l = zMotorIMEGet("Left drive") / 627.2L; - r = -zMotorIMEGet("Right drive") / 627.2L; + speed = 20; + rpm = cl = cr = ca = 0; - dist=(l - r) * 8.64L; - heading = fmod(round(dist / 15),360.0L); + /* + * Reserve needed motors. + */ + + zMotorTake("Left cannon",2); + zMotorTake("Right cannon",2); + + /* + * Here we increase the power provided to the motors until our target + * RPM is reached. + */ + + do{ + + /* + * Bring up the speed, --exiting if we go too high.-- (crossed out) + * The only reasonable explanation for an error such as the speed + * getting too high is a hardware fault in the robot (bad motor, + * bad IME, ...). + */ + + speed += 5; + if(speed > 120) + speed = 127; + + /* + * Set the power levels, and allow the motors to adjust. + */ + + zMotorSet("Left cannon" ,-speed,2); + zMotorSet("Right cannon", speed,2); + + delay(400); + + /* + * Calculate the average RPM, and continue if our target is met. + */ + + cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; + cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; + ca = /*(cl +*/ cr/*) / 2*/; + rpm = ca; + + lcdPrint(uart1,2,"%.3lf/%.3lf",cl,cr); + + }while(cannonProcRun && ca < trpm); + + if(!cannonProcRun) + return; + + /* + * Once we reach our target, we 'idle' at the sped and adjust as + * necessary. `cannInUse` will be cleared by any user attempt to use the + * motor. + */ + + while(cannonProcRun){ + + /* + * Update RPM values. + */ + + cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; + cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; + ca = /*(cl +*/ cr/*) / 2*/; + rpm = ca; + + /* + * Guess an RPM. + */ + + if(xpos < 20) + trpm = 1850; + else if(xpos < 40) + trpm = 1750; + else if(xpos < 60) + trpm = 1650; + else + trpm = 1550; + + trpm += arpm; + + /* + * Handle fluxuations in RPM by adjusting the power level if the + * motor RPMs go out of range after three 'tries' (over a course + * of 600ms). + */ + + if(ca < trpm - 40){ + speed++; + zMotorSet("Left cannon" ,-speed,2); + zMotorSet("Right cannon", speed,2); + }else if(ca > trpm + 50){ + speed--; + zMotorSet("Left cannon" ,-speed,2); + zMotorSet("Right cannon", speed,2); + } + + lcdPrint(uart1,2,"%4.0lf/%4.0lf",trpm,rpm); + + delay(200); + } + + zMotorSet("Left cannon" ,0,2); + zMotorSet("Right cannon",0,2); - zLCDWrite(1,"%.3lf %.3lf",heading,dist); - //zLCDWrite(2,"%.3lf %.3lf",l,r); + zMotorReturn("Left cannon"); + zMotorReturn("Right cannon"); +} + +/** + * The Ball-Pusher process. + * + * This process will handle the pusher that actually sends the ball into the + * cannon through either an instant push or a detected one, in which the lift + * is ran until a ball is seen via a light sensor. + */ + +void armProc(void *procPtr){ + static unsigned int start; + + armProcRun = true; + + /* + * Claim necessary motors. + */ + + zMotorTake("Lift 1",3); + zMotorTake("Lift 2",3); + zMotorTake("Misc",3); + + /* + * Collect the time we started this operation, but negate it so we can add + * the finish millis() call to collect a difference in milliseconds. + */ + + start = -millis(); + + /* + * Just run the pusher if it was requested by the user. + */ + + if(zJoyDigital(1,7,JOY_DOWN)) + goto PUSH; + + /* + * Run the lift and wait until a ball is detected by the light sensor. Once + * a ball is seen, stop the lift so that the pusher can do its job. + */ + + zMotorSet("Lift 1",127,3); + zMotorSet("Lift 2",127,3); + + while(armProcRun && analogRead(8) > lightThresh) + delay(10); + + if(!armProcRun) + return; + + delay(300); + + zMotorSet("Lift 1",0,3); + zMotorSet("Lift 2",0,3); + + /* + * Push a ball into the cannon. + */ +PUSH: + + zMotorSet("Misc",127,3); + delay(1000); + zMotorSet("Misc",-127,3); + delay(1000); + zMotorSet("Misc",0,3); /* - * RPM control code. - */ + * 'Stop' the timer and print the result. Do this on the first line in case + * the RPM tracker or another process is using the second. + */ + + start += millis(); + lcdPrint(uart1,1,"%ums",start); + + /* + * Release the used motors. + */ + + zMotorReturn("Lift 1"); + zMotorReturn("Lift 2"); + zMotorReturn("Misc"); - cl = -zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; - cr = zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; - ca = (cl + cr) / 2; - zLCDWrite(2,"RPM: %.3lf",ca); + armProcRun = false; +} + +/** + * The LCD update function, registered at init time to be called by libZephyr's + * LCD code. + */ + +void lcdUpdateFunc(void *unused_param){ + unsigned long elapsed; + while(1){ + /* + * Track elapsed time since operatorControl() entrance. + */ + + elapsed = millis() - opmillis; + lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60)); + + delay(LCD_RATE); + } } diff --git a/src/zephyr.c b/src/zephyr.c index 7bfd76e..6767dd8 100644 --- a/src/zephyr.c +++ b/src/zephyr.c @@ -4,56 +4,6 @@ #include #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 @@ -67,6 +17,14 @@ void zGyroInit(void){ } } +int zGyroGet(void){ + return gyroGet(gyro); +} + +void zGyroReset(void){ + gyroReset(gyro); +} + #endif // GYRO_PORT /* @@ -88,7 +46,7 @@ const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = { "Left cannon", "Right cannon", "Misc", - "Port 4", + "Intake", "Intake", "Right drive", "Left drive", @@ -97,6 +55,10 @@ const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = { "Rotater" }; +static unsigned int mInUse[10]={ + 0,0,0,0,0,0,0,0,0,0 +}; + #ifdef IME_ENABLE const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = { @@ -117,11 +79,13 @@ void zIMEInit(void){ #endif // IME_ENABLE -void zMotorSet(const char *motor,char speed){ +void zMotorSet(const char *motor,int speed,unsigned int id){ for(unsigned char i=0;i 127)speed = 127; + if(speed < -127)speed = -127; motorSet(i+1,speed); - return; + //return; } } } @@ -135,6 +99,21 @@ char zMotorGet(const char *motor){ return 0; } +void zMotorTake(const char *motor,unsigned int id){ + for(unsigned char i=0;i