u_guessed_it / Mbed 2 deprecated red_rover

init commit

Dependencies:   mbed MODSERIAL telemetry-master

main.cpp@5:868e652f26af, 2015-05-11 (annotated)

Committer:
ericoneill
Date:
Mon May 11 03:40:59 2015 +0000
Revision:
5:868e652f26af
Parent:
4:8ee76f6d32b5 
cleaned code a little

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cheryl_he 0:453d27750ab3 1 #include "mbed.h"
cheryl_he 1:ff0fc50372c8 2 #include "telemetry.h"
cheryl_he 1:ff0fc50372c8 3 #include "telemetry-mbed.h"
cheryl_he 3:39e6440ccd45 4 #include "MODSERIAL.h"
cheryl_he 1:ff0fc50372c8 5
cheryl_he 3:39e6440ccd45 6 #include "stdlib.h"
cheryl_he 3:39e6440ccd45 7 #include <vector>
cheryl_he 3:39e6440ccd45 8
cheryl_he 4:8ee76f6d32b5 9 //MODSERIAL telemetry_serial(PTA2, PTA1);
cheryl_he 4:8ee76f6d32b5 10
cheryl_he 3:39e6440ccd45 11 MODSERIAL telemetry_serial(USBTX, USBRX);
cheryl_he 4:8ee76f6d32b5 12
cheryl_he 1:ff0fc50372c8 13 telemetry::MbedHal telemetry_hal(telemetry_serial);
cheryl_he 1:ff0fc50372c8 14 telemetry::Telemetry telemetry_obj(telemetry_hal);
cheryl_he 1:ff0fc50372c8 15
cheryl_he 1:ff0fc50372c8 16 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
cheryl_he 4:8ee76f6d32b5 17 telemetry::NumericArray<uint8_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
cheryl_he 1:ff0fc50372c8 18 telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor", "Motor PWM", "%DC", 0);
cheryl_he 0:453d27750ab3 19
cheryl_he 3:39e6440ccd45 20 //Outputs
cheryl_he 4:8ee76f6d32b5 21 DigitalOut led1(LED1);
cheryl_he 3:39e6440ccd45 22 DigitalOut clk(PTD5);
cheryl_he 3:39e6440ccd45 23 DigitalOut si(PTD4);
cheryl_he 3:39e6440ccd45 24 PwmOut motor1(PTA12);
cheryl_he 3:39e6440ccd45 25 PwmOut motor2(PTA4);
cheryl_he 3:39e6440ccd45 26 DigitalOut break1(PTC7);
cheryl_he 3:39e6440ccd45 27 DigitalOut break2(PTC0);
cheryl_he 3:39e6440ccd45 28 PwmOut servo(PTA5);
cheryl_he 3:39e6440ccd45 29
cheryl_he 4:8ee76f6d32b5 30 //Serial bt(PTA2, PTA1);
cheryl_he 3:39e6440ccd45 31 //Serial pc(USBTX, USBRX); // tx, rx
cheryl_he 3:39e6440ccd45 32
cheryl_he 3:39e6440ccd45 33 //Inputs
cheryl_he 3:39e6440ccd45 34 AnalogIn camData(PTC2);
cheryl_he 3:39e6440ccd45 35
cheryl_he 3:39e6440ccd45 36 //Encoder setup and variables
cheryl_he 3:39e6440ccd45 37 InterruptIn interrupt(PTA13);
cheryl_he 3:39e6440ccd45 38
cheryl_he 4:8ee76f6d32b5 39 //Line Tracking Variables --------------------------------
cheryl_he 4:8ee76f6d32b5 40 float ADCdata [128];
cheryl_he 4:8ee76f6d32b5 41 float maxAccum;
cheryl_he 4:8ee76f6d32b5 42 float maxCount;
cheryl_he 4:8ee76f6d32b5 43 float approxPos;
cheryl_he 4:8ee76f6d32b5 44 float prevApproxPos;
cheryl_he 4:8ee76f6d32b5 45 int trackWindow = 30;
cheryl_he 4:8ee76f6d32b5 46 int startWindow;
cheryl_he 4:8ee76f6d32b5 47 int endWindow;
cheryl_he 4:8ee76f6d32b5 48 float maxVal;
cheryl_he 4:8ee76f6d32b5 49 int maxLoc;
cheryl_he 4:8ee76f6d32b5 50
cheryl_he 4:8ee76f6d32b5 51 bool firstTime;
cheryl_he 4:8ee76f6d32b5 52
cheryl_he 4:8ee76f6d32b5 53 //Data Collection
cheryl_he 4:8ee76f6d32b5 54 bool dataCol = false;
cheryl_he 4:8ee76f6d32b5 55 int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime;
cheryl_he 4:8ee76f6d32b5 56
cheryl_he 4:8ee76f6d32b5 57 //Line Crossing variables
cheryl_he 4:8ee76f6d32b5 58 int prevTrackLoc;
cheryl_he 4:8ee76f6d32b5 59 int spaceThresh = 3;
cheryl_he 4:8ee76f6d32b5 60 int widthThresh = 10;
cheryl_he 4:8ee76f6d32b5 61 bool space;
cheryl_he 4:8ee76f6d32b5 62
cheryl_he 4:8ee76f6d32b5 63 //Servo turning parameters
cheryl_he 4:8ee76f6d32b5 64 float straight = 0.00155f;
cheryl_he 4:8ee76f6d32b5 65 float hardLeft = 0.0012f;
cheryl_he 4:8ee76f6d32b5 66 float hardRight = 0.0020f;
cheryl_he 4:8ee76f6d32b5 67 //float hardLeft = 0.0010f;
cheryl_he 4:8ee76f6d32b5 68 //float hardRight = 0.00195f;
cheryl_he 4:8ee76f6d32b5 69
cheryl_he 4:8ee76f6d32b5 70 //Servo Directions
cheryl_he 4:8ee76f6d32b5 71 float currDir;
cheryl_he 4:8ee76f6d32b5 72 float prevDir;
cheryl_he 4:8ee76f6d32b5 73
cheryl_he 4:8ee76f6d32b5 74 // All linescan data for the period the car is running on. To be printed after a set amount of time
cheryl_he 4:8ee76f6d32b5 75 //std::vector<std::vector<int> > frames;
cheryl_he 4:8ee76f6d32b5 76 const int numData = 1000;
cheryl_he 4:8ee76f6d32b5 77 int lineCenters [numData] = {0};
cheryl_he 4:8ee76f6d32b5 78 int times [numData] = {0};
cheryl_he 4:8ee76f6d32b5 79 int loopCtr = 0;
cheryl_he 3:39e6440ccd45 80
cheryl_he 4:8ee76f6d32b5 81 //End of Line Tracking Variables -------------------------
cheryl_he 4:8ee76f6d32b5 82
cheryl_he 4:8ee76f6d32b5 83 //Encoder and Motor Driver Variables ---------------------
cheryl_he 4:8ee76f6d32b5 84
cheryl_he 4:8ee76f6d32b5 85 //Intervals used during encoder data collection to measure velocity
cheryl_he 4:8ee76f6d32b5 86 int interval1=0;
cheryl_he 4:8ee76f6d32b5 87 int interval2=0;
cheryl_he 4:8ee76f6d32b5 88 int interval3=0;
cheryl_he 4:8ee76f6d32b5 89 int avg_interval=0;
cheryl_he 4:8ee76f6d32b5 90 int lastchange1 = 0;
cheryl_he 4:8ee76f6d32b5 91 int lastchange2 = 0;
cheryl_he 4:8ee76f6d32b5 92 int lastchange3 = 0;
cheryl_he 4:8ee76f6d32b5 93 int lastchange4 = 0;
cheryl_he 4:8ee76f6d32b5 94
cheryl_he 4:8ee76f6d32b5 95 //Variables used to for velocity control
cheryl_he 4:8ee76f6d32b5 96 float avg_speed = 0;
cheryl_he 4:8ee76f6d32b5 97 float last_speed = 0;
cheryl_he 4:8ee76f6d32b5 98
cheryl_he 4:8ee76f6d32b5 99 float stall_check = 0;
cheryl_he 4:8ee76f6d32b5 100 float tuning_val = 1;
cheryl_he 3:39e6440ccd45 101
cheryl_he 4:8ee76f6d32b5 102 int numInterrupts = 0;
cheryl_he 4:8ee76f6d32b5 103
cheryl_he 4:8ee76f6d32b5 104 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~
cheryl_he 4:8ee76f6d32b5 105 float pulsewidth = 0.2f; //0.2f
cheryl_he 4:8ee76f6d32b5 106 int intTimMod = 0;
cheryl_he 4:8ee76f6d32b5 107 float maxValThresh = .1; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker
cheryl_he 4:8ee76f6d32b5 108 bool turnSpeedControl = true; //have increased PWMs when turning when true.
cheryl_he 4:8ee76f6d32b5 109 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
cheryl_he 4:8ee76f6d32b5 110
cheryl_he 4:8ee76f6d32b5 111 // Hardware periods
cheryl_he 4:8ee76f6d32b5 112 float motorPeriod = .0018;
cheryl_he 4:8ee76f6d32b5 113 float servoPeriod = .0025;
cheryl_he 4:8ee76f6d32b5 114
cheryl_he 4:8ee76f6d32b5 115 Timer t;
cheryl_he 4:8ee76f6d32b5 116 Timer servoTimer;
cheryl_he 4:8ee76f6d32b5 117 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
cheryl_he 4:8ee76f6d32b5 118
cheryl_he 4:8ee76f6d32b5 119 //End of Encoder and Motor Driver Variables ----------------------
cheryl_he 4:8ee76f6d32b5 120
cheryl_he 4:8ee76f6d32b5 121 //Function for speeding up KL25Z ADC
cheryl_he 3:39e6440ccd45 122 void initADC(void){
cheryl_he 3:39e6440ccd45 123
cheryl_he 3:39e6440ccd45 124 ADC0->CFG1 = ADC0->CFG1 & (
cheryl_he 3:39e6440ccd45 125 ~(
cheryl_he 3:39e6440ccd45 126 0x80 // LDLPC = 0 ; no low-power mode
cheryl_he 3:39e6440ccd45 127 | 0x60 // ADIV = 1
cheryl_he 3:39e6440ccd45 128 | 0x10 // Sample time short
cheryl_he 3:39e6440ccd45 129 | 0x03 // input clock = BUS CLK
cheryl_he 3:39e6440ccd45 130 )
cheryl_he 3:39e6440ccd45 131 ) ; // clkdiv <= 1
cheryl_he 3:39e6440ccd45 132 ADC0->CFG2 = ADC0->CFG2
cheryl_he 3:39e6440ccd45 133 | 0x03 ; // Logsample Time 11 = 2 extra ADCK
cheryl_he 3:39e6440ccd45 134 ADC0->SC3 = ADC0->SC3
cheryl_he 3:39e6440ccd45 135 & (~(0x03)) ; // hardware avarage off
cheryl_he 3:39e6440ccd45 136 }
cheryl_he 0:453d27750ab3 137
cheryl_he 0:453d27750ab3 138 int main() {
cheryl_he 3:39e6440ccd45 139 telemetry_serial.baud(115200);
cheryl_he 1:ff0fc50372c8 140 telemetry_obj.transmit_header();
cheryl_he 3:39e6440ccd45 141
cheryl_he 3:39e6440ccd45 142 //Alter reg values to speed up KL25Z
cheryl_he 3:39e6440ccd45 143 initADC();
cheryl_he 3:39e6440ccd45 144
cheryl_he 4:8ee76f6d32b5 145 //Line Tracker Initializations
cheryl_he 4:8ee76f6d32b5 146 int integrationCounter = 0;
cheryl_he 4:8ee76f6d32b5 147
cheryl_he 4:8ee76f6d32b5 148 //Initial values for directions
cheryl_he 4:8ee76f6d32b5 149 currDir = 0;
cheryl_he 4:8ee76f6d32b5 150 prevDir = 0;
cheryl_he 4:8ee76f6d32b5 151
cheryl_he 4:8ee76f6d32b5 152 // Motor Driver Initializations
cheryl_he 4:8ee76f6d32b5 153 motor1.period(motorPeriod);
cheryl_he 4:8ee76f6d32b5 154 motor2.period(motorPeriod);
cheryl_he 4:8ee76f6d32b5 155
cheryl_he 4:8ee76f6d32b5 156 // Servo Initialization
cheryl_he 4:8ee76f6d32b5 157 servo.period(servoPeriod);
cheryl_he 4:8ee76f6d32b5 158 servo.pulsewidth(hardRight);
cheryl_he 4:8ee76f6d32b5 159 wait(3);
cheryl_he 4:8ee76f6d32b5 160
cheryl_he 4:8ee76f6d32b5 161 motor1.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 162 motor2.pulsewidth(motorPeriod*pulsewidth);
cheryl_he 4:8ee76f6d32b5 163 break1 = 0;
cheryl_he 4:8ee76f6d32b5 164 break2 = 0;
cheryl_he 4:8ee76f6d32b5 165
cheryl_he 4:8ee76f6d32b5 166 firstTime = true;
cheryl_he 4:8ee76f6d32b5 167
cheryl_he 3:39e6440ccd45 168 t.start();
cheryl_he 1:ff0fc50372c8 169
ericoneill 5:868e652f26af 170
cheryl_he 4:8ee76f6d32b5 171 //uint16_t* data = camData.read();
cheryl_he 0:453d27750ab3 172 while(1) {
cheryl_he 4:8ee76f6d32b5 173 if(dataCol){
cheryl_he 4:8ee76f6d32b5 174 //break out of main loop if enough time has passed;
cheryl_he 4:8ee76f6d32b5 175 if(loopCtr >= numData && dataCol){
cheryl_he 4:8ee76f6d32b5 176 break;
cheryl_he 4:8ee76f6d32b5 177 }
cheryl_he 4:8ee76f6d32b5 178 }
cheryl_he 4:8ee76f6d32b5 179 if(integrationCounter % 151== 0){
ericoneill 5:868e652f26af 180
cheryl_he 4:8ee76f6d32b5 181 //Send start of integration signal
cheryl_he 4:8ee76f6d32b5 182 si = 1;
cheryl_he 4:8ee76f6d32b5 183 clk = 1;
cheryl_he 4:8ee76f6d32b5 184
cheryl_he 4:8ee76f6d32b5 185 si = 0;
cheryl_he 4:8ee76f6d32b5 186 clk = 0;
cheryl_he 4:8ee76f6d32b5 187
cheryl_he 4:8ee76f6d32b5 188 //Reset timing counter for integration
cheryl_he 4:8ee76f6d32b5 189 integrationCounter = 0;
cheryl_he 4:8ee76f6d32b5 190
cheryl_he 4:8ee76f6d32b5 191 //Reset line tracking variables
cheryl_he 4:8ee76f6d32b5 192 maxAccum = 0;
cheryl_he 4:8ee76f6d32b5 193 maxCount = 0;
cheryl_he 4:8ee76f6d32b5 194 approxPos = 0;
cheryl_he 4:8ee76f6d32b5 195
cheryl_he 4:8ee76f6d32b5 196 space = false;
cheryl_he 4:8ee76f6d32b5 197
cheryl_he 1:ff0fc50372c8 198 }
cheryl_he 4:8ee76f6d32b5 199 else if (integrationCounter > 129){
cheryl_he 4:8ee76f6d32b5 200 //Start Timer
cheryl_he 4:8ee76f6d32b5 201 //t.start();
cheryl_he 4:8ee76f6d32b5 202
cheryl_he 4:8ee76f6d32b5 203 tele_time_ms = t.read_ms();
cheryl_he 4:8ee76f6d32b5 204 for (uint16_t i=0; i<128; i++) {
cheryl_he 4:8ee76f6d32b5 205 tele_linescan[i] = (uint8_t) (ADCdata[i] * 128);
cheryl_he 4:8ee76f6d32b5 206 }
cheryl_he 4:8ee76f6d32b5 207 //telemetry_obj.do_io();
cheryl_he 4:8ee76f6d32b5 208 if (firstTime){
cheryl_he 4:8ee76f6d32b5 209
cheryl_he 4:8ee76f6d32b5 210 maxVal = ADCdata[10];
cheryl_he 4:8ee76f6d32b5 211 for (int c = 11; c < 118; c++) {
cheryl_he 4:8ee76f6d32b5 212 if (ADCdata[c] > maxVal){
cheryl_he 4:8ee76f6d32b5 213 maxVal = ADCdata[c];
cheryl_he 4:8ee76f6d32b5 214 maxLoc = c;
cheryl_he 4:8ee76f6d32b5 215 }
cheryl_he 4:8ee76f6d32b5 216 }
cheryl_he 4:8ee76f6d32b5 217 for (int c = 10; c < 118; c++) {
cheryl_he 4:8ee76f6d32b5 218 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
cheryl_he 4:8ee76f6d32b5 219 maxAccum += c;
cheryl_he 4:8ee76f6d32b5 220 maxCount++;
cheryl_he 4:8ee76f6d32b5 221 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
cheryl_he 4:8ee76f6d32b5 222 space = true;
cheryl_he 4:8ee76f6d32b5 223 }
cheryl_he 4:8ee76f6d32b5 224 prevTrackLoc = c;
cheryl_he 4:8ee76f6d32b5 225 }
cheryl_he 4:8ee76f6d32b5 226 }
cheryl_he 4:8ee76f6d32b5 227
cheryl_he 4:8ee76f6d32b5 228 //firstTime = false;
cheryl_he 4:8ee76f6d32b5 229 } else {
cheryl_he 4:8ee76f6d32b5 230
cheryl_he 4:8ee76f6d32b5 231 startWindow = prevApproxPos - trackWindow;
cheryl_he 4:8ee76f6d32b5 232 endWindow = prevApproxPos + trackWindow;
cheryl_he 4:8ee76f6d32b5 233 if (startWindow < 0){
cheryl_he 4:8ee76f6d32b5 234 startWindow = 0;
cheryl_he 4:8ee76f6d32b5 235 }
cheryl_he 4:8ee76f6d32b5 236 if (endWindow > 118){
cheryl_he 4:8ee76f6d32b5 237 endWindow = 118;
cheryl_he 4:8ee76f6d32b5 238 }
cheryl_he 4:8ee76f6d32b5 239 maxVal = ADCdata[10];
cheryl_he 4:8ee76f6d32b5 240 for (int c = startWindow; c < endWindow; c++) {
cheryl_he 4:8ee76f6d32b5 241 if (ADCdata[c] > maxVal){
cheryl_he 4:8ee76f6d32b5 242 maxVal = ADCdata[c];
cheryl_he 4:8ee76f6d32b5 243 maxLoc = c;
cheryl_he 4:8ee76f6d32b5 244 }
cheryl_he 4:8ee76f6d32b5 245 }
cheryl_he 4:8ee76f6d32b5 246
cheryl_he 4:8ee76f6d32b5 247 for (int c = startWindow; c < endWindow; c++) {
cheryl_he 4:8ee76f6d32b5 248 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
cheryl_he 4:8ee76f6d32b5 249 maxAccum += c;
cheryl_he 4:8ee76f6d32b5 250 maxCount++;
cheryl_he 4:8ee76f6d32b5 251 if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
cheryl_he 4:8ee76f6d32b5 252 space = true;
cheryl_he 4:8ee76f6d32b5 253 }
cheryl_he 4:8ee76f6d32b5 254 prevTrackLoc = c;
cheryl_he 4:8ee76f6d32b5 255 }
cheryl_he 4:8ee76f6d32b5 256 }
cheryl_he 4:8ee76f6d32b5 257 }
cheryl_he 4:8ee76f6d32b5 258 //Line Crossing Checks
cheryl_he 4:8ee76f6d32b5 259 if (space) {
cheryl_he 4:8ee76f6d32b5 260 currDir = prevDir;
cheryl_he 4:8ee76f6d32b5 261 firstTime = true;
cheryl_he 4:8ee76f6d32b5 262 } else {
ericoneill 5:868e652f26af 263 approxPos = (float)maxAccum/(float)maxCount;
cheryl_he 4:8ee76f6d32b5 264 currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
cheryl_he 4:8ee76f6d32b5 265 prevApproxPos = approxPos;
cheryl_he 4:8ee76f6d32b5 266
ericoneill 5:868e652f26af 267 }
cheryl_he 4:8ee76f6d32b5 268 servo.pulsewidth(currDir);
cheryl_he 4:8ee76f6d32b5 269
cheryl_he 4:8ee76f6d32b5 270 //Save current direction as previous direction
cheryl_he 4:8ee76f6d32b5 271 prevDir = currDir;
cheryl_he 4:8ee76f6d32b5 272
cheryl_he 4:8ee76f6d32b5 273 //Prepare to start collecting more data
cheryl_he 4:8ee76f6d32b5 274 integrationCounter = 150;
cheryl_he 4:8ee76f6d32b5 275
cheryl_he 4:8ee76f6d32b5 276 //Stop timer
cheryl_he 4:8ee76f6d32b5 277 //t.stop();
cheryl_he 3:39e6440ccd45 278 }
cheryl_he 4:8ee76f6d32b5 279 else{
cheryl_he 4:8ee76f6d32b5 280 clk = 1;
cheryl_he 4:8ee76f6d32b5 281 wait_us(intTimMod);
cheryl_he 4:8ee76f6d32b5 282 ADCdata[integrationCounter - 1] = camData;
cheryl_he 4:8ee76f6d32b5 283 clk = 0;
cheryl_he 4:8ee76f6d32b5 284 }
cheryl_he 4:8ee76f6d32b5 285
cheryl_he 1:ff0fc50372c8 286 telemetry_obj.do_io();
cheryl_he 4:8ee76f6d32b5 287
cheryl_he 1:ff0fc50372c8 288
cheryl_he 0:453d27750ab3 289 }
ericoneill 5:868e652f26af 290
cheryl_he 0:453d27750ab3 291 }
cheryl_he 4:8ee76f6d32b5 292