summaryrefslogtreecommitdiffstats
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/auto.c29
-rw-r--r--src/control.c72
-rw-r--r--src/init.c36
-rw-r--r--src/opcontrol.c808
-rw-r--r--src/reset.c52
-rw-r--r--src/sensor.c88
-rw-r--r--src/zephyr.c184
7 files changed, 572 insertions, 697 deletions
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 <main.h>
-#include <zephyr.h>
-
-//#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 <main.h>
+
+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 <main.h>
-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 <main.h>
-#include <stdint.h>
#include <math.h>
-/**
- * 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 <main.h>
+
+/**
+ * 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 <main.h>
+
+/**
+ * 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 <zephyr.h>
-#include <main.h>
-
-#include <string.h>
-
-#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<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor) && mInUse[i] == id){
- if(speed > 127)speed = 127;
- if(speed < -127)speed = -127;
- 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;
-}
-
-void zMotorTake(const char *motor,unsigned int id){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor)){
- mInUse[i] = id;
- }
- }
-}
-void zMotorReturn(const char *motor){
- for(unsigned char i=0;i<MOTOR_PORT_COUNT;i++){
- if(!strcmp(MOTOR_PORT_MAP[i],motor)){
- mInUse[i] = 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;
-}
-
-bool zMotorIMEReset(const char *motor){
- for(unsigned char i=0;i<imeCount;i++){
- if(!strcmp(MOTOR_IME_MAP[i],motor)){
- return imeReset(i);
- }
- }
- return false;
-}
-
-#endif // IME_ENABLE
-
-void zDriveUpdate(void){
-
-#ifdef DRIVE_NORMAL
- char y = joystickGetAnalog(DRIVE_JOY,DRIVE_NORMAL);
- char x = joystickGetAnalog(DRIVE_JOY,4);
-
- APPLY_THRESH(x,DRIVE_THRESHOLD);
- APPLY_THRESH(y,DRIVE_THRESHOLD);
-
- zMotorSet("Left drive" ,y+x,0);
- zMotorSet("Right drive",y-x,0);
-
-#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("Left drive",l,0);
- zMotorSet("Right drive",r,0);
-
-#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;
-}
generated by cgit v1.2.3 (git 2.39.1) at 2025-04-05 08:31:02 +0000