From 5d3e755cc37dea817b08e88f4697275b927c56b8 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Tue, 15 Dec 2015 16:28:26 -0500 Subject: autonomizing robot --- src/opcontrol.c | 525 +++++++++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 485 insertions(+), 40 deletions(-) (limited to 'src/opcontrol.c') diff --git a/src/opcontrol.c b/src/opcontrol.c index 9eefe95..93edc13 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -1,91 +1,536 @@ #include +#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); + } } -- cgit v1.2.3