James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

main.cpp@1:cc5636894b95, 2016-03-13 (annotated)

Committer:
balsamfir
Date:
Sun Mar 13 15:22:39 2016 +0000
Revision:
1:cc5636894b95
Parent:
0:80a37292f6b2
Child:
2:2bc519e14bae
Cleanup before change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
balsamfir 1:cc5636894b95 1 // Description: combines the functionality of both the MotorControl 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
balsamfir 1:cc5636894b95 10 #define TARGET_DECIMAL 0x10 //the code of our target in decimal
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
JamesMacLean 0:80a37292f6b2 63 // right
JamesMacLean 0:80a37292f6b2 64 PWM1.period(pwmPeriod);
JamesMacLean 0:80a37292f6b2 65 PWM1.pulsewidth(0);
JamesMacLean 0:80a37292f6b2 66
JamesMacLean 0:80a37292f6b2 67 //mutex for the left and right motor setpoints
JamesMacLean 0:80a37292f6b2 68 setpoint_mutex = osMutexCreate(osMutex(setpoint_mutex));
JamesMacLean 0:80a37292f6b2 69 }
JamesMacLean 0:80a37292f6b2 70
JamesMacLean 0:80a37292f6b2 71 Bumper.rise(&ExtCollisionISR); // Attach the address of the interrupt handler to the rising edge of Bumper
JamesMacLean 0:80a37292f6b2 72
balsamfir 1:cc5636894b95 73 // Start execution of the threads: MotorControlThread,ExtCollisionThread, and SensorControlThread:
JamesMacLean 0:80a37292f6b2 74 SensorControl = osThreadCreate(osThread(SensorControlThread), NULL);
balsamfir 1:cc5636894b95 75 if(MOTORS_ON) MotorControl = osThreadCreate(osThread(MotorControlThread), NULL);
JamesMacLean 0:80a37292f6b2 76 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
JamesMacLean 0:80a37292f6b2 77
JamesMacLean 0:80a37292f6b2 78 // Start the watch dog timer and enable the watch dog interrupt
JamesMacLean 0:80a37292f6b2 79 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
JamesMacLean 0:80a37292f6b2 80 led3=0; // Clear watch dog led3 status
JamesMacLean 0:80a37292f6b2 81
JamesMacLean 0:80a37292f6b2 82 if(MOTORS_ON){
JamesMacLean 0:80a37292f6b2 83 //SPI Section
JamesMacLean 0:80a37292f6b2 84 pc.printf("\n\rStarting SPI...");
JamesMacLean 0:80a37292f6b2 85 // Restart with a new SPI protocol.
JamesMacLean 0:80a37292f6b2 86 DE0.format(16,1); // 16 bit mode 1
JamesMacLean 0:80a37292f6b2 87 IoReset = 0;
JamesMacLean 0:80a37292f6b2 88 IoReset = 1;
JamesMacLean 0:80a37292f6b2 89 wait_us(10);
JamesMacLean 0:80a37292f6b2 90 IoReset = 0;
JamesMacLean 0:80a37292f6b2 91 SpiReset = 0;
JamesMacLean 0:80a37292f6b2 92 SpiReset = 1;
JamesMacLean 0:80a37292f6b2 93 wait_us(10);
JamesMacLean 0:80a37292f6b2 94 SpiReset = 0;
balsamfir 1:cc5636894b95 95 idCode = DE0.write(0x8004); // Read count/time for both motors
JamesMacLean 0:80a37292f6b2 96 pc.printf("\n\rDevice Id: %d", idCode);
JamesMacLean 0:80a37292f6b2 97
JamesMacLean 0:80a37292f6b2 98 // motor Kp and Ki
JamesMacLean 0:80a37292f6b2 99 Kp = 0.000120;
JamesMacLean 0:80a37292f6b2 100 Ki = 0.0000000001;
JamesMacLean 0:80a37292f6b2 101 }
JamesMacLean 0:80a37292f6b2 102
balsamfir 1:cc5636894b95 103 // Specify address of the ISRs and intervals in seconds between interrupts
balsamfir 1:cc5636894b95 104 // TODO: Optimize interrupt time for motor... Currently too fast
balsamfir 1:cc5636894b95 105 if(MOTORS_ON) MotorPeriodicInt.attach(&MotorControllerISR, .001);
balsamfir 1:cc5636894b95 106 SensorPeriodicInt.attach(&SensorControllerISR, .0167); //trigger sensor thread 60 times/sec
JamesMacLean 0:80a37292f6b2 107
JamesMacLean 0:80a37292f6b2 108 //isMeasuring = 1;
JamesMacLean 0:80a37292f6b2 109 //Setpoint = 6;
JamesMacLean 0:80a37292f6b2 110
JamesMacLean 0:80a37292f6b2 111 while (1) {
JamesMacLean 0:80a37292f6b2 112
JamesMacLean 0:80a37292f6b2 113 osTimerStart(OneShot, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms.
JamesMacLean 0:80a37292f6b2 114
balsamfir 1:cc5636894b95 115 pc.printf("X Coordinate: %d, Steering: %d,", x_global, steer_global);
balsamfir 1:cc5636894b95 116 pc.printf("Height: %d, Speed: %d,", height_global, speed_global);
balsamfir 1:cc5636894b95 117 pc.printf(" L setpoint: %d, R setpoint: %d \n\r", left_setpoint, right_setpoint);
balsamfir 1:cc5636894b95 118
JamesMacLean 0:80a37292f6b2 119 /*
JamesMacLean 0:80a37292f6b2 120 if (!isMeasuring && (Setpoint!=0)) {
JamesMacLean 0:80a37292f6b2 121 pc.printf("\n\r-----------------------------------\n\r");
JamesMacLean 0:80a37292f6b2 122 printf("times (ms) \t vel (rads/s) \t error \t\t x value \t u value \n\r");
JamesMacLean 0:80a37292f6b2 123 for (i=0; i<60; i++) {
JamesMacLean 0:80a37292f6b2 124 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 125 }
JamesMacLean 0:80a37292f6b2 126 pc.printf("\n\r-----------------------------------\n\r");
JamesMacLean 0:80a37292f6b2 127 Setpoint = 0;
JamesMacLean 0:80a37292f6b2 128 i = 0;
JamesMacLean 0:80a37292f6b2 129 }
JamesMacLean 0:80a37292f6b2 130 */
JamesMacLean 0:80a37292f6b2 131 Thread::wait(500); // Go to sleep for 500 ms
JamesMacLean 0:80a37292f6b2 132 }
JamesMacLean 0:80a37292f6b2 133 }
JamesMacLean 0:80a37292f6b2 134
JamesMacLean 0:80a37292f6b2 135 // ******** Control Thread ********
JamesMacLean 0:80a37292f6b2 136 void SensorControlThread(void const *argument) {
JamesMacLean 0:80a37292f6b2 137 float speed, steer;
JamesMacLean 0:80a37292f6b2 138 int x, count, height;
JamesMacLean 0:80a37292f6b2 139
JamesMacLean 0:80a37292f6b2 140 Pixy pixy(Pixy::SPI, p11, p12, p13);
JamesMacLean 0:80a37292f6b2 141
JamesMacLean 0:80a37292f6b2 142 while (1) {
JamesMacLean 0:80a37292f6b2 143 osSignalWait(SignalSensor, osWaitForever); //note: SignalSensor is just zero, waits for any signal dirrected at thread.
JamesMacLean 0:80a37292f6b2 144
JamesMacLean 0:80a37292f6b2 145 count = pixy.getBlocks(130); //get as many blocks as are available
JamesMacLean 0:80a37292f6b2 146
JamesMacLean 0:80a37292f6b2 147 //find the target block in all of the blocks that were sent
JamesMacLean 0:80a37292f6b2 148 for(x=0;x<count;x++){
balsamfir 1:cc5636894b95 149 if (pixy.blocks[x].signature == TARGET_DECIMAL) break;
JamesMacLean 0:80a37292f6b2 150 if (x==count) pc.printf("no target in frame");
JamesMacLean 0:80a37292f6b2 151 }
JamesMacLean 0:80a37292f6b2 152
JamesMacLean 0:80a37292f6b2 153 // if blocks were returned
JamesMacLean 0:80a37292f6b2 154 if (count){
JamesMacLean 0:80a37292f6b2 155
JamesMacLean 0:80a37292f6b2 156 //height = CalcHeight() //find the feedback from the pixy for the height
JamesMacLean 0:80a37292f6b2 157 height = pixy.blocks[(x-1)].height; //use this for now
JamesMacLean 0:80a37292f6b2 158
JamesMacLean 0:80a37292f6b2 159 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 160 speed = PI(1, HEIGHT_SETPOINT, SPEED_KP, SPEED_KI, height, SPEED_MAX);
JamesMacLean 0:80a37292f6b2 161
JamesMacLean 0:80a37292f6b2 162 if(!MOTORS_ON){
JamesMacLean 0:80a37292f6b2 163 x_global = pixy.blocks[x-1].x;
JamesMacLean 0:80a37292f6b2 164 height_global = height;
JamesMacLean 0:80a37292f6b2 165 steer_global = steer;
JamesMacLean 0:80a37292f6b2 166 speed_global = speed;
JamesMacLean 0:80a37292f6b2 167 }
JamesMacLean 0:80a37292f6b2 168
balsamfir 1:cc5636894b95 169 // give setpoints to MotorControlThread
JamesMacLean 0:80a37292f6b2 170 osMutexWait(setpoint_mutex, osWaitForever); //enter mutex
JamesMacLean 0:80a37292f6b2 171 left_setpoint = speed - steer;
JamesMacLean 0:80a37292f6b2 172 right_setpoint = speed + steer;
JamesMacLean 0:80a37292f6b2 173 osMutexRelease(setpoint_mutex); //exit mutex
JamesMacLean 0:80a37292f6b2 174 }
JamesMacLean 0:80a37292f6b2 175
JamesMacLean 0:80a37292f6b2 176 }
JamesMacLean 0:80a37292f6b2 177 }
JamesMacLean 0:80a37292f6b2 178
JamesMacLean 0:80a37292f6b2 179 // ******** Control Thread ********
balsamfir 1:cc5636894b95 180 void MotorControlThread(void const *argument) {
JamesMacLean 0:80a37292f6b2 181 short dP_L, dt_L, dP_R, dt_R;
JamesMacLean 0:80a37292f6b2 182 float uL,uR;
JamesMacLean 0:80a37292f6b2 183 float left_set,right_set;
JamesMacLean 0:80a37292f6b2 184
JamesMacLean 0:80a37292f6b2 185 while (1) {
JamesMacLean 0:80a37292f6b2 186 osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received. SignalPi is just zero lol, waits for any signal to thread
balsamfir 1:cc5636894b95 187 led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
JamesMacLean 0:80a37292f6b2 188
JamesMacLean 0:80a37292f6b2 189 dP_L = DE0.write(0); // Read QEI-0 position register
JamesMacLean 0:80a37292f6b2 190 dt_L = DE0.write(0); // Read QEI-0 time register
JamesMacLean 0:80a37292f6b2 191 dP_R = DE0.write(0); // Read QEI-0 position register
JamesMacLean 0:80a37292f6b2 192 dt_R = DE0.write(0); // Read QEI-0 time register
JamesMacLean 0:80a37292f6b2 193
JamesMacLean 0:80a37292f6b2 194 osMutexWait(setpoint_mutex, osWaitForever);//enter mutex
JamesMacLean 0:80a37292f6b2 195 left_set= left_setpoint; //copy setpoint
JamesMacLean 0:80a37292f6b2 196 right_set= right_setpoint;
JamesMacLean 0:80a37292f6b2 197 osMutexRelease(setpoint_mutex);//exit mutex
JamesMacLean 0:80a37292f6b2 198
JamesMacLean 0:80a37292f6b2 199 uL = motorPI(0, left_set, Kp, Ki, dP_L, dt_L);
JamesMacLean 0:80a37292f6b2 200 uR = motorPI(1, right_set, Kp, Ki, dP_R, dt_R);
JamesMacLean 0:80a37292f6b2 201
JamesMacLean 0:80a37292f6b2 202 PWM0.pulsewidth(fabs(uL));
JamesMacLean 0:80a37292f6b2 203 PWM1.pulsewidth(fabs(uR));
JamesMacLean 0:80a37292f6b2 204 }
JamesMacLean 0:80a37292f6b2 205 }
JamesMacLean 0:80a37292f6b2 206
JamesMacLean 0:80a37292f6b2 207 // ******** Collision Thread ********
JamesMacLean 0:80a37292f6b2 208 void ExtCollisionThread(void const *argument) {
JamesMacLean 0:80a37292f6b2 209 while (1) {
JamesMacLean 0:80a37292f6b2 210 osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received
JamesMacLean 0:80a37292f6b2 211 led4 = 1;
JamesMacLean 0:80a37292f6b2 212 }
JamesMacLean 0:80a37292f6b2 213 }
JamesMacLean 0:80a37292f6b2 214
JamesMacLean 0:80a37292f6b2 215 // ******** Watchdog Interrupt Handler ********
balsamfir 1:cc5636894b95 216 // TODO: Shutdown system
JamesMacLean 0:80a37292f6b2 217 void Watchdog(void const *n) {
JamesMacLean 0:80a37292f6b2 218 led3=1; // led3 is activated when the watchdog timer times out
JamesMacLean 0:80a37292f6b2 219 }
JamesMacLean 0:80a37292f6b2 220
JamesMacLean 0:80a37292f6b2 221 // ******** Period Timer Interrupt Handler ********
balsamfir 1:cc5636894b95 222 void MotorControllerISR(void) {
balsamfir 1:cc5636894b95 223 osSignalSet(MotorControl,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt.
JamesMacLean 0:80a37292f6b2 224 }
JamesMacLean 0:80a37292f6b2 225
JamesMacLean 0:80a37292f6b2 226 // ******** Period Timer Interrupt Handler ********
JamesMacLean 0:80a37292f6b2 227 void SensorControllerISR(void) {
JamesMacLean 0:80a37292f6b2 228 osSignalSet(SensorControl,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt. (basically sends signal to id of thread)
JamesMacLean 0:80a37292f6b2 229 }
JamesMacLean 0:80a37292f6b2 230
JamesMacLean 0:80a37292f6b2 231 // ******** Collision Interrupt Handler ********
JamesMacLean 0:80a37292f6b2 232 void ExtCollisionISR(void) {
JamesMacLean 0:80a37292f6b2 233 osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt.
JamesMacLean 0:80a37292f6b2 234 }
JamesMacLean 0:80a37292f6b2 235
JamesMacLean 0:80a37292f6b2 236 // motorPI - motor pi control extacted from previous work
JamesMacLean 0:80a37292f6b2 237 // Input: 0 for Left motor, 1 for right motor
JamesMacLean 0:80a37292f6b2 238 // Output: pwm pulsewidth for corresponding motor
JamesMacLean 0:80a37292f6b2 239
JamesMacLean 0:80a37292f6b2 240 float motorPI(int RL_Motor, float Setpoint, float Kp, float Ki, short dP1, short dt1){
JamesMacLean 0:80a37292f6b2 241 int dP32, i;
JamesMacLean 0:80a37292f6b2 242 float angVel, e, u, xState;
JamesMacLean 0:80a37292f6b2 243
JamesMacLean 0:80a37292f6b2 244 if (dP1 & 0x00008000) {
JamesMacLean 0:80a37292f6b2 245 dP32 = dP1 | 0xFFFF0000;
JamesMacLean 0:80a37292f6b2 246 } else {
JamesMacLean 0:80a37292f6b2 247 dP32 = dP1;
JamesMacLean 0:80a37292f6b2 248 }
JamesMacLean 0:80a37292f6b2 249
JamesMacLean 0:80a37292f6b2 250 angVel = QE2RadsPerSec(dP32, dt1);
JamesMacLean 0:80a37292f6b2 251
JamesMacLean 0:80a37292f6b2 252 e = Setpoint - angVel;
JamesMacLean 0:80a37292f6b2 253
JamesMacLean 0:80a37292f6b2 254 RL_Motor ? xState = R_xState : xState = L_xState; //state is cumulative, select left or right state global variable
JamesMacLean 0:80a37292f6b2 255
JamesMacLean 0:80a37292f6b2 256 // Avoid integrator wind up
JamesMacLean 0:80a37292f6b2 257 if((u>=pwmPeriod)||(u<=-pwmPeriod)){
JamesMacLean 0:80a37292f6b2 258 //xState= xState; - turns off integrator when u maxed
JamesMacLean 0:80a37292f6b2 259 }else{
JamesMacLean 0:80a37292f6b2 260 xState = xState + e;
JamesMacLean 0:80a37292f6b2 261 }
JamesMacLean 0:80a37292f6b2 262
JamesMacLean 0:80a37292f6b2 263 RL_Motor ? R_xState = xState : L_xState = xState; //update state variable
JamesMacLean 0:80a37292f6b2 264
JamesMacLean 0:80a37292f6b2 265 u = Ki*xState + Kp*e;
JamesMacLean 0:80a37292f6b2 266
JamesMacLean 0:80a37292f6b2 267 // Set direction and pwm
JamesMacLean 0:80a37292f6b2 268 if (u >= 0) {
JamesMacLean 0:80a37292f6b2 269 RL_Motor ? DIR1 = 1 : DIR0 = 1;
JamesMacLean 0:80a37292f6b2 270 } else {
JamesMacLean 0:80a37292f6b2 271 RL_Motor ? DIR1 = 0 : DIR0 = 0;
JamesMacLean 0:80a37292f6b2 272 }
JamesMacLean 0:80a37292f6b2 273
JamesMacLean 0:80a37292f6b2 274 if (isMeasuring) {
JamesMacLean 0:80a37292f6b2 275 vels[i] = angVel;
JamesMacLean 0:80a37292f6b2 276 times[i] = i;
JamesMacLean 0:80a37292f6b2 277 errs[i] = e;
JamesMacLean 0:80a37292f6b2 278 xvals[i] = xState;
JamesMacLean 0:80a37292f6b2 279 uvals[i] = u;
JamesMacLean 0:80a37292f6b2 280 if (i>=59) {
JamesMacLean 0:80a37292f6b2 281 isMeasuring = 0;
JamesMacLean 0:80a37292f6b2 282 }
JamesMacLean 0:80a37292f6b2 283 i++;
JamesMacLean 0:80a37292f6b2 284 }
JamesMacLean 0:80a37292f6b2 285
JamesMacLean 0:80a37292f6b2 286 if (fabs(u) > pwmPeriod) u = pwmPeriod;
JamesMacLean 0:80a37292f6b2 287
JamesMacLean 0:80a37292f6b2 288 return u;
JamesMacLean 0:80a37292f6b2 289 }
JamesMacLean 0:80a37292f6b2 290
JamesMacLean 0:80a37292f6b2 291 // PI() - A generic PI controller function used by the sensor control thread
JamesMacLean 0:80a37292f6b2 292 // st_or_sp - 0 for steering, 1 for speed
JamesMacLean 0:80a37292f6b2 293 float PI(int st_or_sp, float setpoint, float Kp, float Ki, int feedback, float max_val){
JamesMacLean 0:80a37292f6b2 294 float e, xState, u;
JamesMacLean 0:80a37292f6b2 295
JamesMacLean 0:80a37292f6b2 296 e = setpoint - (float)feedback;
JamesMacLean 0:80a37292f6b2 297
JamesMacLean 0:80a37292f6b2 298 st_or_sp ? xState = speed_xState : xState = steer_xState;
JamesMacLean 0:80a37292f6b2 299
JamesMacLean 0:80a37292f6b2 300 // Avoid integrator wind up
JamesMacLean 0:80a37292f6b2 301 if((u>=max_val)||(u<=-max_val)){
JamesMacLean 0:80a37292f6b2 302 //xState= xState; - turns off integrator when u maxed
JamesMacLean 0:80a37292f6b2 303 }else{
JamesMacLean 0:80a37292f6b2 304 xState = xState + e;
JamesMacLean 0:80a37292f6b2 305 }
JamesMacLean 0:80a37292f6b2 306
JamesMacLean 0:80a37292f6b2 307 st_or_sp ? speed_xState = xState : steer_xState = xState; //update state variable
JamesMacLean 0:80a37292f6b2 308
JamesMacLean 0:80a37292f6b2 309 u = Ki*xState + Kp*e;
JamesMacLean 0:80a37292f6b2 310
JamesMacLean 0:80a37292f6b2 311 if (fabs(u) > max_val) u = max_val;
JamesMacLean 0:80a37292f6b2 312
JamesMacLean 0:80a37292f6b2 313 return u;
JamesMacLean 0:80a37292f6b2 314 }
JamesMacLean 0:80a37292f6b2 315
JamesMacLean 0:80a37292f6b2 316 float QE2RadsPerSec(int counts, int time) {
JamesMacLean 0:80a37292f6b2 317 return (counts*122.62)/time;
JamesMacLean 0:80a37292f6b2 318 }