The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
main.cpp@0:80a37292f6b2, 2016-03-09 (annotated)
- Committer:
- JamesMacLean
- Date:
- Wed Mar 09 02:45:40 2016 +0000
- Revision:
- 0:80a37292f6b2
- Child:
- 1:cc5636894b95
The first revision
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JamesMacLean | 0:80a37292f6b2 | 1 | // Description: combines the functionality of both the PIcontrol program that controls a motor and the Pixy subsystem |
JamesMacLean | 0:80a37292f6b2 | 2 | // to create a program that will control the robot |
JamesMacLean | 0:80a37292f6b2 | 3 | |
JamesMacLean | 0:80a37292f6b2 | 4 | #include "rtos.h" |
JamesMacLean | 0:80a37292f6b2 | 5 | #include "mbed.h" |
JamesMacLean | 0:80a37292f6b2 | 6 | #include "Robot_Control.h" |
JamesMacLean | 0:80a37292f6b2 | 7 | #include "Pixy.h" |
JamesMacLean | 0:80a37292f6b2 | 8 | |
JamesMacLean | 0:80a37292f6b2 | 9 | #define MAX_BLOCKS 1 |
JamesMacLean | 0:80a37292f6b2 | 10 | #define TARGET 12 //the code of our target, very important, should this be octal...? |
JamesMacLean | 0:80a37292f6b2 | 11 | |
JamesMacLean | 0:80a37292f6b2 | 12 | //controls whether or not motors are a part of the program |
JamesMacLean | 0:80a37292f6b2 | 13 | //for our test of the PIXY, they will not be |
JamesMacLean | 0:80a37292f6b2 | 14 | #define MOTORS_ON 0 |
JamesMacLean | 0:80a37292f6b2 | 15 | |
JamesMacLean | 0:80a37292f6b2 | 16 | //steering inputs, may want to change to variables or something |
JamesMacLean | 0:80a37292f6b2 | 17 | #define STEER_SETPOINT 160 |
JamesMacLean | 0:80a37292f6b2 | 18 | #define STEER_KP 1 |
JamesMacLean | 0:80a37292f6b2 | 19 | #define STEER_KI 0 |
JamesMacLean | 0:80a37292f6b2 | 20 | #define STEER_MAX 2147483648 |
JamesMacLean | 0:80a37292f6b2 | 21 | |
JamesMacLean | 0:80a37292f6b2 | 22 | //speed inputs |
JamesMacLean | 0:80a37292f6b2 | 23 | #define HEIGHT_SETPOINT 160 |
JamesMacLean | 0:80a37292f6b2 | 24 | #define SPEED_KP 1 |
JamesMacLean | 0:80a37292f6b2 | 25 | #define SPEED_KI 0 |
JamesMacLean | 0:80a37292f6b2 | 26 | #define SPEED_MAX 2147483648 |
JamesMacLean | 0:80a37292f6b2 | 27 | |
JamesMacLean | 0:80a37292f6b2 | 28 | //shared variables between sensor thread and motor thread, protected by mutex |
JamesMacLean | 0:80a37292f6b2 | 29 | float left_setpoint, right_setpoint; |
JamesMacLean | 0:80a37292f6b2 | 30 | |
JamesMacLean | 0:80a37292f6b2 | 31 | //the setpoint mutex |
JamesMacLean | 0:80a37292f6b2 | 32 | osMutexId setpoint_mutex; |
JamesMacLean | 0:80a37292f6b2 | 33 | osMutexDef(setpoint_mutex); |
JamesMacLean | 0:80a37292f6b2 | 34 | |
JamesMacLean | 0:80a37292f6b2 | 35 | //motorPI function variables |
JamesMacLean | 0:80a37292f6b2 | 36 | float L_xState, R_xState; //the states for the left and right pi control loop (must have them outside of function to make them cumulative) |
JamesMacLean | 0:80a37292f6b2 | 37 | |
JamesMacLean | 0:80a37292f6b2 | 38 | //PI function variables |
JamesMacLean | 0:80a37292f6b2 | 39 | float steer_xState, speed_xState; //the states for the steering and speed pi control loop |
JamesMacLean | 0:80a37292f6b2 | 40 | |
JamesMacLean | 0:80a37292f6b2 | 41 | //main()variables |
JamesMacLean | 0:80a37292f6b2 | 42 | short idCode; |
JamesMacLean | 0:80a37292f6b2 | 43 | char isMeasuring; |
JamesMacLean | 0:80a37292f6b2 | 44 | char inKey; |
JamesMacLean | 0:80a37292f6b2 | 45 | float pwmPeriod = 0.001; |
JamesMacLean | 0:80a37292f6b2 | 46 | |
JamesMacLean | 0:80a37292f6b2 | 47 | //global variables |
JamesMacLean | 0:80a37292f6b2 | 48 | float Kp, Ki; //kp and ki for motors |
JamesMacLean | 0:80a37292f6b2 | 49 | |
JamesMacLean | 0:80a37292f6b2 | 50 | // Various debugging variables |
JamesMacLean | 0:80a37292f6b2 | 51 | float vels[60], times[60], errs[60], xvals[60], uvals[60]; // Used to measure step response |
JamesMacLean | 0:80a37292f6b2 | 52 | int x_global, height_global; |
JamesMacLean | 0:80a37292f6b2 | 53 | float speed_global, steer_global; |
JamesMacLean | 0:80a37292f6b2 | 54 | |
JamesMacLean | 0:80a37292f6b2 | 55 | // ******** Main Thread ******** |
JamesMacLean | 0:80a37292f6b2 | 56 | int main() { // This thread executes first upon reset or power-on. |
JamesMacLean | 0:80a37292f6b2 | 57 | |
JamesMacLean | 0:80a37292f6b2 | 58 | if(MOTORS_ON){ |
JamesMacLean | 0:80a37292f6b2 | 59 | // left |
JamesMacLean | 0:80a37292f6b2 | 60 | PWM0.period(pwmPeriod); |
JamesMacLean | 0:80a37292f6b2 | 61 | PWM0.pulsewidth(0); |
JamesMacLean | 0:80a37292f6b2 | 62 | Thread::wait(500); |
JamesMacLean | 0:80a37292f6b2 | 63 | |
JamesMacLean | 0:80a37292f6b2 | 64 | // right |
JamesMacLean | 0:80a37292f6b2 | 65 | PWM1.period(pwmPeriod); |
JamesMacLean | 0:80a37292f6b2 | 66 | PWM1.pulsewidth(0); |
JamesMacLean | 0:80a37292f6b2 | 67 | Thread::wait(500); |
JamesMacLean | 0:80a37292f6b2 | 68 | |
JamesMacLean | 0:80a37292f6b2 | 69 | //mutex for the left and right motor setpoints |
JamesMacLean | 0:80a37292f6b2 | 70 | setpoint_mutex = osMutexCreate(osMutex(setpoint_mutex)); |
JamesMacLean | 0:80a37292f6b2 | 71 | } |
JamesMacLean | 0:80a37292f6b2 | 72 | |
JamesMacLean | 0:80a37292f6b2 | 73 | Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the rising edge of Bumper |
JamesMacLean | 0:80a37292f6b2 | 74 | |
JamesMacLean | 0:80a37292f6b2 | 75 | // Start execution of the threads: PiControlThread,ExtCollisionThread, and SensorControlThread: |
JamesMacLean | 0:80a37292f6b2 | 76 | SensorControl = osThreadCreate(osThread(SensorControlThread), NULL); |
JamesMacLean | 0:80a37292f6b2 | 77 | if(MOTORS_ON) PiControl = osThreadCreate(osThread(PiControlThread), NULL); |
JamesMacLean | 0:80a37292f6b2 | 78 | ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL); |
JamesMacLean | 0:80a37292f6b2 | 79 | |
JamesMacLean | 0:80a37292f6b2 | 80 | // Start the watch dog timer and enable the watch dog interrupt |
JamesMacLean | 0:80a37292f6b2 | 81 | osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0); |
JamesMacLean | 0:80a37292f6b2 | 82 | led3=0; // Clear watch dog led3 status |
JamesMacLean | 0:80a37292f6b2 | 83 | |
JamesMacLean | 0:80a37292f6b2 | 84 | if(MOTORS_ON){ |
JamesMacLean | 0:80a37292f6b2 | 85 | //SPI Section |
JamesMacLean | 0:80a37292f6b2 | 86 | pc.printf("\n\rStarting SPI..."); |
JamesMacLean | 0:80a37292f6b2 | 87 | // Restart with a new SPI protocol. |
JamesMacLean | 0:80a37292f6b2 | 88 | DE0.format(16,1); // 16 bit mode 1 |
JamesMacLean | 0:80a37292f6b2 | 89 | IoReset = 0; |
JamesMacLean | 0:80a37292f6b2 | 90 | IoReset = 1; |
JamesMacLean | 0:80a37292f6b2 | 91 | wait_us(10); |
JamesMacLean | 0:80a37292f6b2 | 92 | IoReset = 0; |
JamesMacLean | 0:80a37292f6b2 | 93 | SpiReset = 0; |
JamesMacLean | 0:80a37292f6b2 | 94 | SpiReset = 1; |
JamesMacLean | 0:80a37292f6b2 | 95 | wait_us(10); |
JamesMacLean | 0:80a37292f6b2 | 96 | SpiReset = 0; |
JamesMacLean | 0:80a37292f6b2 | 97 | idCode = DE0.write(0x8004); //changed from 8002 for one motor to 8004 for two |
JamesMacLean | 0:80a37292f6b2 | 98 | pc.printf("\n\rDevice Id: %d", idCode); |
JamesMacLean | 0:80a37292f6b2 | 99 | |
JamesMacLean | 0:80a37292f6b2 | 100 | // motor Kp and Ki |
JamesMacLean | 0:80a37292f6b2 | 101 | Kp = 0.000120; |
JamesMacLean | 0:80a37292f6b2 | 102 | Ki = 0.0000000001; |
JamesMacLean | 0:80a37292f6b2 | 103 | } |
JamesMacLean | 0:80a37292f6b2 | 104 | |
JamesMacLean | 0:80a37292f6b2 | 105 | // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval |
JamesMacLean | 0:80a37292f6b2 | 106 | // in seconds between interrupts, and start interrupt generation: |
JamesMacLean | 0:80a37292f6b2 | 107 | if(MOTORS_ON) PeriodicInt.attach(&PiControllerISR, .001); |
JamesMacLean | 0:80a37292f6b2 | 108 | PeriodicInt2.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec |
JamesMacLean | 0:80a37292f6b2 | 109 | |
JamesMacLean | 0:80a37292f6b2 | 110 | //isMeasuring = 1; |
JamesMacLean | 0:80a37292f6b2 | 111 | //Setpoint = 6; |
JamesMacLean | 0:80a37292f6b2 | 112 | |
JamesMacLean | 0:80a37292f6b2 | 113 | while (1) { |
JamesMacLean | 0:80a37292f6b2 | 114 | |
JamesMacLean | 0:80a37292f6b2 | 115 | osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
JamesMacLean | 0:80a37292f6b2 | 116 | |
JamesMacLean | 0:80a37292f6b2 | 117 | if(!MOTORS_ON){ |
JamesMacLean | 0:80a37292f6b2 | 118 | pc.printf("X Coordinate: %d, Steering: %d,", x_global, steer_global); |
JamesMacLean | 0:80a37292f6b2 | 119 | pc.printf("Height: %d, Speed: %d,", height_global, speed_global); |
JamesMacLean | 0:80a37292f6b2 | 120 | pc.printf(" L setpoint: %d, R setpoint: %d \n\r", left_setpoint, right_setpoint); |
JamesMacLean | 0:80a37292f6b2 | 121 | } |
JamesMacLean | 0:80a37292f6b2 | 122 | /* |
JamesMacLean | 0:80a37292f6b2 | 123 | if (!isMeasuring && (Setpoint!=0)) { |
JamesMacLean | 0:80a37292f6b2 | 124 | pc.printf("\n\r-----------------------------------\n\r"); |
JamesMacLean | 0:80a37292f6b2 | 125 | printf("times (ms) \t vel (rads/s) \t error \t\t x value \t u value \n\r"); |
JamesMacLean | 0:80a37292f6b2 | 126 | for (i=0; i<60; i++) { |
JamesMacLean | 0:80a37292f6b2 | 127 | printf("%f, \t %f, \t %f, \t %f, \t %f \n\r", times[i], vels[i], errs[i], xvals[i], uvals[i]); |
JamesMacLean | 0:80a37292f6b2 | 128 | } |
JamesMacLean | 0:80a37292f6b2 | 129 | pc.printf("\n\r-----------------------------------\n\r"); |
JamesMacLean | 0:80a37292f6b2 | 130 | Setpoint = 0; |
JamesMacLean | 0:80a37292f6b2 | 131 | i = 0; |
JamesMacLean | 0:80a37292f6b2 | 132 | } |
JamesMacLean | 0:80a37292f6b2 | 133 | */ |
JamesMacLean | 0:80a37292f6b2 | 134 | Thread::wait(500); // Go to sleep for 500 ms |
JamesMacLean | 0:80a37292f6b2 | 135 | } |
JamesMacLean | 0:80a37292f6b2 | 136 | } |
JamesMacLean | 0:80a37292f6b2 | 137 | |
JamesMacLean | 0:80a37292f6b2 | 138 | // ******** Control Thread ******** |
JamesMacLean | 0:80a37292f6b2 | 139 | void SensorControlThread(void const *argument) { |
JamesMacLean | 0:80a37292f6b2 | 140 | float speed, steer; |
JamesMacLean | 0:80a37292f6b2 | 141 | int x, count, height; |
JamesMacLean | 0:80a37292f6b2 | 142 | |
JamesMacLean | 0:80a37292f6b2 | 143 | Pixy pixy(Pixy::SPI, p11, p12, p13); |
JamesMacLean | 0:80a37292f6b2 | 144 | |
JamesMacLean | 0:80a37292f6b2 | 145 | while (1) { |
JamesMacLean | 0:80a37292f6b2 | 146 | osSignalWait(SignalSensor, osWaitForever); //note: SignalSensor is just zero, waits for any signal dirrected at thread. |
JamesMacLean | 0:80a37292f6b2 | 147 | |
JamesMacLean | 0:80a37292f6b2 | 148 | count = pixy.getBlocks(130); //get as many blocks as are available |
JamesMacLean | 0:80a37292f6b2 | 149 | |
JamesMacLean | 0:80a37292f6b2 | 150 | //find the target block in all of the blocks that were sent |
JamesMacLean | 0:80a37292f6b2 | 151 | for(x=0;x<count;x++){ |
JamesMacLean | 0:80a37292f6b2 | 152 | if (pixy.blocks[x].signature == TARGET) break; |
JamesMacLean | 0:80a37292f6b2 | 153 | if (x==count) pc.printf("no target in frame"); |
JamesMacLean | 0:80a37292f6b2 | 154 | } |
JamesMacLean | 0:80a37292f6b2 | 155 | |
JamesMacLean | 0:80a37292f6b2 | 156 | // if blocks were returned |
JamesMacLean | 0:80a37292f6b2 | 157 | if (count){ |
JamesMacLean | 0:80a37292f6b2 | 158 | |
JamesMacLean | 0:80a37292f6b2 | 159 | //height = CalcHeight() //find the feedback from the pixy for the height |
JamesMacLean | 0:80a37292f6b2 | 160 | height = pixy.blocks[(x-1)].height; //use this for now |
JamesMacLean | 0:80a37292f6b2 | 161 | |
JamesMacLean | 0:80a37292f6b2 | 162 | steer = PI(0, STEER_SETPOINT, STEER_KP, STEER_KI, pixy.blocks[(x-1)].x, STEER_MAX); //the feedback for the steering is the x location of the target block |
JamesMacLean | 0:80a37292f6b2 | 163 | speed = PI(1, HEIGHT_SETPOINT, SPEED_KP, SPEED_KI, height, SPEED_MAX); |
JamesMacLean | 0:80a37292f6b2 | 164 | |
JamesMacLean | 0:80a37292f6b2 | 165 | if(!MOTORS_ON){ |
JamesMacLean | 0:80a37292f6b2 | 166 | x_global = pixy.blocks[x-1].x; |
JamesMacLean | 0:80a37292f6b2 | 167 | height_global = height; |
JamesMacLean | 0:80a37292f6b2 | 168 | steer_global = steer; |
JamesMacLean | 0:80a37292f6b2 | 169 | speed_global = speed; |
JamesMacLean | 0:80a37292f6b2 | 170 | } |
JamesMacLean | 0:80a37292f6b2 | 171 | |
JamesMacLean | 0:80a37292f6b2 | 172 | // give setpoints to PiControlThread |
JamesMacLean | 0:80a37292f6b2 | 173 | osMutexWait(setpoint_mutex, osWaitForever); //enter mutex |
JamesMacLean | 0:80a37292f6b2 | 174 | left_setpoint = speed - steer; |
JamesMacLean | 0:80a37292f6b2 | 175 | right_setpoint = speed + steer; |
JamesMacLean | 0:80a37292f6b2 | 176 | osMutexRelease(setpoint_mutex); //exit mutex |
JamesMacLean | 0:80a37292f6b2 | 177 | } |
JamesMacLean | 0:80a37292f6b2 | 178 | |
JamesMacLean | 0:80a37292f6b2 | 179 | } |
JamesMacLean | 0:80a37292f6b2 | 180 | } |
JamesMacLean | 0:80a37292f6b2 | 181 | |
JamesMacLean | 0:80a37292f6b2 | 182 | // ******** Control Thread ******** |
JamesMacLean | 0:80a37292f6b2 | 183 | void PiControlThread(void const *argument) { |
JamesMacLean | 0:80a37292f6b2 | 184 | short dP_L, dt_L, dP_R, dt_R; |
JamesMacLean | 0:80a37292f6b2 | 185 | float uL,uR; |
JamesMacLean | 0:80a37292f6b2 | 186 | float left_set,right_set; |
JamesMacLean | 0:80a37292f6b2 | 187 | |
JamesMacLean | 0:80a37292f6b2 | 188 | while (1) { |
JamesMacLean | 0:80a37292f6b2 | 189 | osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received. SignalPi is just zero lol, waits for any signal to thread |
JamesMacLean | 0:80a37292f6b2 | 190 | led2= !led2; // Alive status - led2 toggles each time PiControlThread is signaled. |
JamesMacLean | 0:80a37292f6b2 | 191 | |
JamesMacLean | 0:80a37292f6b2 | 192 | dP_L = DE0.write(0); // Read QEI-0 position register |
JamesMacLean | 0:80a37292f6b2 | 193 | dt_L = DE0.write(0); // Read QEI-0 time register |
JamesMacLean | 0:80a37292f6b2 | 194 | dP_R = DE0.write(0); // Read QEI-0 position register |
JamesMacLean | 0:80a37292f6b2 | 195 | dt_R = DE0.write(0); // Read QEI-0 time register |
JamesMacLean | 0:80a37292f6b2 | 196 | |
JamesMacLean | 0:80a37292f6b2 | 197 | osMutexWait(setpoint_mutex, osWaitForever);//enter mutex |
JamesMacLean | 0:80a37292f6b2 | 198 | left_set= left_setpoint; //copy setpoint |
JamesMacLean | 0:80a37292f6b2 | 199 | right_set= right_setpoint; |
JamesMacLean | 0:80a37292f6b2 | 200 | osMutexRelease(setpoint_mutex);//exit mutex |
JamesMacLean | 0:80a37292f6b2 | 201 | |
JamesMacLean | 0:80a37292f6b2 | 202 | uL = motorPI(0, left_set, Kp, Ki, dP_L, dt_L); |
JamesMacLean | 0:80a37292f6b2 | 203 | uR = motorPI(1, right_set, Kp, Ki, dP_R, dt_R); |
JamesMacLean | 0:80a37292f6b2 | 204 | |
JamesMacLean | 0:80a37292f6b2 | 205 | PWM0.pulsewidth(fabs(uL)); |
JamesMacLean | 0:80a37292f6b2 | 206 | PWM1.pulsewidth(fabs(uR)); |
JamesMacLean | 0:80a37292f6b2 | 207 | } |
JamesMacLean | 0:80a37292f6b2 | 208 | } |
JamesMacLean | 0:80a37292f6b2 | 209 | |
JamesMacLean | 0:80a37292f6b2 | 210 | // ******** Collision Thread ******** |
JamesMacLean | 0:80a37292f6b2 | 211 | void ExtCollisionThread(void const *argument) { |
JamesMacLean | 0:80a37292f6b2 | 212 | while (1) { |
JamesMacLean | 0:80a37292f6b2 | 213 | osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received |
JamesMacLean | 0:80a37292f6b2 | 214 | led4 = 1; |
JamesMacLean | 0:80a37292f6b2 | 215 | } |
JamesMacLean | 0:80a37292f6b2 | 216 | } |
JamesMacLean | 0:80a37292f6b2 | 217 | |
JamesMacLean | 0:80a37292f6b2 | 218 | // ******** Watchdog Interrupt Handler ******** |
JamesMacLean | 0:80a37292f6b2 | 219 | void Watchdog(void const *n) { |
JamesMacLean | 0:80a37292f6b2 | 220 | led3=1; // led3 is activated when the watchdog timer times out |
JamesMacLean | 0:80a37292f6b2 | 221 | } |
JamesMacLean | 0:80a37292f6b2 | 222 | |
JamesMacLean | 0:80a37292f6b2 | 223 | // ******** Period Timer Interrupt Handler ******** |
JamesMacLean | 0:80a37292f6b2 | 224 | void PiControllerISR(void) { |
JamesMacLean | 0:80a37292f6b2 | 225 | osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. |
JamesMacLean | 0:80a37292f6b2 | 226 | } |
JamesMacLean | 0:80a37292f6b2 | 227 | |
JamesMacLean | 0:80a37292f6b2 | 228 | // ******** Period Timer Interrupt Handler ******** |
JamesMacLean | 0:80a37292f6b2 | 229 | void SensorControllerISR(void) { |
JamesMacLean | 0:80a37292f6b2 | 230 | osSignalSet(SensorControl,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt. (basically sends signal to id of thread) |
JamesMacLean | 0:80a37292f6b2 | 231 | } |
JamesMacLean | 0:80a37292f6b2 | 232 | |
JamesMacLean | 0:80a37292f6b2 | 233 | // ******** Collision Interrupt Handler ******** |
JamesMacLean | 0:80a37292f6b2 | 234 | void ExtCollisionISR(void) { |
JamesMacLean | 0:80a37292f6b2 | 235 | osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt. |
JamesMacLean | 0:80a37292f6b2 | 236 | } |
JamesMacLean | 0:80a37292f6b2 | 237 | |
JamesMacLean | 0:80a37292f6b2 | 238 | // motorPI - motor pi control extacted from previous work |
JamesMacLean | 0:80a37292f6b2 | 239 | // Input: 0 for Left motor, 1 for right motor |
JamesMacLean | 0:80a37292f6b2 | 240 | // Output: pwm pulsewidth for corresponding motor |
JamesMacLean | 0:80a37292f6b2 | 241 | |
JamesMacLean | 0:80a37292f6b2 | 242 | float motorPI(int RL_Motor, float Setpoint, float Kp, float Ki, short dP1, short dt1){ |
JamesMacLean | 0:80a37292f6b2 | 243 | int dP32, i; |
JamesMacLean | 0:80a37292f6b2 | 244 | float angVel, e, u, xState; |
JamesMacLean | 0:80a37292f6b2 | 245 | |
JamesMacLean | 0:80a37292f6b2 | 246 | if (dP1 & 0x00008000) { |
JamesMacLean | 0:80a37292f6b2 | 247 | dP32 = dP1 | 0xFFFF0000; |
JamesMacLean | 0:80a37292f6b2 | 248 | } else { |
JamesMacLean | 0:80a37292f6b2 | 249 | dP32 = dP1; |
JamesMacLean | 0:80a37292f6b2 | 250 | } |
JamesMacLean | 0:80a37292f6b2 | 251 | |
JamesMacLean | 0:80a37292f6b2 | 252 | angVel = QE2RadsPerSec(dP32, dt1); |
JamesMacLean | 0:80a37292f6b2 | 253 | |
JamesMacLean | 0:80a37292f6b2 | 254 | e = Setpoint - angVel; |
JamesMacLean | 0:80a37292f6b2 | 255 | |
JamesMacLean | 0:80a37292f6b2 | 256 | RL_Motor ? xState = R_xState : xState = L_xState; //state is cumulative, select left or right state global variable |
JamesMacLean | 0:80a37292f6b2 | 257 | |
JamesMacLean | 0:80a37292f6b2 | 258 | // Avoid integrator wind up |
JamesMacLean | 0:80a37292f6b2 | 259 | if((u>=pwmPeriod)||(u<=-pwmPeriod)){ |
JamesMacLean | 0:80a37292f6b2 | 260 | //xState= xState; - turns off integrator when u maxed |
JamesMacLean | 0:80a37292f6b2 | 261 | }else{ |
JamesMacLean | 0:80a37292f6b2 | 262 | xState = xState + e; |
JamesMacLean | 0:80a37292f6b2 | 263 | } |
JamesMacLean | 0:80a37292f6b2 | 264 | |
JamesMacLean | 0:80a37292f6b2 | 265 | RL_Motor ? R_xState = xState : L_xState = xState; //update state variable |
JamesMacLean | 0:80a37292f6b2 | 266 | |
JamesMacLean | 0:80a37292f6b2 | 267 | u = Ki*xState + Kp*e; |
JamesMacLean | 0:80a37292f6b2 | 268 | |
JamesMacLean | 0:80a37292f6b2 | 269 | // Set direction and pwm |
JamesMacLean | 0:80a37292f6b2 | 270 | if (u >= 0) { |
JamesMacLean | 0:80a37292f6b2 | 271 | RL_Motor ? DIR1 = 1 : DIR0 = 1; |
JamesMacLean | 0:80a37292f6b2 | 272 | } else { |
JamesMacLean | 0:80a37292f6b2 | 273 | RL_Motor ? DIR1 = 0 : DIR0 = 0; |
JamesMacLean | 0:80a37292f6b2 | 274 | } |
JamesMacLean | 0:80a37292f6b2 | 275 | |
JamesMacLean | 0:80a37292f6b2 | 276 | if (isMeasuring) { |
JamesMacLean | 0:80a37292f6b2 | 277 | vels[i] = angVel; |
JamesMacLean | 0:80a37292f6b2 | 278 | times[i] = i; |
JamesMacLean | 0:80a37292f6b2 | 279 | errs[i] = e; |
JamesMacLean | 0:80a37292f6b2 | 280 | xvals[i] = xState; |
JamesMacLean | 0:80a37292f6b2 | 281 | uvals[i] = u; |
JamesMacLean | 0:80a37292f6b2 | 282 | if (i>=59) { |
JamesMacLean | 0:80a37292f6b2 | 283 | isMeasuring = 0; |
JamesMacLean | 0:80a37292f6b2 | 284 | } |
JamesMacLean | 0:80a37292f6b2 | 285 | i++; |
JamesMacLean | 0:80a37292f6b2 | 286 | } |
JamesMacLean | 0:80a37292f6b2 | 287 | |
JamesMacLean | 0:80a37292f6b2 | 288 | if (fabs(u) > pwmPeriod) u = pwmPeriod; |
JamesMacLean | 0:80a37292f6b2 | 289 | |
JamesMacLean | 0:80a37292f6b2 | 290 | return u; |
JamesMacLean | 0:80a37292f6b2 | 291 | } |
JamesMacLean | 0:80a37292f6b2 | 292 | |
JamesMacLean | 0:80a37292f6b2 | 293 | // PI() - A generic PI controller function used by the sensor control thread |
JamesMacLean | 0:80a37292f6b2 | 294 | // st_or_sp - 0 for steering, 1 for speed |
JamesMacLean | 0:80a37292f6b2 | 295 | float PI(int st_or_sp, float setpoint, float Kp, float Ki, int feedback, float max_val){ |
JamesMacLean | 0:80a37292f6b2 | 296 | float e, xState, u; |
JamesMacLean | 0:80a37292f6b2 | 297 | |
JamesMacLean | 0:80a37292f6b2 | 298 | e = setpoint - (float)feedback; |
JamesMacLean | 0:80a37292f6b2 | 299 | |
JamesMacLean | 0:80a37292f6b2 | 300 | st_or_sp ? xState = speed_xState : xState = steer_xState; |
JamesMacLean | 0:80a37292f6b2 | 301 | |
JamesMacLean | 0:80a37292f6b2 | 302 | // Avoid integrator wind up |
JamesMacLean | 0:80a37292f6b2 | 303 | if((u>=max_val)||(u<=-max_val)){ |
JamesMacLean | 0:80a37292f6b2 | 304 | //xState= xState; - turns off integrator when u maxed |
JamesMacLean | 0:80a37292f6b2 | 305 | }else{ |
JamesMacLean | 0:80a37292f6b2 | 306 | xState = xState + e; |
JamesMacLean | 0:80a37292f6b2 | 307 | } |
JamesMacLean | 0:80a37292f6b2 | 308 | |
JamesMacLean | 0:80a37292f6b2 | 309 | st_or_sp ? speed_xState = xState : steer_xState = xState; //update state variable |
JamesMacLean | 0:80a37292f6b2 | 310 | |
JamesMacLean | 0:80a37292f6b2 | 311 | u = Ki*xState + Kp*e; |
JamesMacLean | 0:80a37292f6b2 | 312 | |
JamesMacLean | 0:80a37292f6b2 | 313 | if (fabs(u) > max_val) u = max_val; |
JamesMacLean | 0:80a37292f6b2 | 314 | |
JamesMacLean | 0:80a37292f6b2 | 315 | return u; |
JamesMacLean | 0:80a37292f6b2 | 316 | } |
JamesMacLean | 0:80a37292f6b2 | 317 | |
JamesMacLean | 0:80a37292f6b2 | 318 | float QE2RadsPerSec(int counts, int time) { |
JamesMacLean | 0:80a37292f6b2 | 319 | return (counts*122.62)/time; |
JamesMacLean | 0:80a37292f6b2 | 320 | } |