2015-2016_Mouserat / WorkingPID

Goes in a Line!

Dependencies:   AVEncoder mbed-src-AV

Fork of Test by 2015-2016_Mouserat

main.cpp@1:3cacc3c50e68, 2015-11-21 (annotated)

Committer:
jimmery
Date:
Sat Nov 21 05:32:35 2015 +0000
Revision:
1:3cacc3c50e68
Parent:
0:13d8a77fb1d7 
A working PID test that we did on 11/20/15;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jimmery 0:13d8a77fb1d7 1 #include "mbed.h"
jimmery 0:13d8a77fb1d7 2 #include "AVEncoder.h"
jimmery 0:13d8a77fb1d7 3
jimmery 0:13d8a77fb1d7 4 // set things
jimmery 0:13d8a77fb1d7 5 Serial pc(SERIAL_TX, SERIAL_RX);
jimmery 0:13d8a77fb1d7 6 Ticker Systicker;
jimmery 0:13d8a77fb1d7 7 Timer timer;
jimmery 0:13d8a77fb1d7 8
jimmery 0:13d8a77fb1d7 9 PwmOut right_forward(PB_10);
jimmery 0:13d8a77fb1d7 10 PwmOut right_reverse(PA_6);
jimmery 0:13d8a77fb1d7 11 PwmOut left_forward(PA_7);
jimmery 0:13d8a77fb1d7 12 PwmOut left_reverse(PB_6);
jimmery 0:13d8a77fb1d7 13
jimmery 0:13d8a77fb1d7 14 // TODO: change our encoder pins from AnalogIn into:
jimmery 0:13d8a77fb1d7 15 // otherwise, we can also use the AVEncoder thing as well.
jimmery 0:13d8a77fb1d7 16 AVEncoder l_enco(PA_15, PB_3);
jimmery 0:13d8a77fb1d7 17 AVEncoder r_enco(PA_1, PA_10);
jimmery 0:13d8a77fb1d7 18
jimmery 0:13d8a77fb1d7 19 // gyro
jimmery 0:13d8a77fb1d7 20 AnalogIn _gyro(PA_0);
jimmery 0:13d8a77fb1d7 21 // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected.
jimmery 0:13d8a77fb1d7 22
jimmery 0:13d8a77fb1d7 23 //Left Front IR
jimmery 0:13d8a77fb1d7 24 DigitalOut eLF(PC_3);
jimmery 0:13d8a77fb1d7 25 AnalogIn rLF(PC_0);
jimmery 0:13d8a77fb1d7 26 //PC_4 is an ADC
jimmery 0:13d8a77fb1d7 27 //Left Side IR
jimmery 0:13d8a77fb1d7 28 DigitalOut eLS(PC_2);
jimmery 0:13d8a77fb1d7 29 AnalogIn rLS(PC_1);
jimmery 0:13d8a77fb1d7 30
jimmery 0:13d8a77fb1d7 31 //Right Front IR
jimmery 0:13d8a77fb1d7 32 DigitalOut eRF(PC_12);
jimmery 0:13d8a77fb1d7 33 AnalogIn rRF(PA_4);
jimmery 0:13d8a77fb1d7 34
jimmery 0:13d8a77fb1d7 35 //Right Side IR
jimmery 0:13d8a77fb1d7 36 DigitalOut eRS(PC_15);
jimmery 0:13d8a77fb1d7 37 AnalogIn rRS(PB_0);
jimmery 0:13d8a77fb1d7 38
jimmery 0:13d8a77fb1d7 39 DigitalOut myled(LED1);
jimmery 0:13d8a77fb1d7 40
jimmery 0:13d8a77fb1d7 41 volatile float gyro_offset = 0;
jimmery 0:13d8a77fb1d7 42
jimmery 0:13d8a77fb1d7 43 volatile float line_prevError = 0;
jimmery 0:13d8a77fb1d7 44 volatile float enco_prevError = 0;
jimmery 0:13d8a77fb1d7 45 volatile float gyro_prevError = 0;
jimmery 0:13d8a77fb1d7 46
jimmery 0:13d8a77fb1d7 47 volatile float line_accumulator = 0;
jimmery 0:13d8a77fb1d7 48 volatile float line_decayFactor = 1;
jimmery 0:13d8a77fb1d7 49 volatile float enco_accumulator = 0;
jimmery 1:3cacc3c50e68 50 volatile float enco_decayFactor = 1.2;
jimmery 0:13d8a77fb1d7 51 volatile float gyro_accumulator = 0;
jimmery 1:3cacc3c50e68 52 volatile float gyro_decayFactor = 1.2;
jimmery 0:13d8a77fb1d7 53
jimmery 1:3cacc3c50e68 54 volatile float set_speed = 0.5;
jimmery 0:13d8a77fb1d7 55 volatile float left_speed = 0.5;
jimmery 0:13d8a77fb1d7 56 volatile float right_speed = 0.5;
jimmery 0:13d8a77fb1d7 57
jimmery 0:13d8a77fb1d7 58 const float left_max_speed = 6; // max speed is 6 encoder pulses per ms.
jimmery 0:13d8a77fb1d7 59 const float right_max_speed = 6.2;
jimmery 0:13d8a77fb1d7 60
jimmery 1:3cacc3c50e68 61 const float gyro_propo = 6.5;
jimmery 0:13d8a77fb1d7 62 const float gyro_integ = 0;
jimmery 1:3cacc3c50e68 63 const float gyro_deriv = 10;
jimmery 0:13d8a77fb1d7 64
jimmery 1:3cacc3c50e68 65 const float enco_propo = .0005;
jimmery 0:13d8a77fb1d7 66 const float enco_integ = 0;
jimmery 1:3cacc3c50e68 67 const float enco_deriv = .0002;
jimmery 0:13d8a77fb1d7 68
jimmery 1:3cacc3c50e68 69 const float spin_enco_weight = 0.75;
jimmery 0:13d8a77fb1d7 70 const float spin_gyro_weight = 1 - spin_enco_weight;
jimmery 0:13d8a77fb1d7 71
jimmery 1:3cacc3c50e68 72 const float frontWall = 0.7; //need to calibrate this threshold to a value where mouse can stop in time
jimmery 1:3cacc3c50e68 73 //something like this may be useful
jimmery 1:3cacc3c50e68 74
jimmery 0:13d8a77fb1d7 75 volatile float enco_error;
jimmery 0:13d8a77fb1d7 76 volatile float enco_pid;
jimmery 0:13d8a77fb1d7 77 volatile float gyro_error;
jimmery 0:13d8a77fb1d7 78 volatile float gyro_pid;
jimmery 0:13d8a77fb1d7 79 volatile float w_error;
jimmery 0:13d8a77fb1d7 80
jimmery 1:3cacc3c50e68 81 // this is just so that we can maintain what state our mouse is in.
jimmery 1:3cacc3c50e68 82 // currently this has no real use, but it may in the future.
jimmery 1:3cacc3c50e68 83 // or we could just remove this entirely.
jimmery 1:3cacc3c50e68 84 typedef enum
jimmery 1:3cacc3c50e68 85 {
jimmery 1:3cacc3c50e68 86 STOPPED,
jimmery 1:3cacc3c50e68 87 FORWARD,
jimmery 1:3cacc3c50e68 88 TURNING,
jimmery 1:3cacc3c50e68 89 UNKNOWN
jimmery 1:3cacc3c50e68 90 } STATE;
jimmery 1:3cacc3c50e68 91 volatile STATE mouse_state;
jimmery 1:3cacc3c50e68 92
jimmery 1:3cacc3c50e68 93 void watchOut();
jimmery 1:3cacc3c50e68 94 void offsetCalc();
jimmery 1:3cacc3c50e68 95 void stop();
jimmery 1:3cacc3c50e68 96
jimmery 0:13d8a77fb1d7 97 void reset()
jimmery 0:13d8a77fb1d7 98 {
jimmery 0:13d8a77fb1d7 99 l_enco.reset();
jimmery 0:13d8a77fb1d7 100 r_enco.reset();
jimmery 0:13d8a77fb1d7 101 }
jimmery 0:13d8a77fb1d7 102
jimmery 0:13d8a77fb1d7 103 void systick()
jimmery 0:13d8a77fb1d7 104 {
jimmery 1:3cacc3c50e68 105 watchOut();
jimmery 1:3cacc3c50e68 106 if ( mouse_state == STOPPED )
jimmery 1:3cacc3c50e68 107 {
jimmery 1:3cacc3c50e68 108 offsetCalc();
jimmery 1:3cacc3c50e68 109 stop();
jimmery 1:3cacc3c50e68 110 return;
jimmery 1:3cacc3c50e68 111 }
jimmery 1:3cacc3c50e68 112
jimmery 0:13d8a77fb1d7 113 enco_error = l_enco.getPulses() - r_enco.getPulses();
jimmery 0:13d8a77fb1d7 114 gyro_error = _gyro.read() - gyro_offset;
jimmery 0:13d8a77fb1d7 115
jimmery 1:3cacc3c50e68 116 enco_accumulator += enco_error;
jimmery 1:3cacc3c50e68 117 gyro_accumulator += gyro_error;
jimmery 1:3cacc3c50e68 118
jimmery 0:13d8a77fb1d7 119 enco_pid = 0;
jimmery 1:3cacc3c50e68 120 enco_pid += enco_propo * enco_error;
jimmery 1:3cacc3c50e68 121 enco_pid += enco_integ * enco_accumulator;
jimmery 1:3cacc3c50e68 122 enco_pid += enco_deriv * (enco_error - enco_prevError);
jimmery 0:13d8a77fb1d7 123
jimmery 0:13d8a77fb1d7 124 gyro_pid = 0;
jimmery 0:13d8a77fb1d7 125 gyro_pid += gyro_propo * gyro_error;
jimmery 1:3cacc3c50e68 126 gyro_pid += gyro_integ * gyro_accumulator;
jimmery 0:13d8a77fb1d7 127 gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
jimmery 0:13d8a77fb1d7 128
jimmery 0:13d8a77fb1d7 129 w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
jimmery 1:3cacc3c50e68 130 left_speed = set_speed + w_error;
jimmery 1:3cacc3c50e68 131 right_speed = set_speed - w_error;
jimmery 0:13d8a77fb1d7 132
jimmery 0:13d8a77fb1d7 133 left_forward = left_speed / left_max_speed;
jimmery 0:13d8a77fb1d7 134 left_reverse = 0;
jimmery 0:13d8a77fb1d7 135 right_forward = right_speed / right_max_speed;
jimmery 0:13d8a77fb1d7 136 right_reverse = 0;
jimmery 0:13d8a77fb1d7 137
jimmery 1:3cacc3c50e68 138 enco_prevError = enco_error;
jimmery 1:3cacc3c50e68 139 gyro_prevError = gyro_error;
jimmery 1:3cacc3c50e68 140
jimmery 1:3cacc3c50e68 141 enco_accumulator += enco_error;
jimmery 1:3cacc3c50e68 142 gyro_accumulator += gyro_error;
jimmery 1:3cacc3c50e68 143
jimmery 1:3cacc3c50e68 144 enco_accumulator /= enco_decayFactor;
jimmery 1:3cacc3c50e68 145 gyro_accumulator /= gyro_decayFactor;
jimmery 1:3cacc3c50e68 146
jimmery 1:3cacc3c50e68 147 //reset();
jimmery 0:13d8a77fb1d7 148 }
jimmery 0:13d8a77fb1d7 149
jimmery 0:13d8a77fb1d7 150 // computes gyro_offset
jimmery 0:13d8a77fb1d7 151 // uses a "weighted" average.
jimmery 0:13d8a77fb1d7 152 // idea is that the current gyro offset is weighted more than previous ones.
jimmery 0:13d8a77fb1d7 153 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
jimmery 0:13d8a77fb1d7 154 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
jimmery 0:13d8a77fb1d7 155 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
jimmery 0:13d8a77fb1d7 156 // currently this is only in the setup function. we can run this when the mouse is running in a line
jimmery 0:13d8a77fb1d7 157 // when we figure out good line running pid.
jimmery 0:13d8a77fb1d7 158 void offsetCalc()
jimmery 0:13d8a77fb1d7 159 {
jimmery 0:13d8a77fb1d7 160 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
jimmery 0:13d8a77fb1d7 161 }
jimmery 0:13d8a77fb1d7 162
jimmery 0:13d8a77fb1d7 163
jimmery 1:3cacc3c50e68 164 void stop()
jimmery 1:3cacc3c50e68 165 {
jimmery 1:3cacc3c50e68 166 left_forward = 1;
jimmery 1:3cacc3c50e68 167 left_reverse = 1;
jimmery 1:3cacc3c50e68 168 right_forward = 1;
jimmery 1:3cacc3c50e68 169 right_reverse = 1;
jimmery 1:3cacc3c50e68 170 }
jimmery 1:3cacc3c50e68 171
jimmery 1:3cacc3c50e68 172 void watchOut()
jimmery 1:3cacc3c50e68 173 {
jimmery 1:3cacc3c50e68 174 eRF = 1;
jimmery 1:3cacc3c50e68 175 float right = rRF.read();
jimmery 1:3cacc3c50e68 176 eRF = 0;
jimmery 1:3cacc3c50e68 177 eLF = 1;
jimmery 1:3cacc3c50e68 178 float left = rLF.read();
jimmery 1:3cacc3c50e68 179 eLF = 0;
jimmery 1:3cacc3c50e68 180 if(left > frontWall || right > frontWall)
jimmery 1:3cacc3c50e68 181 {
jimmery 1:3cacc3c50e68 182 mouse_state = STOPPED;
jimmery 1:3cacc3c50e68 183 }
jimmery 1:3cacc3c50e68 184 }
jimmery 1:3cacc3c50e68 185
jimmery 1:3cacc3c50e68 186
jimmery 0:13d8a77fb1d7 187 void setup()
jimmery 0:13d8a77fb1d7 188 {
jimmery 0:13d8a77fb1d7 189 pc.printf("Hello World\r\n");
jimmery 0:13d8a77fb1d7 190
jimmery 0:13d8a77fb1d7 191 eRS = 0;
jimmery 0:13d8a77fb1d7 192 eRF = 0;
jimmery 0:13d8a77fb1d7 193 eLS = 0;
jimmery 0:13d8a77fb1d7 194 eLF = 0;
jimmery 0:13d8a77fb1d7 195
jimmery 1:3cacc3c50e68 196 mouse_state = FORWARD;
jimmery 1:3cacc3c50e68 197
jimmery 0:13d8a77fb1d7 198 myled = 1;
jimmery 0:13d8a77fb1d7 199
jimmery 0:13d8a77fb1d7 200 for ( int i = 0; i < 1000; i++ )
jimmery 0:13d8a77fb1d7 201 {
jimmery 0:13d8a77fb1d7 202 offsetCalc();
jimmery 0:13d8a77fb1d7 203 }
jimmery 0:13d8a77fb1d7 204
jimmery 0:13d8a77fb1d7 205 //left_forward = left_speed / left_max_speed;
jimmery 0:13d8a77fb1d7 206 // left_reverse = 0;
jimmery 0:13d8a77fb1d7 207 // right_forward = right_speed / right_max_speed;
jimmery 0:13d8a77fb1d7 208 // right_reverse = 0;
jimmery 0:13d8a77fb1d7 209
jimmery 0:13d8a77fb1d7 210 wait(2);
jimmery 0:13d8a77fb1d7 211
jimmery 0:13d8a77fb1d7 212 // repeat this for some time frame.
jimmery 0:13d8a77fb1d7 213 Systicker.attach_us(&systick, 1000);
jimmery 0:13d8a77fb1d7 214 }
jimmery 0:13d8a77fb1d7 215
jimmery 0:13d8a77fb1d7 216
jimmery 0:13d8a77fb1d7 217 int main() {
jimmery 0:13d8a77fb1d7 218 setup();
jimmery 0:13d8a77fb1d7 219 while(1) {
jimmery 0:13d8a77fb1d7 220
jimmery 1:3cacc3c50e68 221 pc.printf("enco_pid: %f, gyro_pid: %f, w_error: %f\r\n", enco_pid, gyro_pid, w_error);
jimmery 0:13d8a77fb1d7 222 //wait(1);
jimmery 0:13d8a77fb1d7 223 }
jimmery 0:13d8a77fb1d7 224 }