From b2deec192837840a8f36d19ceeb99044db171ebf Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Sat, 20 Feb 2016 11:19:47 -0500 Subject: it works? --- firmware/libccos.a | Bin 90392 -> 90342 bytes include/arm.h | 55 ++++++++++++++++++++ include/main.h | 150 ++++++++++++++++++++++++++++++++++++++++++----------- src/init.c | 3 ++ src/opcontrol.c | 99 +++++++++++++++++++++++------------ 5 files changed, 246 insertions(+), 61 deletions(-) create mode 100644 include/arm.h diff --git a/firmware/libccos.a b/firmware/libccos.a index 469b330..d79a629 100644 Binary files a/firmware/libccos.a and b/firmware/libccos.a differ diff --git a/include/arm.h b/include/arm.h new file mode 100644 index 0000000..51a485b --- /dev/null +++ b/include/arm.h @@ -0,0 +1,55 @@ +#ifndef INCLUDE_ARM_H_ +#define INCLUDE_ARM_H_ + +#include + +typedef void (*funcptr_t)(void); + +volatile struct _ExceptionTable { + funcptr_t unknown1; + funcptr_t entry; + funcptr_t NMIException; + funcptr_t HardFaultException; + funcptr_t MemManageException; + funcptr_t BusFaultException; + funcptr_t UsageFaultException; + funcptr_t unknown2[4]; + funcptr_t SVCHandler; + funcptr_t DebugMonitor; + funcptr_t unknown3; + funcptr_t PendSVC; + funcptr_t SysTickHandler; + funcptr_t InterruptHandler[]; +} __attribute__ ((packed)) *ExceptionTable; + +volatile struct _SystemControlBlock { + uint32_t CPUID; + uint32_t ICSR; + uint32_t VTOR; + uint32_t AIRCR; + uint32_t SCR; + uint32_t CCR; + uint32_t SHPR[3]; + uint32_t SHCRS; + uint8_t MMSR; + uint8_t BFSR; + uint16_t UFSR; + uint32_t HFSR; + uint32_t unused1; + uint32_t MMAR; + uint32_t BFAR; + uint32_t AFSR; +} __attribute__ ((packed)) *SystemControlBlock = (struct _SystemControlBlock *)0xE000ED00; + +/** + * "Allow" divide by zero instructions to be run. + * + * By setting this bit in the CCR the Cortex will handle divide by zero + * instructions by returning a quotient of 0. + */ + +void handleDiv0(void){ + SystemControlBlock->CCR |= (1<<4); +} + +#endif // INCLUDE_ARM_H_ diff --git a/include/main.h b/include/main.h index d0ee41b..d680b2b 100644 --- a/include/main.h +++ b/include/main.h @@ -4,67 +4,152 @@ #include #include +/** + * A two-dimensional vector structure. + * + * This structure contains a 2D coordinate for organized storage. + */ + typedef struct{ - int x; - int y; -}vec2; + int x; /**< The 'x' coordinate */ + int y; /**< The 'y' coordinate */ +} vec2; + +/** + * An enum for storing a button's position. + * + * The controller subsystem allows buttons to generate three signals. These are + * the expected pressed and unpressed states, named DOWN and UP respectively, + * along with a third signal called KEY_UP that is sent out upon the immediate + * release of a button. This allows for one-time events to be called on button + * releases. + */ typedef enum { - UP, - DOWN, - KEY_UP + UP, /**< The unpressed state */ + DOWN, /**< The pressed state */ + KEY_UP /**< The 'just-released' state */ } Button; +/** + * A structure for storing all input states of a single controller. + * + * This object groups a joystick's axes and buttons in a convenient manner, + * with simple function calls to poll inputs and update them. + */ + typedef struct { - unsigned int num; + unsigned int num; /**< The joystick's number as seen by PROS */ + + /** + * The 'Side' structures separate the left and right halves of the + * controller. + */ + struct Side { + + /** + * The 'Group' structure groups the buttons as written on the + * controller. + */ + struct Group { - Button l; - Button r; - Button u; - Button d; + Button l; /**< The left button, if it exists */ + Button r; /**< The right button, if it exists */ + Button u; /**< The up button */ + Button d; /**< The down button */ } front, back; - vec2 stick; + + vec2 stick; /**< The joystick's position, stored as a 2D coordinate. */ + } left, right; + } Controller; +/** + * A structure for handling most robot sensors. + * + * This structure contains the information necessary to handle most of the + * sensors provided by VEX, and allows for easier sensor reading and handling. + */ + typedef struct { + + /** + * A 'type' enum for recognizing the sensor's type. + */ + enum type { - DIGITAL, - ANALOG, - GYRO, - ULTRASONIC + DIGITAL, /**< A generic digital sensor */ + ANALOG, /**< A generic analog sensor */ + GYRO, /**< A gyroscore (BROKEN) */ + ULTRASONIC /**< An ultrasonic sensor */ } type; + + /** + * A special 'data' item for sensors that need extra variables. + */ + union data { - Gyro gyro; - Ultrasonic sonic; + Gyro gyro; /**< The gyroscope object provided by PROS */ + Ultrasonic sonic; /**< The ultrasonic object provided by PROS */ } data; - unsigned int port; - int value; - int initial; + + unsigned int port; /**< The port that the sensor is connected to */ + int value; /**< The most recent value of the sensor */ + int initial; /**< The initial reading from the sensor */ } Sensor; +/** + * A structure for tracking processes. + * + * This object contains most information about a task, as well as flags for + * controlling the task's state. + */ + typedef struct { - bool kill_req; - bool exiting; - TaskCode code; - TaskHandle handle; - void *param; + bool kill_req; /**< A kill request signal for ending the process */ + TaskCode code; /**< The function that is the task */ + TaskHandle handle; /**< The task handle provided by PROS */ + void *param; /**< The argument provided for the task */ } Process; #ifdef __cplusplus extern "C" { #endif -#define DEFAULT_TRPM trpm = 1850; -#define EXTRA_TRPM trpm = 1900; +/** + * A macro for setting the target RPM to it's default value. + */ + +#define DEFAULT_TRPM trpm = 1650; + +/** + * A macro for setting the target RPM to a little higher than usually + * necessary. + */ + +#define EXTRA_TRPM trpm = 1650; + +/** + * The mathematical constant 'pi', for trig/angular calculations. + */ #define PI 3.14159265L +/** + * The port that an LCD is expected to be plugged into. + */ + #define LCD_PORT uart2 /** - * Be sure that getIMEPort() matches these values (see sensor.c). + * An enum for the set of motor ports on the Cortex, for tagging what motors + * are. + * + * The getIMEPort() function defined in sensor.c takes these values for + * accessing their IME counterparts, so insure that this definition matches + * what getIMEPort() expects. */ enum MOTOR_MAP { @@ -80,6 +165,13 @@ enum MOTOR_MAP { LIFT_ROTATER }; +/** + * An enum for tagging IMEs. + * + * getIMEPort() will allow access to these values by referencing their + * MOTOR_MAP counterparts, so hard-coding these shouldn't be necessary. + */ + enum MOTOR_IME_MAP { DRIVE_RIGHT_IME = 0, DRIVE_LEFT_IME, diff --git a/src/init.c b/src/init.c index f8154d7..704b199 100644 --- a/src/init.c +++ b/src/init.c @@ -16,6 +16,9 @@ void initializeIO(void){ intakeLiftBase = initUltrasonic(1,2); intakeLiftTop = initSensor(8,ANALOG); //robotGyro = initSensor(2,GYRO); + + pinMode(12,OUTPUT); + digitalWrite(12,1); } void initialize(void){ diff --git a/src/opcontrol.c b/src/opcontrol.c index 1efcb10..a549e57 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -1,7 +1,12 @@ #include #include -#define AUTO_SKILLS +#include + +#define PUSH_UP 500 +#define PUSH_BACK 500 + +//#define AUTO_SKILLS extern Sensor intakeFrontLeft, intakeFrontRight, @@ -25,7 +30,7 @@ static float xpos = 0, static double cangle = 0; -static double rpm = 0, trpm = 1850, arpm = 0; +static double rpm = 0, trpm = 1650, arpm = 0; static bool cannReady = false; static Controller c[2]; @@ -40,9 +45,23 @@ void taskArmCode(void *); // 100ms void taskAimCode(void *); // 100ms void taskLCDCode(void *); // 500ms +static unsigned int cid = 0; + + +const char *dream = "CarryOnM:d=8,o=6,b=125:a5,c,d,c,b5,a5,4a5,a5,g5,4a5,4b5,4p,a5,c,d,c,b5,a5,4e,d,c,4c,4d,4p,f,f,e,d,d,c,4e,2d,4p,f,f,4e,d,c,2g5,4p,a5,c,d,c,b5,a5,4a5,a5,g5,4a5,4b5,4p,a5,c,d,c,b5,a5,4e,d,c,4c,4d,4p,f,f,e,d,d,c,4e,2d,4p,f,f,4e,4g,2g"; +const char *final = "FinalCou:d=4,o=5,b=140:16c#6,32b,32p,8c#.6,16p,f#,p.,32d6,32p,32c#6,32p,32d6,16p.,32c#6,16p.,b.,p,32d6,32p,32c#6,32p,d6,f#,p.,32b,32p,32a,32p,32b,16p.,32a,16p.,32g#,16p.,32b,16p.,a.,32c#6,32p,32b,32p,c#6,2f#,p,16p,32d6,32p,32c#6,32p,32d6,16p.,32c#6,16p.,b.,p,32d6,32p,32c#6,32p,d6,f#,p.,32b,32p,32a,32p,32b,16p.,32a,16p.,32g#,16p.,32b,16p.,2a,16p,32g#,32p,32a,32p,b.,16a,16b,8c#6,8b,8a,8g#,f#,d6,1c#6,8p,16c#6,16d6,16c#6,16b,2c#.6,16p"; + + +void cdelay(unsigned int id,unsigned int ms){ + cid = id; + delay(ms); +} + void operatorControl(void){ static bool invert; + handleDiv0(); + DEFAULT_TRPM; c[0].num = 1; c[1].num = 2; @@ -70,9 +89,6 @@ void operatorControl(void){ taskInit(taskLCD,NULL); taskInit(taskPos,NULL); - if(taskCan) - taskDelete(taskCan); - while(1){ /** @@ -104,6 +120,17 @@ void operatorControl(void){ onKeyUp(c[0].right.front.r) invert ^= true; onKeyUp(c[1].right.front.r) invert ^= true; + if(keyUp(c[0].left.front.r)){ + speakerInit(); + speakerPlayRtttl(final); + speakerShutdown(); + } + if(keyUp(c[0].left.front.l)){ + speakerInit(); + speakerPlayRtttl(dream); + speakerShutdown(); + } + motorSetN(DRIVE_LEFT ,c[0].left.stick.y); motorSetN(DRIVE_RIGHT,c[0].right.stick.y); @@ -118,6 +145,8 @@ void operatorControl(void){ motorSetN(LIFT_ROTATER,-c[1].right.stick.x / 4); + //motorSetBN(LIFT_PUSHER,127,c[1].left.front); + delay(50); } } @@ -140,7 +169,8 @@ void taskLiftCode(void *unused){ motorSetK(INTAKE_2,127,1); do{ - turned = (cangle > 30) | (cangle < -30); + + turned = (cangle > 40) | (cangle < -40); loaded = underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT); /* @@ -205,6 +235,7 @@ void taskAimCode(void *unused){ target = cangle; do{ + if(cangle > target){ motorSetK(LIFT_ROTATER,30,4); }else if(cangle < target){ @@ -336,13 +367,13 @@ void taskCanCode(void *unused){ */ if(xpos < 20) - trpm = 1850; + trpm = 1650; else if(xpos < 40) - trpm = 1750; + trpm = 1550; else if(xpos < 60) - trpm = 1650; + trpm = 1450; else - trpm = 1550; + trpm = 1350; trpm += arpm; @@ -353,7 +384,7 @@ void taskCanCode(void *unused){ */ if(ca < trpm - 40){ - speed += 2 * ((trpm - rpm) / 60); + speed += 2; motorSetK(CANNON_LEFT,-speed,2); motorSetK(CANNON_RIGHT,speed,2); cannReady = false; @@ -368,6 +399,7 @@ void taskCanCode(void *unused){ delay(100); } + motorSetK(CANNON_LEFT,0,2); motorSetK(CANNON_RIGHT,0,2); @@ -381,8 +413,8 @@ void taskCanCode(void *unused){ } void taskArmCode(void *unused){ - if(keyDown(c[1].left.front.r)) - goto PUSH; + /*if(keyDown(c[1].left.front.r)) + goto PUSH;*/ while(!underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT) && ((rpm < trpm - 30) | (rpm > trpm + 50))) delay(100); @@ -390,14 +422,14 @@ void taskArmCode(void *unused){ /*while(ballPos != 5) delay(100);*/ -PUSH: +//PUSH: motorTake(LIFT_PUSHER,3); motorSetK(LIFT_PUSHER,127,3); - delay(500); + delay(PUSH_UP); motorSetK(LIFT_PUSHER,-127,3); - delay(800); + delay(PUSH_BACK); motorSetK(LIFT_PUSHER,0,3); motorFree(LIFT_PUSHER); @@ -476,9 +508,9 @@ void autonomous(){ delay(500); motorSet(LIFT_PUSHER,127); - delay(500); + delay(PUSH_UP); motorSet(LIFT_PUSHER,-127); - delay(800); + delay(PUSH_BACK); motorSet(LIFT_PUSHER,0); lcdPrint(LCD_PORT,2," "); @@ -495,11 +527,12 @@ void autonomous(){ *****************************************************************************/ void autonomous(){ - static unsigned long start,elapsed = 0; - static unsigned long inc = 0; + //static unsigned long start,elapsed = 0; + static unsigned int inc = 0; + static bool onetime = false; EXTRA_TRPM; - start = millis(); + //start = millis(); intakeLiftTop.initial = readSensor(&intakeLiftTop); taskInit(taskCan,&No); @@ -510,39 +543,41 @@ void autonomous(){ //motorSet(INTAKE_2,127); while(1){ - elapsed = millis() - start; + //elapsed = millis() - start; 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); + lcdPrint(LCD_PORT,1,"%.0lf/%.0lf",rpm,trpm); } readSensor(&intakeLiftTop); if(underSensor(intakeLiftTop,LIGHT_THRESH_DEFAULT)){ - - delay(200); - motorSet(LIFT_1,0); - motorSet(LIFT_2,0); + if(!onetime){ + delay(400); + motorSet(LIFT_1,0); + motorSet(LIFT_2,0); + onetime = true; + } //motorSet(INTAKE_1,0); //motorSet(INTAKE_2,0); - if(rpm >= trpm){ + if(rpm >= trpm - 50){ motorSet(LIFT_PUSHER,127); - delay(500); + delay(PUSH_UP); motorSet(LIFT_PUSHER,-127); - delay(500); + delay(PUSH_BACK); motorSet(LIFT_PUSHER,0); motorSet(LIFT_1,127); motorSet(LIFT_2,127); //motorSet(INTAKE_1,127); //motorSet(INTAKE_2,127); + onetime = false; } } - delay(10); + delay(100); } } -- cgit v1.2.3