James MacLean / Robot_Control

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?

UserRevisionLine numberNew 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 }