main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "MODSERIAL.h"
00004 #include "telemetry.h"
00005 #include "telemetry-mbed.h"
00006 
00007 // Camera Pins
00008 DigitalOut clk(PTA13);
00009 DigitalOut si(PTD4);
00010 AnalogIn camData(PTC2);
00011 
00012 //Servo, Motor, Serial Pins
00013 PwmOut servo(PTA5);
00014 PwmOut motor1(PTA4);
00015 PwmOut motor2(PTA12);
00016 DigitalOut break1(PTC12);
00017 DigitalOut break2(PTC13);
00018 Serial pc(USBTX, USBRX);
00019 
00020 //Telemetry
00021 MODSERIAL telemetry_serial(PTA2, PTA1);
00022 telemetry::MbedHal telemetry_hal(telemetry_serial);
00023 telemetry::Telemetry telemetry_obj(telemetry_hal);
00024 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
00025 telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
00026 telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor1", "Motor PWM", "%DC", 0);
00027 
00028 
00029 // Interrupt for encoder
00030 InterruptIn interrupt(PTA13);
00031 const int maxFrames = 3;
00032 float frames[maxFrames][128];
00033 float ADCdata [128];
00034 Mutex line_mutex;
00035 
00036 
00037 //servo rotations
00038 float straight = 0.00155f;
00039 float hardLeft = 0.0013f;
00040 float slightLeft = 0.00145f;
00041 float hardRight = 0.0018f;
00042 float slightRight = 0.00165f;
00043 
00044 
00045 void linecam_thread(void const *args){
00046     tele_motor_pwm.set_limits(0, 1);
00047     telemetry_obj.transmit_header();
00048     while(1){
00049         //line_mutex.lock();
00050         pc.printf("[");
00051         for(int i=0; i<128; i++){
00052             //pc.printf("%f",ADCdata[i]);
00053             pc.printf(" ");
00054         }
00055         pc.printf("]");
00056         //line_mutex.unlock();
00057     }
00058     
00059 }
00060 
00061 int find_track(float line[]){
00062     int track_location = -1;
00063     float slope_threshold = .05;
00064     bool downslope_indices [128] = {false};
00065     bool upslope_indices [128] = {false};
00066     for(int i=10; i<118; i++){
00067         if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
00068             downslope_indices[i] = true;
00069         }
00070         if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
00071             upslope_indices[i] = true;
00072         }
00073     }
00074     int numDownslopes = 0;
00075     int numUpslopes = 0;
00076     for(int i=0; i<128; i++){
00077         if(downslope_indices[i] == true){
00078             numDownslopes ++;
00079         }
00080         if(upslope_indices[i] == true){
00081             numUpslopes ++;
00082         }
00083     }
00084     int downslope_locs [numDownslopes];
00085     int upslope_locs [numUpslopes];
00086     int dsctr = 0;
00087     int usctr = 0;
00088     
00089     for (int i=0; i<128; i++){
00090         if(downslope_indices[i] == true){
00091             downslope_locs[dsctr] = i;
00092             dsctr++;
00093         }
00094         if(upslope_indices[i] == true){
00095             upslope_locs[usctr] = i;
00096             usctr++;
00097         }
00098     }
00099     
00100     for(int i=0; i<numDownslopes; i++){
00101         for(int j=0; j<numUpslopes; j++){
00102             if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
00103                 track_location = downslope_locs[i] + 2 ;
00104             }
00105         }
00106     }
00107     
00108     pc.printf("Downslopes at: ["); 
00109     for(int i = 0; i<numDownslopes; i++){
00110         pc.printf("%i ", downslope_locs[i]);
00111         
00112     }
00113     pc.printf("]\n\r");
00114     pc.printf("Upslopes at: [");
00115     for(int i=0; i<numUpslopes; i++){
00116         pc.printf("%i ", upslope_locs[i]);
00117     }
00118     pc.printf("]\n\r");
00119     return track_location;
00120 }
00121 
00122 int main() {
00123     //Thread thread(linecam_thread);
00124     //thread.set_priority(osPriorityIdle);
00125     pc.printf("Beginning linecam data acquisition... \n\r");
00126     for(int numFrames =  0; numFrames < maxFrames; numFrames++){
00127         for(int integrationCounter = 0;integrationCounter < 151;) {
00128             if(integrationCounter % 151== 0){
00129                 si = 1;
00130                 clk = 1;
00131                 //wait(.00001);
00132                 si = 0;
00133                 clk = 0;
00134                 integrationCounter = 0;
00135                
00136                     
00137             }
00138             else if (integrationCounter > 129){
00139                //uint16_t * data = camData.read_u16();
00140                for (uint16_t i =0; i < 128; i++) {
00141                    tele_linescan[i] = camData.read_u16();
00142                }
00143                 telemetry_obj.do_io(); 
00144                 motor1.write(tele_motor_pwm);
00145                 integrationCounter = 150;
00146             }
00147             else{
00148                 clk = 1;
00149                 wait_us(50);
00150                 //line_mutex.lock();
00151                 frames[numFrames][integrationCounter - 1] = 1-camData; // 1- is for light line
00152                 //line_mutex.unlock();
00153                 clk = 0;
00154             }
00155     
00156             integrationCounter++;
00157             
00158         }
00159         //frames[numFrames] = ADCdata;
00160     }
00161     
00162     for(int i =0; i<maxFrames; i++){
00163         pc.printf("LINE %i: [",i);
00164         
00165         
00166         for(int j = 0; j < 128; j++){
00167             pc.printf(" %f",frames[i][j]);
00168         }
00169         pc.printf("]\n\r");
00170         float* line = frames[i];
00171         int track = find_track(line);
00172         printf("track at %i\n\r",track);
00173     }
00174     
00175     
00176 }