From c3b516ee1f6e0d317e629ba95abf4ac0cd94f534 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Wed, 10 Feb 2016 14:28:16 -0500 Subject: revised everything --- src/auto.c | 29 -- src/control.c | 72 +++++ src/init.c | 36 ++- src/opcontrol.c | 808 +++++++++++++++++++++++--------------------------------- src/reset.c | 52 ++++ src/sensor.c | 88 ++++++ src/zephyr.c | 184 ------------- 7 files changed, 572 insertions(+), 697 deletions(-) delete mode 100644 src/auto.c create mode 100644 src/control.c create mode 100644 src/reset.c create mode 100644 src/sensor.c delete mode 100644 src/zephyr.c (limited to 'src') diff --git a/src/auto.c b/src/auto.c deleted file mode 100644 index c26bfd9..0000000 --- a/src/auto.c +++ /dev/null @@ -1,29 +0,0 @@ -#include -#include - -//#define TARGET_RPM 1700 - -/*void autonomous(){ - static double cl,cr,ca; - static char speed; - - speed = 30; - - do{ - speed += 10; - zMotorSet("Left cannon",-speed); - zMotorSet("Right cannon",speed); - delay(800); - cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; - cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; - ca = (cl + cr) / 2; - }while(ca < TARGET_RPM); - - zMotorSet("Misc",127); - delay(900); - zMotorSet("Misc",-127); - delay(900); - zMotorSet("Misc",0); - - motorStopAll(); -}*/ diff --git a/src/control.c b/src/control.c new file mode 100644 index 0000000..0ca6026 --- /dev/null +++ b/src/control.c @@ -0,0 +1,72 @@ +#include + +void setEvent(Controller *c){ + c->left.stick.x = joystickGetAnalog(c->num, 4); + c->left.stick.y = joystickGetAnalog(c->num, 3); + + c->right.stick.x = joystickGetAnalog(c->num, 1); + c->right.stick.y = joystickGetAnalog(c->num, 2); + + /** + * BACK RIGHT BUTTONS + */ + + if(c->right.back.d == KEY_UP)c->right.back.d = UP; + else if(c->right.back.d == DOWN && !joystickGetDigital(c->num, 6, JOY_DOWN))c->right.back.d = KEY_UP; + else c->right.back.d = joystickGetDigital(c->num, 6, JOY_DOWN); + + if(c->right.back.u == KEY_UP)c->right.back.u = UP; + else if(c->right.back.u == DOWN && !joystickGetDigital(c->num, 6, JOY_UP))c->right.back.u = KEY_UP; + else c->right.back.u = joystickGetDigital(c->num, 6, JOY_UP); + + + /* + * BACK LEFT BUTTONS + */ + if(c->left.back.d == KEY_UP)c->left.back.d = UP; + else if(c->left.back.d == DOWN && !joystickGetDigital(c->num, 5, JOY_DOWN))c->left.back.d = KEY_UP; + else c->left.back.d = joystickGetDigital(c->num, 5, JOY_DOWN); + + if(c->left.back.u == KEY_UP)c->left.back.u = UP; + else if(c->left.back.u == DOWN && !joystickGetDigital(c->num, 5, JOY_UP))c->left.back.u = KEY_UP; + else c->left.back.u = joystickGetDigital(c->num, 5, JOY_UP); + + + /* + * LEFT FRONT BUTTONS + */ + if(c->left.front.l == KEY_UP)c->left.front.l = UP; + else if(c->left.front.l == DOWN && !joystickGetDigital(c->num, 7, JOY_LEFT))c->left.front.l = KEY_UP; + else c->left.front.l = joystickGetDigital(c->num, 7, JOY_LEFT); + + if(c->left.front.r == KEY_UP)c->left.front.r = UP; + else if(c->left.front.r == DOWN && !joystickGetDigital(c->num, 7, JOY_RIGHT))c->left.front.r = KEY_UP; + else c->left.front.r = joystickGetDigital(c->num, 7, JOY_RIGHT); + + if(c->left.front.u == KEY_UP)c->left.front.u = UP; + else if(c->left.front.u == DOWN && !joystickGetDigital(c->num, 7, JOY_UP))c->left.front.u = KEY_UP; + else c->left.front.u = joystickGetDigital(c->num, 7, JOY_UP); + + if(c->left.front.d == KEY_UP)c->left.front.d = UP; + else if(c->left.front.d == DOWN && !joystickGetDigital(c->num, 7, JOY_DOWN))c->left.front.d = KEY_UP; + else c->left.front.d = joystickGetDigital(c->num, 7, JOY_DOWN); + + /* + * RIGHT FRONT BUTTONS + */ + if(c->right.front.d == KEY_UP)c->right.front.d = UP; + else if(c->right.front.d == DOWN && !joystickGetDigital(c->num, 8, JOY_DOWN))c->right.front.d = KEY_UP; + else c->right.front.d = joystickGetDigital(c->num, 8, JOY_DOWN); + + if(c->right.front.l == KEY_UP)c->right.front.l = UP; + else if(c->right.front.l == DOWN && !joystickGetDigital(c->num, 8, JOY_LEFT))c->right.front.l = KEY_UP; + else c->right.front.l = joystickGetDigital(c->num, 8, JOY_LEFT); + + if(c->right.front.u == KEY_UP)c->right.front.u = UP; + else if(c->right.front.u == DOWN && !joystickGetDigital(c->num, 8, JOY_UP))c->right.front.u = KEY_UP; + else c->right.front.u = joystickGetDigital(c->num, 8, JOY_UP); + + if(c->right.front.r == KEY_UP)c->right.front.r = UP; + else if(c->right.front.r == DOWN && !joystickGetDigital(c->num, 8, JOY_RIGHT))c->right.front.r = KEY_UP; + else c->right.front.r = joystickGetDigital(c->num, 8, JOY_RIGHT); +} diff --git a/src/init.c b/src/init.c index e85313d..f8154d7 100644 --- a/src/init.c +++ b/src/init.c @@ -1,20 +1,34 @@ #include -void initializeIO(){ -} +Sensor intakeFrontLeft, + intakeFrontRight, + intakeLiftBase, + intakeLiftTop, + robotGyro; + +Gyro gyro; -void initialize(){ +bool initializeDone = false; + +void initializeIO(void){ + intakeFrontLeft = initSensor(7,ANALOG); + intakeFrontRight = initSensor(6,ANALOG); + intakeLiftBase = initUltrasonic(1,2); + intakeLiftTop = initSensor(8,ANALOG); + //robotGyro = initSensor(2,GYRO); +} - pinMode(20,INPUT_ANALOG); - pinMode(13,INPUT_ANALOG); +void initialize(void){ + lcdInit(uart2); + lcdClear(uart2); + lcdSetBacklight(uart2,true); - lcdInit(LCD_PORT); - lcdClear(LCD_PORT); - lcdSetBacklight(LCD_PORT,true); + gyro = gyroInit(2,0); - zGyroInit(); - zIMEInit(); + imeInitializeAll(); - delay(1000); + lcdPrint(uart2,1," 5106 ZEPHYR "); + initializeDone = true; + delay(2000); } diff --git a/src/opcontrol.c b/src/opcontrol.c index 41da208..7338883 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -1,320 +1,234 @@ #include -#include #include -/** - * The distance in inches the robot starts from the goal. - */ +#define AUTO_SKILLS -#define GOAL_DISTANCE 120 +extern Sensor intakeFrontLeft, + intakeFrontRight, + intakeLiftBase, + intakeLiftTop;//, + //robotGyro; -/** - * RTTTL songs for speaker usage. - */ +extern Gyro gyro; -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,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"; +extern bool initializeDone; -/** - * Contains how many milliseconds it took to enter operatorControl(), used to - * measure how long we've been in operatorControl(). - */ +static TaskHandle taskLCD = NULL, + taskPos = NULL, + taskCan = NULL, + taskArm = NULL, + taskLift = NULL, + taskAim = NULL; -static unsigned long opmillis = 0; +static float xpos = 0, + ypos = 0; -/** - * Contains a light sensor value collected at init time for object detection. - */ +static double cangle = 0; -static int lightThresh = 0; - -/** - * The value passed to motorSet() for the cannons. This variable is declared - * in a global scope so that the 'Speed Adjust' buttons can modify raw speed - * when not in RPM Tracking Mode. - */ - -static char cann = 0; - -/** - * 'rpm' is the current RPM of the cannon motors when in RPM Tracking Mode. - * 'trpm' contains the target RPM for the cannon to reach, which is adjusted - * when the robot moves. - * 'arpm' stands for Adjust RPM, and contains a value modifiable by the - * operator and added to the target RPM. - */ - -static double rpm = 0, - trpm = 1850, - arpm = 0; +static double rpm = 0, trpm = 1850, arpm = 0; static bool cannReady = false; -/** - * Contains the current X and Y position in inches. The X axis extends from - * the front of the robot, with the Y axis perpendicular to the X axis. - */ - -static double xpos=0,ypos=0; - -/** - * These bools are set by their corresponding tasks when those tasks are ran or - * stopped. These prevent multiple instances of the same task. - */ - -static bool cannonProcRun = false, - armProcRun = false, - aimProcRun = false; - -/** - * Task handles for the tasks, should they be needed. - */ - -TaskHandle taskLCD, - taskCannon, - taskArm, - taskMove, - taskAim; - -/** - * Task function prototypes so that they can be spawned from operatorControl(). - */ - -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. - */ +static Controller c[2]; - uint32_t AIRCR = *((uint32_t *)AIRCR_ADDR); +static Button No = UP; - /* - * 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) | (VECTKEY << 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. - */ +void taskLCDCode(void *); +void taskPosCode(void *); +void taskCanCode(void *); +void taskLiftCode(void *); +void taskArmCode(void *); +void taskAimCode(void *); - asm("DSB"); +void operatorControl(void){ + static bool invert = false; - // Wait until the system reset completes. - - while(1); -} - -/****************************************************************************** - * OPERATOR CONTROL * - ******************************************************************************/ - -void operatorControl(){ - - /** - * 'ui_inc' is used to have button reads happen at a certain interval. - * 'cyc' contains what song to play next off of the speaker. - */ - - static unsigned char ui_inc = 0; - static unsigned char cyc = 0; + DEFAULT_TRPM; + c[0].num = 1; + c[1].num = 2; /** - * The raw speed to set the lift motors to. + * Insure that the initialization functions were executed. */ - static char lift; + if(!initializeDone){ + initializeIO(); + initialize(); + } /** - * Collected init-time variables. + * Get initial readings from each sensor. */ - opmillis = millis(); - lightThresh = analogRead(8) - 60; + intakeFrontLeft.initial = readSensor(&intakeFrontLeft); + intakeFrontRight.initial = readSensor(&intakeFrontRight); + intakeLiftBase.initial = readSensor(&intakeLiftBase); + intakeLiftTop.initial = readSensor(&intakeLiftTop); + //robotGyro.initial = readSensor(&robotGyro); - /** - * Spawn the LCD task and the position-tracking task. - */ + taskInit(taskLCD,NULL); + taskInit(taskPos,NULL); - taskLCD=taskCreate(lcdUpdateFunc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); - taskMove=taskCreate(moveProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT+1); + if(taskCan) + taskDelete(taskCan); while(1){ - /* - * Handle drive controls and update drive motor speeds. + /** + * Update sensor values. */ - zDriveUpdate(); - - // Set the rotating motor speed. - - if(!aimProcRun) - zMotorSet("Rotater",-zJoyAnalog(1,1) / 4,0); - - // Set the intake's speed. + readSensor(&intakeFrontLeft); + readSensor(&intakeFrontRight); + readSensor(&intakeLiftBase); + readSensor(&intakeLiftTop); + //readSensor(&robotGyro); - zMotorSet("Intake", - zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127)/*| - zGetDigitalMotorSpeed(2,5,JOY_UP,JOY_DOWN,127)*/, - 0 - ); - - // Set the lift's speed. + /** + * Read the joystick!!! + */ - if(!armProcRun){ - lift = zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127); - zMotorSet("Lift 1",lift,0); - zMotorSet("Lift 2",lift,0); + 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); } - // Set the cannon's speed. + 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(); + } - if(!cannonProcRun){ - zMotorSet("Left cannon" ,-cann,0); - zMotorSet("Right cannon", cann,0); + if(keyUp(c[0].right.front.r) | keyUp(c[1].right.front.r)){ + invert ^= true; } - if(++ui_inc == 50){ - ui_inc = 0; + motorSetN(DRIVE_LEFT ,c[0].left.stick.y); + motorSetN(DRIVE_RIGHT,c[0].right.stick.y); - // 1-5 CPU reset + 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)); - if(zJoyDigital(1,5,JOY_UP) && zJoyDigital(1,5,JOY_DOWN)) - softwareReset(); + motorSetN(LIFT_1,c[1].right.back.u ? 127 : c[1].right.back.d ? -127 : 0); + motorSetN(LIFT_2,motorGet(LIFT_1)); - // 1-7L Speaker function + motorSetN(LIFT_ROTATER,-c[1].right.stick.x / 4); - /*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(); - - // 2-8R Gyroscope reset + delay(20); + } +} - }else if(zJoyDigital(2,8,JOY_RIGHT)){ - zGyroReset(); - zMotorIMEReset("Rotater"); +static unsigned int ballPos = 0; - // 2-8U Auto-Aim start +void taskLiftCode(void *unused){ + Button *kill = (Button *)unused; - }else if(zJoyDigital(2,8,JOY_UP)) - taskAim = taskCreate(aimProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + motorTake(INTAKE_1,1); + motorTake(INTAKE_2,1); - // 2-8D Auto-Aim kill + 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); + motorSetK(INTAKE_2,0,1); + } + } + }else{ + motorSetK(INTAKE_1,0,1); + motorSetK(INTAKE_2,0,1); + motorSetN(LIFT_1,0); + motorSetN(LIFT_2,0); + } + }while(!keyDown(*kill)); - else if(zJoyDigital(2,8,JOY_DOWN)) - aimProcRun = false;*/ + motorFree(INTAKE_1); + motorFree(INTAKE_2); - // 2-7U Increase cannon (speed/target RPM) + while(keyDown(*kill)) + delay(100); - if(zJoyDigital(1,7,JOY_UP)){ - if(cannonProcRun) arpm += 50; - else if(cann < 120)cann += 10; + taskLift = NULL; +} - // 2-7D Decrease cannon +void taskAimCode(void *unused){ + Button *kill = (Button *)unused; - }else if(zJoyDigital(1,7,JOY_DOWN)){ - if(cannonProcRun) arpm -= 50; - else if(cann > -120)cann -= 10; + static double target = 0; - // 2-7L Toggle RPM Tracking task. + motorTake(LIFT_ROTATER,4); - }else if(zJoyDigital(1,7,JOY_LEFT)){ - if(!cannonProcRun) - taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); - else - cannonProcRun = false; - } + target = cangle; - // 2-7R Start Ball Pusher task. + do{ - if(zJoyDigital(1,7,JOY_RIGHT)) - taskArm = taskCreate(armProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + if(cangle > target){ + motorSetK(LIFT_ROTATER,30,4); + }else if(cangle < target){ + motorSetK(LIFT_ROTATER,-30,4); + }else{ + motorSetK(LIFT_ROTATER,0,4); } - delay(10); // Short delay to allow task switching - } -} - -/** - * 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){ + delay(100); + }while(!keyDown(*kill)); - /** - * 'l' and 'r' contain IMEGet() values, 'lv' and 'rv' contain - * IMEGetVelocity() values. - */ + motorFree(LIFT_ROTATER); - static double l,r,lv,rv; + while(keyDown(*kill)) + delay(100); - /** - * 'dist' might contain the total distance traveled in the past 10 - * milliseconds. - * 'head' contains the angle the robot is facing at relative to its origin. - * 'dpos' is the delta position.? - */ + taskAim = NULL; +} - static double dist,head; +void taskPosCode(void *unused){ + static unsigned int dlime,drime; + static double /*l,r,*/lv,rv; + static double /*dist,*/head; static double dpos; + dlime = getIMEPort(DRIVE_LEFT); + drime = getIMEPort(DRIVE_RIGHT); + while(1){ + cangle = (int)floor(getIME(getIMEPort(LIFT_ROTATER)) / 627.2L * 112.5); + cangle += 0.28L * (cangle / 25); + /** - * Get IMEGet values. + * Get IME values. */ - l = zMotorIMEGet("Left drive") / 627.2L; - r = -zMotorIMEGet("Right drive") / 627.2L; + //l = getIME(dlime) / 627.2L; + //r = getIME(drime) / 627.2L; /** - * Get IMEGetVelocity values and convert to inches per millisecond: + * Get IME velocity values and convert to inches per millisecond: * * random # -> * motor RPM -> @@ -322,21 +236,22 @@ void moveProc(void *unused_param){ * inches per millisecond */ - lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 12.566L / 60000; - rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000; + lv = getIMEVelocity(dlime) / 39.2L * 12.566L / 60000; + rv = -getIMEVelocity(drime) / 39.2L * 12.566L / 60000; /** * Get the distance thing. */ - dist = (l - r) * 8.64L; + //dist = (l - r) * 8.64L; /** * Calculate heading, then trash the result in favor of the gyroscope. */ - head = fmod(dist / 15,360.0L) / 2 * 90; - head = /*(head +*/ zGyroGet()/*) / 2*/; + //head = fmod(dist / 15,360.0L) / 2 * 90; + //head = 0;//getSensor(robotGyro); + head = gyroGet(gyro); /** * Calculate the delta position thing. @@ -355,149 +270,47 @@ void moveProc(void *unused_param){ } } -/** - * 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 - - // Tell others we're running. - - aimProcRun = true; +void taskCanCode(void *unused){ + Button *kill = (Button *)unused; - /** - * Claim necessary motors to prevent others from using them. - */ - - 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() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI); - - lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle); - - /** - * 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 unsigned int clime,crime; static double cl,cr,ca; - static int speed;//,ispeed = 0; - - cannonProcRun = true; + static int speed; - /* - * 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. - */ + clime = getIMEPort(CANNON_LEFT); + crime = getIMEPort(CANNON_RIGHT); speed = 20; rpm = cl = cr = ca = 0; - /* - * 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. - */ + motorTake(CANNON_LEFT,2); + motorTake(CANNON_RIGHT,2); do{ - /* - * Bring up the speed ... - * 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 += 10; 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); + motorSetK(CANNON_LEFT,-speed,2); + motorSetK(CANNON_RIGHT,speed,2); delay(300); - /* - * 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; + cl = getIMEVelocity(clime) / 16.3333125L * 9; + cr = -getIMEVelocity(crime) / 16.3333125L * 9; rpm = ca = cr;//(cl + cr) / 2; - }while(cannonProcRun && ca < trpm); - - if(!cannonProcRun) - return; + }while(ca < trpm); - /* - * 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){ + while(!keyDown(*kill)){ /* * Update RPM values. */ - cl = zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; - cr = -zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; + cl = getIMEVelocity(clime) / 16.3333125L * 9; + cr = -getIMEVelocity(crime) / 16.3333125L * 9; rpm = ca = cr;//(cl + cr) / 2; /* @@ -522,152 +335,201 @@ void cannonProc(void *procPtr){ */ if(ca < trpm - 40){ - speed += 2; - zMotorSet("Left cannon" ,-speed,2); - zMotorSet("Right cannon", speed,2); + speed += 2 * ((trpm - rpm) / 60); + motorSetK(CANNON_LEFT,-speed,2); + motorSetK(CANNON_RIGHT,speed,2); cannReady = false; }else if(ca > trpm + 40){ - speed -= 2; - //if(speed < ispeed) speed = ispeed; - zMotorSet("Left cannon" ,-speed,2); - zMotorSet("Right cannon", speed,2); + speed --; + motorSetK(CANNON_LEFT,-speed,2); + motorSetK(CANNON_RIGHT,speed,2); cannReady = false; - }else{ + }else cannReady = true; - //ispeed = speed; - } - - lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm); - //printf("%.0lf,%.3lf,%d\n",trpm,rpm,speed); delay(100); } - zMotorSet("Right cannon",0,2); + motorSetK(CANNON_LEFT,0,2); + motorSetK(CANNON_RIGHT,0,2); + + motorFree(CANNON_LEFT); + motorFree(CANNON_RIGHT); + + while(keyDown(*kill)) + delay(100); - zMotorReturn("Left cannon"); - zMotorReturn("Right cannon"); + taskCan = NULL; } -/** - * 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 taskArmCode(void *unused){ + if(keyDown(c[1].left.front.r)) + goto PUSH; -void armProc(void *procPtr){ - static unsigned int start; + while(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT) && ((rpm < trpm - 30) | (rpm > trpm + 50))) + delay(100); - armProcRun = true; + /*while(ballPos != 5) + delay(100);*/ - /* - * Claim necessary motors. - */ +PUSH: - zMotorTake("Lift 1",3); - zMotorTake("Lift 2",3); - zMotorTake("Misc",3); + motorTake(LIFT_PUSHER,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. - */ + motorSetK(LIFT_PUSHER,127,3); + delay(500); + motorSetK(LIFT_PUSHER,-127,3); + delay(800); + motorSetK(LIFT_PUSHER,0,3); - start = -millis(); + motorFree(LIFT_PUSHER); - /* - * Just run the pusher if it was requested by the user. - */ + taskArm = NULL; +} - if(zJoyDigital(1,8,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. - */ +void taskLCDCode(void *unused){ + static unsigned int pos = 1; + unsigned int lcdInput; + static double cangle = 0; - zMotorSet("Lift 1",127,3); - zMotorSet("Lift 2",127,3); + while(1){ - while(armProcRun && analogRead(8) > lightThresh) - delay(10); + switch(pos){ + case 0: + lcdPrint(LCD_PORT,1,"(%.2lf,%.2lf)",xpos,ypos); + lcdPrint(LCD_PORT,2,"%s: %u",taskGetState(taskLift) == TASK_RUNNING ? "Running" : "Stopped",ballPos); + break; + case 1: + lcdPrint(LCD_PORT,1,"%.0lf | %.2lf\n",trpm,rpm); + 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; + } - if(!armProcRun) - return; + lcdInput = lcdReadButtons(LCD_PORT); - delay(300); + if((lcdInput & LCD_BTN_LEFT) && pos) + pos--; + else if((lcdInput & LCD_BTN_RIGHT) && pos < 3) + pos++; + else if(lcdInput & LCD_BTN_CENTER) + softwareReset(); - zMotorSet("Lift 1",0,3); - zMotorSet("Lift 2",0,3); + delay(500); + } +} - /* - * Push a ball into the cannon. - */ -PUSH: +#ifdef AUTO_SKILLS - zMotorSet("Misc",127,3); - delay(500); - zMotorSet("Misc",-127,3); - delay(500); - zMotorSet("Misc",0,3); +/***************************************************************************** + * AUTONOMOUS - SKILLS * + *****************************************************************************/ - /* - * '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. - */ +void autonomous(){ + static unsigned long inc = 0; - start += millis(); - lcdPrint(uart1,1,"%ums",start); + EXTRA_TRPM; + intakeLiftTop.initial = readSensor(&intakeLiftTop); - /* - * Release the used motors. - */ + taskInit(taskPos,&No); + taskInit(taskAim,&No); + taskInit(taskCan,&No); - zMotorReturn("Lift 1"); - zMotorReturn("Lift 2"); - zMotorReturn("Misc"); + while(1){ - armProcRun = false; -} + if(++inc == 50){ + inc = 0; + lcdPrint(LCD_PORT,1,"%.0lf RPM",rpm); + } -/** - * The LCD update function, registered at init time to be called by libZephyr's - * LCD code. - */ + //readSensor(&intakeLiftTop); + //if(rpm >= trpm){// && underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){ + if(rpm > trpm - 30 && rpm < trpm + 50){ -void lcdUpdateFunc(void *unused_param){ - while(1){ - /* - * Track elapsed time since operatorControl() entrance. - */ + lcdPrint(LCD_PORT,2,"Launch!"); - //elapsed = millis() - opmillis; - //lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60)); - //lcdPrint(uart1,1,"%.3lf V",(float)analogRead(1)/70/4); + delay(500); + + motorSet(LIFT_PUSHER,127); + delay(500); + motorSet(LIFT_PUSHER,-127); + delay(800); + motorSet(LIFT_PUSHER,0); - //lcdPrint(uart1,2,"%4.0lf/%4.0lf",trpm,rpm); - delay(LCD_RATE); + lcdPrint(LCD_PORT,2," "); + } + + delay(10); } } +#else + +/***************************************************************************** + * AUTONOMOUS - NORMAL * + *****************************************************************************/ + void autonomous(){ - static unsigned long elapsed = 0; - opmillis = millis(); - taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT); + static unsigned long start,elapsed = 0; + static unsigned long inc = 0; + + EXTRA_TRPM; + start = millis(); + intakeLiftTop.initial = readSensor(&intakeLiftTop); + + taskInit(taskCan,&No); + + motorSet(LIFT_1,127); + motorSet(LIFT_2,127); + //motorSet(INTAKE_1,127); + //motorSet(INTAKE_2,127); + while(1){ - elapsed = millis() - opmillis; - lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60)); + elapsed = millis() - start; - if(cannReady){ - zMotorSet("Misc",127,0); - delay(500); - zMotorSet("Misc",-127,0); - delay(500); - zMotorSet("Misc",0,0); + if(++inc == 50){ + inc = 0; + lcdPrint(LCD_PORT,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60)); + lcdPrint(LCD_PORT,2,"%.0lf RPM",rpm); + } + + readSensor(&intakeLiftTop); + if(underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){ + + + delay(200); + motorSet(LIFT_1,0); + motorSet(LIFT_2,0); + //motorSet(INTAKE_1,0); + //motorSet(INTAKE_2,0); + + if(rpm >= trpm){ + motorSet(LIFT_PUSHER,127); + delay(500); + motorSet(LIFT_PUSHER,-127); + delay(500); + motorSet(LIFT_PUSHER,0); + + motorSet(LIFT_1,127); + motorSet(LIFT_2,127); + //motorSet(INTAKE_1,127); + //motorSet(INTAKE_2,127); + } } + delay(10); } } + +#endif // AUTO_SKILLS diff --git a/src/reset.c b/src/reset.c new file mode 100644 index 0000000..60b5b2b --- /dev/null +++ b/src/reset.c @@ -0,0 +1,52 @@ +#include + +/** + * 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) | (VECTKEY << 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); +} diff --git a/src/sensor.c b/src/sensor.c new file mode 100644 index 0000000..2725642 --- /dev/null +++ b/src/sensor.c @@ -0,0 +1,88 @@ +#include + +/** + * Nowhere else to put this...? + */ + +unsigned char MOTOR_USE_MAP[10] = { + 0,0,0,0,0,0,0,0,0,0 +}; + +int getIMEPort(unsigned int port){ + switch(port){ + case CANNON_LEFT : return CANNON_LEFT_IME; + case CANNON_RIGHT : return CANNON_RIGHT_IME; + case LIFT_PUSHER : + case INTAKE_1 : + case INTAKE_2 : return -1; + case DRIVE_RIGHT : return DRIVE_RIGHT_IME; + case DRIVE_LEFT : return DRIVE_LEFT_IME; + case LIFT_1 : return LIFT_1_IME; + case LIFT_2 : return LIFT_2_IME; + case LIFT_ROTATER : return LIFT_ROTATER_IME; + } + return -1; +} + +int getIME(unsigned int port){ + int ret; + imeGet(port,&ret); + return ret; +} + +int getIMEVelocity(unsigned int port){ + int ret; + imeGetVelocity(port,&ret); + return ret; +} + +/** + * Actual sensor stuffs + */ + +Sensor initSensor(uint32_t port,unsigned char type){ + Sensor s; + s.type = type; + switch(s.type){ + case DIGITAL: + pinMode(port,INPUT); + s.port = port; + //s.initial = digitalRead(port); + break; + case ANALOG: + pinMode(port + 13,INPUT_ANALOG); + s.port = port; + //s.initial = analogRead(port); + break; + case GYRO: + s.port = 0; + s.data.gyro = gyroInit(port,0); + //s.initial = gyroGet(s.data.gyro); + break; + case ULTRASONIC: + s.port = 0; + s.data.sonic = ultrasonicInit((uint16_t)port,port>>16); + //s.initial = ultrasonicGet(s.data.sonic); + break; + } + s.value = 0; + return s; +} + +int readSensor(Sensor *s){ + switch(s->type){ + case DIGITAL: + return (s->value = digitalRead(s->port)); + break; + case ANALOG: + return (s->value = analogRead(s->port)); + break; + case GYRO: + return (s->value = gyroGet(s->data.gyro)); + break; + case ULTRASONIC: + return (s->value = ultrasonicGet(s->data.sonic)); + break; + } + return (s->value = -1); +} diff --git a/src/zephyr.c b/src/zephyr.c deleted file mode 100644 index 6767dd8..0000000 --- a/src/zephyr.c +++ /dev/null @@ -1,184 +0,0 @@ -#include -#include - -#include - -#ifdef LCD_PORT -#endif // LCD_PORT - -#ifdef GYRO_PORT - -static Gyro gyro; -static bool gyroEnabled = false; - -void zGyroInit(void){ - if((gyro=gyroInit(2,0))){ - gyroEnabled = true; - } -} - -int zGyroGet(void){ - return gyroGet(gyro); -} - -void zGyroReset(void){ - gyroReset(gyro); -} - -#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 7 -#endif // IME_ENABLE - -const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = { - "Left cannon", - "Right cannon", - "Misc", - "Intake", - "Intake", - "Right drive", - "Left drive", - "Lift 1", - "Lift 2", - "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] = { - "Right drive", - "Left drive", - "Rotater", - "Lift 1", - "Lift 2", - "Left cannon", - "Right cannon" -}; - -static unsigned int imeCount = 0; - -void zIMEInit(void){ - imeCount = imeInitializeAll(); -} - -#endif // IME_ENABLE - -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; - } - } -} - -char zMotorGet(const char *motor){ - for(unsigned char i=0;i