Michael Romain / Mbed 2 deprecated Shitty_figure8

Framework for doing the figure 8 track.

Dependencies:   mbed

main.cpp@0:ad375c052b4c, 2015-03-20 (annotated)

Committer:
mawk2311
Date:
Fri Mar 20 08:59:05 2015 +0000
Revision:
0:ad375c052b4c
Child:
1:55e0aaf71bda
Figure 8 code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mawk2311 0:ad375c052b4c 1 #include "mbed.h"
mawk2311 0:ad375c052b4c 2 #include "stdlib.h"
mawk2311 0:ad375c052b4c 3
mawk2311 0:ad375c052b4c 4 //Outputs
mawk2311 0:ad375c052b4c 5 DigitalOut led1(LED1);
mawk2311 0:ad375c052b4c 6 DigitalOut clk(PTD5);
mawk2311 0:ad375c052b4c 7 DigitalOut si(PTD4);
mawk2311 0:ad375c052b4c 8 PwmOut motor1(PTA4);
mawk2311 0:ad375c052b4c 9 PwmOut motor2(PTA12);
mawk2311 0:ad375c052b4c 10 DigitalOut break1(PTC12);
mawk2311 0:ad375c052b4c 11 DigitalOut break2(PTC13);
mawk2311 0:ad375c052b4c 12 PwmOut servo(PTA5);
mawk2311 0:ad375c052b4c 13
mawk2311 0:ad375c052b4c 14 //Inputs
mawk2311 0:ad375c052b4c 15 AnalogIn camData(PTC2);
mawk2311 0:ad375c052b4c 16
mawk2311 0:ad375c052b4c 17 //Encoder setup and variables
mawk2311 0:ad375c052b4c 18 InterruptIn interrupt(PTA13);
mawk2311 0:ad375c052b4c 19
mawk2311 0:ad375c052b4c 20 //Line Tracking Variables --------------------------------
mawk2311 0:ad375c052b4c 21 float ADCdata [128];
mawk2311 0:ad375c052b4c 22 float minAccum;
mawk2311 0:ad375c052b4c 23 float minCount;
mawk2311 0:ad375c052b4c 24 float approxPos;
mawk2311 0:ad375c052b4c 25 float minVal;
mawk2311 0:ad375c052b4c 26 int minLoc;
mawk2311 0:ad375c052b4c 27
mawk2311 0:ad375c052b4c 28 //Servo turning parameters
mawk2311 0:ad375c052b4c 29 float straight = 0.00155f;
mawk2311 0:ad375c052b4c 30 float hardLeft = 0.0013f;
mawk2311 0:ad375c052b4c 31 float slightLeft = 0.00145f;
mawk2311 0:ad375c052b4c 32 float hardRight = 0.0018f;
mawk2311 0:ad375c052b4c 33 float slightRight = 0.00165f;
mawk2311 0:ad375c052b4c 34
mawk2311 0:ad375c052b4c 35 //End of Line Tracking Variables -------------------------
mawk2311 0:ad375c052b4c 36
mawk2311 0:ad375c052b4c 37 //Encoder and Motor Driver Variables ---------------------
mawk2311 0:ad375c052b4c 38
mawk2311 0:ad375c052b4c 39 //Intervals used during encoder data collection to measure velocity
mawk2311 0:ad375c052b4c 40 int interval1=0;
mawk2311 0:ad375c052b4c 41 int interval2=0;
mawk2311 0:ad375c052b4c 42 int interval3=0;
mawk2311 0:ad375c052b4c 43 int avg_interval=0;
mawk2311 0:ad375c052b4c 44 int lastchange1 = 0;
mawk2311 0:ad375c052b4c 45 int lastchange2 = 0;
mawk2311 0:ad375c052b4c 46 int lastchange3 = 0;
mawk2311 0:ad375c052b4c 47 int lastchange4 = 0;
mawk2311 0:ad375c052b4c 48
mawk2311 0:ad375c052b4c 49 //Variables used to for velocity control
mawk2311 0:ad375c052b4c 50 float avg_speed = 0;
mawk2311 0:ad375c052b4c 51 float stall_check = 0;
mawk2311 0:ad375c052b4c 52 float tuning_val = 1;
mawk2311 0:ad375c052b4c 53
mawk2311 0:ad375c052b4c 54 Timer t;
mawk2311 0:ad375c052b4c 55 Timer servoTimer;
mawk2311 0:ad375c052b4c 56
mawk2311 0:ad375c052b4c 57 //Observed average speeds for each duty cycle
mawk2311 0:ad375c052b4c 58 const float DESIRED_SPEED = 0.5;
mawk2311 0:ad375c052b4c 59 const float TUNING_CONSTANT_10 = 1.10;
mawk2311 0:ad375c052b4c 60 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 0:ad375c052b4c 61 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 0:ad375c052b4c 62 const float TUNING_CONSTANT_50 = 6.880;
mawk2311 0:ad375c052b4c 63 const float PI = 3.14159;
mawk2311 0:ad375c052b4c 64 const float WHEEL_CIRCUMFERENCE = .05*PI;
mawk2311 0:ad375c052b4c 65
mawk2311 0:ad375c052b4c 66 //Velocity Control Tuning Constants
mawk2311 0:ad375c052b4c 67 const float TUNE_THRESH = 0.5f;
mawk2311 0:ad375c052b4c 68 const float TUNE_AMT = 0.1f;
mawk2311 0:ad375c052b4c 69
mawk2311 0:ad375c052b4c 70 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 0:ad375c052b4c 71 float num_samples_small = 3.0f;
mawk2311 0:ad375c052b4c 72 float delay_small = 0.05f;
mawk2311 0:ad375c052b4c 73 float num_samples_large = 100.0f;
mawk2311 0:ad375c052b4c 74 float delay_large = 0.1f;
mawk2311 0:ad375c052b4c 75
mawk2311 0:ad375c052b4c 76 //Large and small arrays used to get average velocity values
mawk2311 0:ad375c052b4c 77 float large_avg_speed_list [100];
mawk2311 0:ad375c052b4c 78 float small_avg_speed_list [10];
mawk2311 0:ad375c052b4c 79
mawk2311 0:ad375c052b4c 80 //End of Encoder and Motor Driver Variables ----------------------
mawk2311 0:ad375c052b4c 81
mawk2311 0:ad375c052b4c 82 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 83
mawk2311 0:ad375c052b4c 84 float get_speed(){
mawk2311 0:ad375c052b4c 85 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
mawk2311 0:ad375c052b4c 86 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
mawk2311 0:ad375c052b4c 87 return linearSpeed;
mawk2311 0:ad375c052b4c 88 }
mawk2311 0:ad375c052b4c 89
mawk2311 0:ad375c052b4c 90 float get_avg_speed(float num_samples, float delay) {
mawk2311 0:ad375c052b4c 91
mawk2311 0:ad375c052b4c 92 float avg_avg_speed = 0;
mawk2311 0:ad375c052b4c 93
mawk2311 0:ad375c052b4c 94 for (int c = 0; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 95 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 96 small_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 97 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 98 large_avg_speed_list[c] = get_speed();
mawk2311 0:ad375c052b4c 99 }
mawk2311 0:ad375c052b4c 100 //wait(delay);
mawk2311 0:ad375c052b4c 101 }
mawk2311 0:ad375c052b4c 102
mawk2311 0:ad375c052b4c 103 for (int c = 1; c < num_samples; c++) {
mawk2311 0:ad375c052b4c 104 if (num_samples == num_samples_small){
mawk2311 0:ad375c052b4c 105 avg_avg_speed += small_avg_speed_list[c];
mawk2311 0:ad375c052b4c 106 } else if (num_samples == num_samples_large){
mawk2311 0:ad375c052b4c 107 avg_avg_speed += large_avg_speed_list[c];
mawk2311 0:ad375c052b4c 108 }
mawk2311 0:ad375c052b4c 109 }
mawk2311 0:ad375c052b4c 110 return avg_avg_speed/num_samples;
mawk2311 0:ad375c052b4c 111 }
mawk2311 0:ad375c052b4c 112
mawk2311 0:ad375c052b4c 113 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 0:ad375c052b4c 114
mawk2311 0:ad375c052b4c 115 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
mawk2311 0:ad375c052b4c 116
mawk2311 0:ad375c052b4c 117 if (avg_speed == stall_check) {
mawk2311 0:ad375c052b4c 118 avg_speed = 0;
mawk2311 0:ad375c052b4c 119 tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 120 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 0:ad375c052b4c 121 tuning_val -= TUNE_AMT;
mawk2311 0:ad375c052b4c 122 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 123 } else if (tuning_const - avg_speed > TUNE_THRESH){
mawk2311 0:ad375c052b4c 124 //tuning_val += TUNE_AMT;
mawk2311 0:ad375c052b4c 125 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 126 } else {
mawk2311 0:ad375c052b4c 127 //tuning_val = 1;
mawk2311 0:ad375c052b4c 128 stall_check = avg_speed;
mawk2311 0:ad375c052b4c 129 }
mawk2311 0:ad375c052b4c 130 motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 0:ad375c052b4c 131 motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 0:ad375c052b4c 132
mawk2311 0:ad375c052b4c 133 }
mawk2311 0:ad375c052b4c 134
mawk2311 0:ad375c052b4c 135 void fallInterrupt(){
mawk2311 0:ad375c052b4c 136
mawk2311 0:ad375c052b4c 137 int time = t.read_us();
mawk2311 0:ad375c052b4c 138 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 139 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 140 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 141 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 142
mawk2311 0:ad375c052b4c 143 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 144 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 145 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 146 lastchange1 = time;
mawk2311 0:ad375c052b4c 147 }
mawk2311 0:ad375c052b4c 148
mawk2311 0:ad375c052b4c 149 void riseInterrupt(){
mawk2311 0:ad375c052b4c 150 int time = t.read_us();
mawk2311 0:ad375c052b4c 151 interval1 = time - lastchange2;
mawk2311 0:ad375c052b4c 152 interval2 = lastchange1-lastchange3;
mawk2311 0:ad375c052b4c 153 interval3 = lastchange2 - lastchange4;
mawk2311 0:ad375c052b4c 154 avg_interval = (interval1 + interval2 + interval3)/3;
mawk2311 0:ad375c052b4c 155
mawk2311 0:ad375c052b4c 156 lastchange4 = lastchange3;
mawk2311 0:ad375c052b4c 157 lastchange3 = lastchange2;
mawk2311 0:ad375c052b4c 158 lastchange2 = lastchange1;
mawk2311 0:ad375c052b4c 159 lastchange1 = time;
mawk2311 0:ad375c052b4c 160 }
mawk2311 0:ad375c052b4c 161
mawk2311 0:ad375c052b4c 162
mawk2311 0:ad375c052b4c 163 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mawk2311 0:ad375c052b4c 164
mawk2311 0:ad375c052b4c 165 int main() {
mawk2311 0:ad375c052b4c 166 //Line Tracker Initializations
mawk2311 0:ad375c052b4c 167 int integrationCounter = 0;
mawk2311 0:ad375c052b4c 168
mawk2311 0:ad375c052b4c 169 // Motor Driver Initializations
mawk2311 0:ad375c052b4c 170 motor1.period(.0025);
mawk2311 0:ad375c052b4c 171 motor2.period(.0025);
mawk2311 0:ad375c052b4c 172 interrupt.fall(&fallInterrupt);
mawk2311 0:ad375c052b4c 173 interrupt.rise(&riseInterrupt);
mawk2311 0:ad375c052b4c 174
mawk2311 0:ad375c052b4c 175 //wait(5);
mawk2311 0:ad375c052b4c 176
mawk2311 0:ad375c052b4c 177 motor1.pulsewidth(.0025*.1);
mawk2311 0:ad375c052b4c 178 motor2.pulsewidth(.0025*.1);
mawk2311 0:ad375c052b4c 179 break1 = 0;
mawk2311 0:ad375c052b4c 180 break2 = 0;
mawk2311 0:ad375c052b4c 181
mawk2311 0:ad375c052b4c 182 t.start();
mawk2311 0:ad375c052b4c 183
mawk2311 0:ad375c052b4c 184 wait(5);
mawk2311 0:ad375c052b4c 185
mawk2311 0:ad375c052b4c 186 while(1) {
mawk2311 0:ad375c052b4c 187
mawk2311 0:ad375c052b4c 188 if(integrationCounter % 151== 0){
mawk2311 0:ad375c052b4c 189 //Disable interrupts
mawk2311 0:ad375c052b4c 190 //__disable_irq();
mawk2311 0:ad375c052b4c 191 //interrupt.fall(NULL);
mawk2311 0:ad375c052b4c 192 //interrupt.rise(NULL);
mawk2311 0:ad375c052b4c 193
mawk2311 0:ad375c052b4c 194 //Send start of integration signal
mawk2311 0:ad375c052b4c 195 si = 1;
mawk2311 0:ad375c052b4c 196 clk = 1;
mawk2311 0:ad375c052b4c 197
mawk2311 0:ad375c052b4c 198 si = 0;
mawk2311 0:ad375c052b4c 199 clk = 0;
mawk2311 0:ad375c052b4c 200
mawk2311 0:ad375c052b4c 201 //Reset timing counter for integration
mawk2311 0:ad375c052b4c 202 integrationCounter = 0;
mawk2311 0:ad375c052b4c 203
mawk2311 0:ad375c052b4c 204 //Reset line tracking variables
mawk2311 0:ad375c052b4c 205 minAccum = 0;
mawk2311 0:ad375c052b4c 206 minCount = 0;
mawk2311 0:ad375c052b4c 207 approxPos = 0;
mawk2311 0:ad375c052b4c 208
mawk2311 0:ad375c052b4c 209 //Reset Encoder and Motor Driver Variables
mawk2311 0:ad375c052b4c 210 interval1=0;
mawk2311 0:ad375c052b4c 211 interval2=0;
mawk2311 0:ad375c052b4c 212 interval3=0;
mawk2311 0:ad375c052b4c 213 avg_interval=0;
mawk2311 0:ad375c052b4c 214 lastchange1 = 0;
mawk2311 0:ad375c052b4c 215 lastchange2 = 0;
mawk2311 0:ad375c052b4c 216 lastchange3 = 0;
mawk2311 0:ad375c052b4c 217 lastchange4 = 0;
mawk2311 0:ad375c052b4c 218
mawk2311 0:ad375c052b4c 219 t.reset();
mawk2311 0:ad375c052b4c 220
mawk2311 0:ad375c052b4c 221 }
mawk2311 0:ad375c052b4c 222 else if (integrationCounter > 129){
mawk2311 0:ad375c052b4c 223 //Enable interrupts
mawk2311 0:ad375c052b4c 224 //__enable_irq();
mawk2311 0:ad375c052b4c 225 //interrupt.fall(&fallInterrupt);
mawk2311 0:ad375c052b4c 226 //interrupt.rise(&riseInterrupt);
mawk2311 0:ad375c052b4c 227
mawk2311 0:ad375c052b4c 228 minVal = ADCdata[15];
mawk2311 0:ad375c052b4c 229 for (int c = 15; c < 118; c++) {
mawk2311 0:ad375c052b4c 230 if (ADCdata[c] < minVal){
mawk2311 0:ad375c052b4c 231 minVal = ADCdata[c];
mawk2311 0:ad375c052b4c 232 minLoc = c;
mawk2311 0:ad375c052b4c 233 }
mawk2311 0:ad375c052b4c 234 }
mawk2311 0:ad375c052b4c 235
mawk2311 0:ad375c052b4c 236 for (int c = 15; c < 118; c++) {
mawk2311 0:ad375c052b4c 237 if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.04f && ADCdata[c] > 0.1f){
mawk2311 0:ad375c052b4c 238 minAccum += c;
mawk2311 0:ad375c052b4c 239 minCount++;
mawk2311 0:ad375c052b4c 240 }
mawk2311 0:ad375c052b4c 241 }
mawk2311 0:ad375c052b4c 242
mawk2311 0:ad375c052b4c 243 approxPos = (float)minAccum/(float)minCount;
mawk2311 0:ad375c052b4c 244
mawk2311 0:ad375c052b4c 245 if(approxPos > 0 && approxPos <= 20){
mawk2311 0:ad375c052b4c 246 servo.pulsewidth(hardLeft);
mawk2311 0:ad375c052b4c 247 } else if (approxPos > 20 && approxPos <= 45){
mawk2311 0:ad375c052b4c 248 servo.pulsewidth(slightLeft);
mawk2311 0:ad375c052b4c 249 } else if (approxPos > 45 && approxPos <= 90){
mawk2311 0:ad375c052b4c 250 servo.pulsewidth(straight);
mawk2311 0:ad375c052b4c 251 } else if (approxPos > 90 && approxPos <= 105){
mawk2311 0:ad375c052b4c 252 servo.pulsewidth(slightRight);
mawk2311 0:ad375c052b4c 253 } else if (approxPos > 105 && approxPos <= 128){
mawk2311 0:ad375c052b4c 254 servo.pulsewidth(hardRight);
mawk2311 0:ad375c052b4c 255 }
mawk2311 0:ad375c052b4c 256
mawk2311 0:ad375c052b4c 257 velocity_control(0.05f, DESIRED_SPEED);
mawk2311 0:ad375c052b4c 258
mawk2311 0:ad375c052b4c 259 integrationCounter = 150;
mawk2311 0:ad375c052b4c 260 }
mawk2311 0:ad375c052b4c 261 else{
mawk2311 0:ad375c052b4c 262 clk = 1;
mawk2311 0:ad375c052b4c 263 wait_us(70);
mawk2311 0:ad375c052b4c 264 ADCdata[integrationCounter - 1] = camData;
mawk2311 0:ad375c052b4c 265 clk = 0;
mawk2311 0:ad375c052b4c 266 }
mawk2311 0:ad375c052b4c 267
mawk2311 0:ad375c052b4c 268 //clk = 0;
mawk2311 0:ad375c052b4c 269 integrationCounter++;
mawk2311 0:ad375c052b4c 270 //camData.
mawk2311 0:ad375c052b4c 271
mawk2311 0:ad375c052b4c 272 }
mawk2311 0:ad375c052b4c 273 }