2015-2016_Mouserat / Lab1_Test

hello

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@4:453d534b1c1d, 2015-11-14 (annotated)

Committer:
intgsull
Date:
Sat Nov 14 00:15:41 2015 +0000
Revision:
4:453d534b1c1d
Parent:
2:5f9b78950a17
Child:
5:f9837617817b
This is pre-Lab3, with the skeleton PID code. Just in case we need to reset the entire process.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
intgsull 4:453d534b1c1d 1 //Micromouse code
intgsull 0:fa523db3f4f5 2 #include "mbed.h"
intgsull 4:453d534b1c1d 3 #include "AVEncoder.h"
intgsull 0:fa523db3f4f5 4
intgsull 4:453d534b1c1d 5 // set things
intgsull 4:453d534b1c1d 6 Serial pc(SERIAL_TX, SERIAL_RX);
intgsull 4:453d534b1c1d 7 Ticker Systicker;
intgsull 4:453d534b1c1d 8 Timer timer;
intgsull 4:453d534b1c1d 9
intgsull 4:453d534b1c1d 10 PwmOut motor1_forward(PB_10);
intgsull 2:5f9b78950a17 11 PwmOut motor1_reverse(PA_6);
intgsull 0:fa523db3f4f5 12 PwmOut motor2_forward(PA_7);
intgsull 0:fa523db3f4f5 13 PwmOut motor2_reverse(PB_6);
intgsull 0:fa523db3f4f5 14
intgsull 4:453d534b1c1d 15 // TODO: change our encoder pins from AnalogIn into:
intgsull 4:453d534b1c1d 16 // otherwise, we can also use the AVEncoder thing as well.
intgsull 4:453d534b1c1d 17 AVEncoder l_enco(PA_15, PB_3);
intgsull 4:453d534b1c1d 18 AVEncoder r_enco(PA_1, PA_10);
intgsull 2:5f9b78950a17 19
intgsull 4:453d534b1c1d 20 // gyro
intgsull 2:5f9b78950a17 21 AnalogIn _gyro(PA_0);
intgsull 4:453d534b1c1d 22 // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected.
intgsull 1:5b9fa1823663 23
intgsull 2:5f9b78950a17 24 //Left Front IR
intgsull 2:5f9b78950a17 25 DigitalOut eLF(PC_3);
intgsull 2:5f9b78950a17 26 AnalogIn rLF(PC_0);
intgsull 2:5f9b78950a17 27 //PC_4 is an ADC
intgsull 2:5f9b78950a17 28 //Left Side IR
intgsull 2:5f9b78950a17 29 DigitalOut eLS(PC_2);
intgsull 2:5f9b78950a17 30 AnalogIn rLS(PC_1);
intgsull 2:5f9b78950a17 31
intgsull 2:5f9b78950a17 32 //Right Front IR
intgsull 2:5f9b78950a17 33 DigitalOut eRF(PC_12);
intgsull 2:5f9b78950a17 34 AnalogIn rRF(PA_4);
intgsull 2:5f9b78950a17 35
intgsull 2:5f9b78950a17 36 //Right Side IR
intgsull 2:5f9b78950a17 37 DigitalOut eRS(PC_15);
intgsull 2:5f9b78950a17 38 AnalogIn rRS(PB_0);
intgsull 1:5b9fa1823663 39
intgsull 0:fa523db3f4f5 40 DigitalOut myled(LED1);
intgsull 0:fa523db3f4f5 41
intgsull 4:453d534b1c1d 42 // global variables.
intgsull 4:453d534b1c1d 43 volatile float gyro_offset = 0;
intgsull 4:453d534b1c1d 44
intgsull 4:453d534b1c1d 45 volatile float line_speed = 1; // currently is in terms of encoder pulses / ms.
intgsull 4:453d534b1c1d 46 volatile float angl_speed = 0; // should not turn while moving forward.
intgsull 4:453d534b1c1d 47
intgsull 4:453d534b1c1d 48 volatile float line_prevError = 0;
intgsull 4:453d534b1c1d 49 volatile int enco_prevError = 0;
intgsull 4:453d534b1c1d 50 volatile float gyro_prevError = 0;
intgsull 1:5b9fa1823663 51
intgsull 4:453d534b1c1d 52 volatile float line_accumulator = 0;
intgsull 4:453d534b1c1d 53 volatile float line_decayFactor = 1.5;
intgsull 4:453d534b1c1d 54 volatile float enco_accumulator = 0;
intgsull 4:453d534b1c1d 55 volatile float enco_decayFactor = 1.5;
intgsull 4:453d534b1c1d 56 volatile float gyro_accumulator = 0;
intgsull 4:453d534b1c1d 57 volatile float gyro_decayFactor = 1.5;
intgsull 4:453d534b1c1d 58
intgsull 4:453d534b1c1d 59 volatile float left_speed = 0;
intgsull 4:453d534b1c1d 60 volatile float right_speed = 0;
intgsull 1:5b9fa1823663 61
intgsull 4:453d534b1c1d 62 volatile unsigned long l_pulses = 0;
intgsull 4:453d534b1c1d 63 volatile unsigned long r_pulses = 0;
intgsull 0:fa523db3f4f5 64
intgsull 4:453d534b1c1d 65 // pid constants. these need to be tuned. but i set them as a default val for now.
intgsull 4:453d534b1c1d 66 // line refers to the translational speed.
intgsull 4:453d534b1c1d 67 // enco and gyro will be used primarily for angular speed.
intgsull 4:453d534b1c1d 68 // (we can change the names later,
intgsull 4:453d534b1c1d 69 // i added line in after i realized that i already had the angular code)
intgsull 4:453d534b1c1d 70 const float line_propo = 1;
intgsull 4:453d534b1c1d 71 const float line_integ = 1;
intgsull 4:453d534b1c1d 72 const float line_deriv = 1;
intgsull 2:5f9b78950a17 73
intgsull 4:453d534b1c1d 74 const float gyro_propo = 1;
intgsull 4:453d534b1c1d 75 const float gyro_integ = 1;
intgsull 4:453d534b1c1d 76 const float gyro_deriv = 1;
intgsull 4:453d534b1c1d 77
intgsull 4:453d534b1c1d 78 const float enco_propo = 1;
intgsull 4:453d534b1c1d 79 const float enco_integ = 1;
intgsull 4:453d534b1c1d 80 const float enco_deriv = 1;
intgsull 4:453d534b1c1d 81
intgsull 4:453d534b1c1d 82 const float spin_enco_weight = 0.5;
intgsull 4:453d534b1c1d 83 const float spin_gyro_weight = 1 - spin_enco_weight;
intgsull 1:5b9fa1823663 84
intgsull 4:453d534b1c1d 85 // this is just so that we can maintain what state our mouse is in.
intgsull 4:453d534b1c1d 86 // currently this has no real use, but it may in the future.
intgsull 4:453d534b1c1d 87 // or we could just remove this entirely.
intgsull 4:453d534b1c1d 88 typedef enum
intgsull 4:453d534b1c1d 89 {
intgsull 4:453d534b1c1d 90 STOPPED,
intgsull 4:453d534b1c1d 91 FORWARD,
intgsull 4:453d534b1c1d 92 TURNING,
intgsull 4:453d534b1c1d 93 UNKNOWN
intgsull 4:453d534b1c1d 94 } STATE;
intgsull 4:453d534b1c1d 95 STATE mouse_state;
intgsull 4:453d534b1c1d 96
intgsull 4:453d534b1c1d 97 // helper functions
intgsull 4:453d534b1c1d 98 void reset();
intgsull 4:453d534b1c1d 99 void offsetCalc();
intgsull 4:453d534b1c1d 100 void stop();
intgsull 4:453d534b1c1d 101 void moveForward();
intgsull 4:453d534b1c1d 102 void turn();
intgsull 1:5b9fa1823663 103
intgsull 4:453d534b1c1d 104 // interrupt stuff.
intgsull 4:453d534b1c1d 105 void incLENC()
intgsull 4:453d534b1c1d 106 {
intgsull 4:453d534b1c1d 107 l_pulses++;
intgsull 4:453d534b1c1d 108 }
intgsull 4:453d534b1c1d 109
intgsull 4:453d534b1c1d 110 void incRENC()
intgsull 4:453d534b1c1d 111 {
intgsull 4:453d534b1c1d 112 r_pulses++;
intgsull 4:453d534b1c1d 113 }
intgsull 2:5f9b78950a17 114
intgsull 4:453d534b1c1d 115 // PID Control
intgsull 4:453d534b1c1d 116 // this contains the simplistic PID control for the most part.
intgsull 4:453d534b1c1d 117 // we do have to calibrate constants though.
intgsull 4:453d534b1c1d 118 void systick()
intgsull 4:453d534b1c1d 119 {
intgsull 4:453d534b1c1d 120 if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
intgsull 4:453d534b1c1d 121 {
intgsull 4:453d534b1c1d 122 // do nothing?
intgsull 4:453d534b1c1d 123 // reset?
intgsull 4:453d534b1c1d 124 reset();
intgsull 4:453d534b1c1d 125 offsetCalc();
intgsull 4:453d534b1c1d 126 return;
intgsull 2:5f9b78950a17 127 }
intgsull 2:5f9b78950a17 128
intgsull 4:453d534b1c1d 129 int dt = timer.read_us(); // should be around 1 ms.
intgsull 4:453d534b1c1d 130 timer.reset();
intgsull 4:453d534b1c1d 131
intgsull 4:453d534b1c1d 132 float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses());
intgsull 4:453d534b1c1d 133 int enco_error = l_enco.getPulses() - r_enco.getPulses();
intgsull 4:453d534b1c1d 134 float gyro_error = _gyro.read() - gyro_offset;
intgsull 4:453d534b1c1d 135
intgsull 4:453d534b1c1d 136 line_accumulator += line_error;
intgsull 4:453d534b1c1d 137 enco_accumulator += enco_error;
intgsull 4:453d534b1c1d 138 gyro_accumulator += gyro_error;
intgsull 4:453d534b1c1d 139
intgsull 4:453d534b1c1d 140 float line_pid = 0;
intgsull 4:453d534b1c1d 141 line_pid += line_propo * line_error;
intgsull 4:453d534b1c1d 142 line_pid += line_integ * line_accumulator;
intgsull 4:453d534b1c1d 143 line_pid += line_deriv * (line_error - line_prevError)/dt;
intgsull 4:453d534b1c1d 144
intgsull 4:453d534b1c1d 145 float enco_pid = 0;
intgsull 4:453d534b1c1d 146 enco_pid += enco_propo * enco_error;
intgsull 4:453d534b1c1d 147 enco_pid += enco_integ * enco_accumulator;
intgsull 4:453d534b1c1d 148 enco_pid += enco_deriv * (enco_error - enco_prevError)/dt;
intgsull 4:453d534b1c1d 149
intgsull 4:453d534b1c1d 150 float gyro_pid = 0;
intgsull 4:453d534b1c1d 151 gyro_pid += gyro_propo * gyro_error;
intgsull 4:453d534b1c1d 152 gyro_pid += gyro_integ * gyro_accumulator;
intgsull 4:453d534b1c1d 153 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError)/dt;
intgsull 4:453d534b1c1d 154
intgsull 4:453d534b1c1d 155 // forward moving pid control.
intgsull 4:453d534b1c1d 156 if ( mouse_state == FORWARD )
intgsull 4:453d534b1c1d 157 {
intgsull 4:453d534b1c1d 158 float x_error = line_pid;
intgsull 4:453d534b1c1d 159 float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
intgsull 4:453d534b1c1d 160 left_speed += x_error + w_error;
intgsull 4:453d534b1c1d 161 right_speed += x_error - w_error;
intgsull 4:453d534b1c1d 162 // offsetCalc();
intgsull 4:453d534b1c1d 163 }
intgsull 4:453d534b1c1d 164 if ( mouse_state == TURNING )
intgsull 4:453d534b1c1d 165 {
intgsull 4:453d534b1c1d 166 // nothing for now. if we turn in place, we assume no pid control.
intgsull 4:453d534b1c1d 167 // this may have to change when we try curve turns.
intgsull 4:453d534b1c1d 168 }
intgsull 4:453d534b1c1d 169
intgsull 4:453d534b1c1d 170 line_prevError = line_error;
intgsull 4:453d534b1c1d 171 enco_prevError = enco_error;
intgsull 4:453d534b1c1d 172 gyro_prevError = gyro_error;
intgsull 4:453d534b1c1d 173
intgsull 4:453d534b1c1d 174 line_accumulator /= line_decayFactor;
intgsull 4:453d534b1c1d 175 enco_accumulator /= enco_decayFactor;
intgsull 4:453d534b1c1d 176 gyro_accumulator /= gyro_decayFactor;
intgsull 4:453d534b1c1d 177 }
intgsull 4:453d534b1c1d 178
intgsull 4:453d534b1c1d 179 // setup stuff.
intgsull 4:453d534b1c1d 180 void setup()
intgsull 1:5b9fa1823663 181 {
intgsull 4:453d534b1c1d 182 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 183
intgsull 4:453d534b1c1d 184 eRS = 0;
intgsull 4:453d534b1c1d 185 eRF = 0;
intgsull 4:453d534b1c1d 186 eLS = 0;
intgsull 4:453d534b1c1d 187 eLF = 0;
intgsull 4:453d534b1c1d 188
intgsull 4:453d534b1c1d 189 myled = 1;
intgsull 1:5b9fa1823663 190
intgsull 4:453d534b1c1d 191 // repeat this for some time frame.
intgsull 4:453d534b1c1d 192 offsetCalc();
intgsull 4:453d534b1c1d 193 Systicker.attach_us(&systick, 1000);
intgsull 4:453d534b1c1d 194 }
intgsull 4:453d534b1c1d 195
intgsull 4:453d534b1c1d 196 void reset()
intgsull 4:453d534b1c1d 197 {
intgsull 4:453d534b1c1d 198 l_pulses = 0;
intgsull 4:453d534b1c1d 199 r_pulses = 0;
intgsull 1:5b9fa1823663 200 }
intgsull 1:5b9fa1823663 201
intgsull 1:5b9fa1823663 202
intgsull 4:453d534b1c1d 203 // computes gyro_offset
intgsull 4:453d534b1c1d 204 // uses a "weighted" average.
intgsull 4:453d534b1c1d 205 // idea is that the current gyro offset is weighted more than previous ones.
intgsull 4:453d534b1c1d 206 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
intgsull 4:453d534b1c1d 207 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
intgsull 4:453d534b1c1d 208 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
intgsull 4:453d534b1c1d 209 // currently this is only in the setup function. we can run this when the mouse is running in a line
intgsull 4:453d534b1c1d 210 // when we figure out good line running pid.
intgsull 4:453d534b1c1d 211 void offsetCalc()
intgsull 4:453d534b1c1d 212 {
intgsull 4:453d534b1c1d 213 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
intgsull 4:453d534b1c1d 214 }
intgsull 1:5b9fa1823663 215
intgsull 4:453d534b1c1d 216
intgsull 0:fa523db3f4f5 217
intgsull 4:453d534b1c1d 218 int main()
intgsull 4:453d534b1c1d 219 {
intgsull 4:453d534b1c1d 220 setup();
intgsull 4:453d534b1c1d 221 while(1)
intgsull 4:453d534b1c1d 222 {
intgsull 4:453d534b1c1d 223 pc.printf("The left motor is going at speed: %d\r\n", left_speed);
intgsull 4:453d534b1c1d 224 pc.printf("The left motor is going at speed: %d\r\n", right_speed);
intgsull 4:453d534b1c1d 225 wait(1);
intgsull 0:fa523db3f4f5 226 }
intgsull 0:fa523db3f4f5 227 }
intgsull 0:fa523db3f4f5 228
intgsull 4:453d534b1c1d 229
intgsull 4:453d534b1c1d 230 // movement functions.
intgsull 4:453d534b1c1d 231 void moveForward()
intgsull 4:453d534b1c1d 232 {
intgsull 4:453d534b1c1d 233 mouse_state = FORWARD;
intgsull 4:453d534b1c1d 234
intgsull 4:453d534b1c1d 235 if ( left_speed > 0 ) // which should be always.
intgsull 4:453d534b1c1d 236 {
intgsull 4:453d534b1c1d 237 motor1_forward = left_speed;
intgsull 4:453d534b1c1d 238 motor1_reverse = 0;
intgsull 4:453d534b1c1d 239 }
intgsull 4:453d534b1c1d 240 else
intgsull 4:453d534b1c1d 241 {
intgsull 4:453d534b1c1d 242 motor1_forward = 0;
intgsull 4:453d534b1c1d 243 motor1_reverse = -left_speed;
intgsull 0:fa523db3f4f5 244 }
intgsull 0:fa523db3f4f5 245
intgsull 4:453d534b1c1d 246 if ( right_speed > 0 ) // which again should be always.
intgsull 4:453d534b1c1d 247 {
intgsull 4:453d534b1c1d 248 motor2_forward = right_speed;
intgsull 0:fa523db3f4f5 249 motor2_reverse = 0;
intgsull 0:fa523db3f4f5 250 }
intgsull 4:453d534b1c1d 251 else
intgsull 4:453d534b1c1d 252 {
intgsull 0:fa523db3f4f5 253 motor2_forward = 0;
intgsull 4:453d534b1c1d 254 motor2_reverse = -right_speed;
intgsull 0:fa523db3f4f5 255 }
intgsull 4:453d534b1c1d 256 }
intgsull 4:453d534b1c1d 257
intgsull 4:453d534b1c1d 258 void stop()
intgsull 4:453d534b1c1d 259 {
intgsull 4:453d534b1c1d 260 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 261
intgsull 4:453d534b1c1d 262 motor1_forward = 1.0;
intgsull 4:453d534b1c1d 263 motor1_reverse = 1.0;
intgsull 4:453d534b1c1d 264 motor2_forward = 1.0;
intgsull 4:453d534b1c1d 265 motor2_reverse = 1.0;
intgsull 4:453d534b1c1d 266 }
intgsull 4:453d534b1c1d 267
intgsull 4:453d534b1c1d 268 void turn(int dir); // maybe split this into two functions?