E=MC / Mbed 2 deprecated figure_eight

Code for the car to drive in a figure eight motion

Dependencies:   mbed-rtos mbed MODSERIAL mbed-dsp telemetry

main.cpp@23:d8cacd996cce, 2015-03-20 (annotated)

Committer:
cheryl_he
Date:
Fri Mar 20 09:49:57 2015 +0000
Revision:
23:d8cacd996cce
Parent:
20:53f5e6d3e47b 
telemetry stuff is compiling, needs to be debugged

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 0:2002d645d2a6 1 #include "mbed.h"
ericoneill 2:3eb545c3e6a1 2 #include "rtos.h"
cheryl_he 23:d8cacd996cce 3 #include "MODSERIAL.h"
cheryl_he 14:09d61b20b102 4 #include "telemetry.h"
cheryl_he 23:d8cacd996cce 5 #include "telemetry-mbed.h"
ericoneill 0:2002d645d2a6 6
ericoneill 2:3eb545c3e6a1 7 // Camera Pins
cheryl_he 23:d8cacd996cce 8 DigitalOut clk(PTA13);
ericoneill 2:3eb545c3e6a1 9 DigitalOut si(PTD4);
ericoneill 2:3eb545c3e6a1 10 AnalogIn camData(PTC2);
cheryl_he 23:d8cacd996cce 11
ericoneill 2:3eb545c3e6a1 12 //Servo, Motor, Serial Pins
ericoneill 2:3eb545c3e6a1 13 PwmOut servo(PTA5);
cheryl_he 10:85f486ad4e5f 14 PwmOut motor1(PTA4);
cheryl_he 10:85f486ad4e5f 15 PwmOut motor2(PTA12);
cheryl_he 10:85f486ad4e5f 16 DigitalOut break1(PTC12);
cheryl_he 10:85f486ad4e5f 17 DigitalOut break2(PTC13);
ericoneill 2:3eb545c3e6a1 18 Serial pc(USBTX, USBRX);
cheryl_he 23:d8cacd996cce 19
cheryl_he 23:d8cacd996cce 20 //Telemetry
cheryl_he 23:d8cacd996cce 21 MODSERIAL telemetry_serial(PTA2, PTA1);
cheryl_he 23:d8cacd996cce 22 telemetry::MbedHal telemetry_hal(telemetry_serial);
cheryl_he 23:d8cacd996cce 23 telemetry::Telemetry telemetry_obj(telemetry_hal);
cheryl_he 23:d8cacd996cce 24 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
cheryl_he 23:d8cacd996cce 25 telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
cheryl_he 23:d8cacd996cce 26 telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor1", "Motor PWM", "%DC", 0);
cheryl_he 23:d8cacd996cce 27
cheryl_he 23:d8cacd996cce 28
ericoneill 2:3eb545c3e6a1 29 // Interrupt for encoder
ericoneill 2:3eb545c3e6a1 30 InterruptIn interrupt(PTA13);
ericoneill 19:29b3decea5e2 31 const int maxFrames = 3;
ericoneill 18:16f1297a4260 32 float frames[maxFrames][128];
ericoneill 18:16f1297a4260 33 float ADCdata [128];
ericoneill 12:45e6cb021301 34 Mutex line_mutex;
ericoneill 4:03594bf9a0de 35
ericoneill 20:53f5e6d3e47b 36
ericoneill 20:53f5e6d3e47b 37 //servo rotations
ericoneill 20:53f5e6d3e47b 38 float straight = 0.00155f;
ericoneill 20:53f5e6d3e47b 39 float hardLeft = 0.0013f;
ericoneill 20:53f5e6d3e47b 40 float slightLeft = 0.00145f;
ericoneill 20:53f5e6d3e47b 41 float hardRight = 0.0018f;
ericoneill 20:53f5e6d3e47b 42 float slightRight = 0.00165f;
ericoneill 20:53f5e6d3e47b 43
ericoneill 20:53f5e6d3e47b 44
ericoneill 4:03594bf9a0de 45 void linecam_thread(void const *args){
cheryl_he 23:d8cacd996cce 46 tele_motor_pwm.set_limits(0, 1);
cheryl_he 23:d8cacd996cce 47 telemetry_obj.transmit_header();
ericoneill 13:97708869a4ba 48 while(1){
ericoneill 18:16f1297a4260 49 //line_mutex.lock();
ericoneill 13:97708869a4ba 50 pc.printf("[");
ericoneill 13:97708869a4ba 51 for(int i=0; i<128; i++){
ericoneill 18:16f1297a4260 52 //pc.printf("%f",ADCdata[i]);
ericoneill 13:97708869a4ba 53 pc.printf(" ");
ericoneill 13:97708869a4ba 54 }
ericoneill 13:97708869a4ba 55 pc.printf("]");
ericoneill 18:16f1297a4260 56 //line_mutex.unlock();
ericoneill 13:97708869a4ba 57 }
ericoneill 5:764c2ffb523a 58
ericoneill 4:03594bf9a0de 59 }
ericoneill 4:03594bf9a0de 60
ericoneill 19:29b3decea5e2 61 int find_track(float line[]){
ericoneill 19:29b3decea5e2 62 int track_location = -1;
ericoneill 19:29b3decea5e2 63 float slope_threshold = .05;
ericoneill 19:29b3decea5e2 64 bool downslope_indices [128] = {false};
ericoneill 19:29b3decea5e2 65 bool upslope_indices [128] = {false};
ericoneill 19:29b3decea5e2 66 for(int i=10; i<118; i++){
ericoneill 19:29b3decea5e2 67 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
ericoneill 19:29b3decea5e2 68 downslope_indices[i] = true;
ericoneill 19:29b3decea5e2 69 }
ericoneill 19:29b3decea5e2 70 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
ericoneill 19:29b3decea5e2 71 upslope_indices[i] = true;
ericoneill 19:29b3decea5e2 72 }
ericoneill 19:29b3decea5e2 73 }
ericoneill 19:29b3decea5e2 74 int numDownslopes = 0;
ericoneill 19:29b3decea5e2 75 int numUpslopes = 0;
ericoneill 19:29b3decea5e2 76 for(int i=0; i<128; i++){
ericoneill 19:29b3decea5e2 77 if(downslope_indices[i] == true){
ericoneill 19:29b3decea5e2 78 numDownslopes ++;
ericoneill 19:29b3decea5e2 79 }
ericoneill 19:29b3decea5e2 80 if(upslope_indices[i] == true){
ericoneill 19:29b3decea5e2 81 numUpslopes ++;
ericoneill 19:29b3decea5e2 82 }
ericoneill 19:29b3decea5e2 83 }
ericoneill 19:29b3decea5e2 84 int downslope_locs [numDownslopes];
ericoneill 19:29b3decea5e2 85 int upslope_locs [numUpslopes];
ericoneill 19:29b3decea5e2 86 int dsctr = 0;
ericoneill 19:29b3decea5e2 87 int usctr = 0;
ericoneill 19:29b3decea5e2 88
ericoneill 19:29b3decea5e2 89 for (int i=0; i<128; i++){
ericoneill 19:29b3decea5e2 90 if(downslope_indices[i] == true){
ericoneill 19:29b3decea5e2 91 downslope_locs[dsctr] = i;
ericoneill 19:29b3decea5e2 92 dsctr++;
ericoneill 19:29b3decea5e2 93 }
ericoneill 19:29b3decea5e2 94 if(upslope_indices[i] == true){
ericoneill 19:29b3decea5e2 95 upslope_locs[usctr] = i;
ericoneill 19:29b3decea5e2 96 usctr++;
ericoneill 19:29b3decea5e2 97 }
ericoneill 19:29b3decea5e2 98 }
ericoneill 19:29b3decea5e2 99
ericoneill 19:29b3decea5e2 100 for(int i=0; i<numDownslopes; i++){
ericoneill 19:29b3decea5e2 101 for(int j=0; j<numUpslopes; j++){
ericoneill 19:29b3decea5e2 102 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
ericoneill 19:29b3decea5e2 103 track_location = downslope_locs[i] + 2 ;
ericoneill 19:29b3decea5e2 104 }
ericoneill 19:29b3decea5e2 105 }
ericoneill 19:29b3decea5e2 106 }
ericoneill 19:29b3decea5e2 107
ericoneill 19:29b3decea5e2 108 pc.printf("Downslopes at: [");
ericoneill 19:29b3decea5e2 109 for(int i = 0; i<numDownslopes; i++){
ericoneill 19:29b3decea5e2 110 pc.printf("%i ", downslope_locs[i]);
ericoneill 19:29b3decea5e2 111
ericoneill 19:29b3decea5e2 112 }
ericoneill 19:29b3decea5e2 113 pc.printf("]\n\r");
ericoneill 19:29b3decea5e2 114 pc.printf("Upslopes at: [");
ericoneill 19:29b3decea5e2 115 for(int i=0; i<numUpslopes; i++){
ericoneill 19:29b3decea5e2 116 pc.printf("%i ", upslope_locs[i]);
ericoneill 19:29b3decea5e2 117 }
ericoneill 19:29b3decea5e2 118 pc.printf("]\n\r");
ericoneill 19:29b3decea5e2 119 return track_location;
ericoneill 19:29b3decea5e2 120 }
ericoneill 4:03594bf9a0de 121
ericoneill 0:2002d645d2a6 122 int main() {
ericoneill 18:16f1297a4260 123 //Thread thread(linecam_thread);
ericoneill 18:16f1297a4260 124 //thread.set_priority(osPriorityIdle);
ericoneill 19:29b3decea5e2 125 pc.printf("Beginning linecam data acquisition... \n\r");
ericoneill 18:16f1297a4260 126 for(int numFrames = 0; numFrames < maxFrames; numFrames++){
ericoneill 18:16f1297a4260 127 for(int integrationCounter = 0;integrationCounter < 151;) {
ericoneill 18:16f1297a4260 128 if(integrationCounter % 151== 0){
ericoneill 18:16f1297a4260 129 si = 1;
ericoneill 18:16f1297a4260 130 clk = 1;
ericoneill 18:16f1297a4260 131 //wait(.00001);
ericoneill 18:16f1297a4260 132 si = 0;
ericoneill 18:16f1297a4260 133 clk = 0;
ericoneill 18:16f1297a4260 134 integrationCounter = 0;
ericoneill 18:16f1297a4260 135
ericoneill 18:16f1297a4260 136
ericoneill 18:16f1297a4260 137 }
ericoneill 18:16f1297a4260 138 else if (integrationCounter > 129){
cheryl_he 23:d8cacd996cce 139 //uint16_t * data = camData.read_u16();
cheryl_he 23:d8cacd996cce 140 for (uint16_t i =0; i < 128; i++) {
cheryl_he 23:d8cacd996cce 141 tele_linescan[i] = camData.read_u16();
cheryl_he 23:d8cacd996cce 142 }
cheryl_he 23:d8cacd996cce 143 telemetry_obj.do_io();
cheryl_he 23:d8cacd996cce 144 motor1.write(tele_motor_pwm);
ericoneill 18:16f1297a4260 145 integrationCounter = 150;
ericoneill 18:16f1297a4260 146 }
ericoneill 18:16f1297a4260 147 else{
ericoneill 18:16f1297a4260 148 clk = 1;
ericoneill 18:16f1297a4260 149 wait_us(50);
ericoneill 18:16f1297a4260 150 //line_mutex.lock();
ericoneill 19:29b3decea5e2 151 frames[numFrames][integrationCounter - 1] = 1-camData; // 1- is for light line
ericoneill 18:16f1297a4260 152 //line_mutex.unlock();
ericoneill 18:16f1297a4260 153 clk = 0;
ericoneill 18:16f1297a4260 154 }
ericoneill 12:45e6cb021301 155
ericoneill 18:16f1297a4260 156 integrationCounter++;
ericoneill 16:fa13ac00f583 157
ericoneill 16:fa13ac00f583 158 }
ericoneill 18:16f1297a4260 159 //frames[numFrames] = ADCdata;
ericoneill 18:16f1297a4260 160 }
ericoneill 18:16f1297a4260 161
ericoneill 18:16f1297a4260 162 for(int i =0; i<maxFrames; i++){
ericoneill 19:29b3decea5e2 163 pc.printf("LINE %i: [",i);
ericoneill 19:29b3decea5e2 164
ericoneill 19:29b3decea5e2 165
ericoneill 18:16f1297a4260 166 for(int j = 0; j < 128; j++){
ericoneill 18:16f1297a4260 167 pc.printf(" %f",frames[i][j]);
ericoneill 16:fa13ac00f583 168 }
ericoneill 18:16f1297a4260 169 pc.printf("]\n\r");
ericoneill 19:29b3decea5e2 170 float* line = frames[i];
ericoneill 19:29b3decea5e2 171 int track = find_track(line);
ericoneill 19:29b3decea5e2 172 printf("track at %i\n\r",track);
ericoneill 0:2002d645d2a6 173 }
ericoneill 19:29b3decea5e2 174
ericoneill 19:29b3decea5e2 175
ericoneill 0:2002d645d2a6 176 }