init commit
Dependencies: mbed MODSERIAL telemetry-master
main.cpp
- Committer:
- ericoneill
- Date:
- 2015-05-11
- Revision:
- 5:868e652f26af
- Parent:
- 4:8ee76f6d32b5
File content as of revision 5:868e652f26af:
#include "mbed.h" #include "telemetry.h" #include "telemetry-mbed.h" #include "MODSERIAL.h" #include "stdlib.h" #include <vector> //MODSERIAL telemetry_serial(PTA2, PTA1); MODSERIAL telemetry_serial(USBTX, USBRX); telemetry::MbedHal telemetry_hal(telemetry_serial); telemetry::Telemetry telemetry_obj(telemetry_hal); telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); telemetry::NumericArray<uint8_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); telemetry::Numeric<float> tele_motor_pwm(telemetry_obj, "motor", "Motor PWM", "%DC", 0); //Outputs DigitalOut led1(LED1); DigitalOut clk(PTD5); DigitalOut si(PTD4); PwmOut motor1(PTA12); PwmOut motor2(PTA4); DigitalOut break1(PTC7); DigitalOut break2(PTC0); PwmOut servo(PTA5); //Serial bt(PTA2, PTA1); //Serial pc(USBTX, USBRX); // tx, rx //Inputs AnalogIn camData(PTC2); //Encoder setup and variables InterruptIn interrupt(PTA13); //Line Tracking Variables -------------------------------- float ADCdata [128]; float maxAccum; float maxCount; float approxPos; float prevApproxPos; int trackWindow = 30; int startWindow; int endWindow; float maxVal; int maxLoc; bool firstTime; //Data Collection bool dataCol = false; int loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime; //Line Crossing variables int prevTrackLoc; int spaceThresh = 3; int widthThresh = 10; bool space; //Servo turning parameters float straight = 0.00155f; float hardLeft = 0.0012f; float hardRight = 0.0020f; //float hardLeft = 0.0010f; //float hardRight = 0.00195f; //Servo Directions float currDir; float prevDir; // All linescan data for the period the car is running on. To be printed after a set amount of time //std::vector<std::vector<int> > frames; const int numData = 1000; int lineCenters [numData] = {0}; int times [numData] = {0}; int loopCtr = 0; //End of Line Tracking Variables ------------------------- //Encoder and Motor Driver Variables --------------------- //Intervals used during encoder data collection to measure velocity int interval1=0; int interval2=0; int interval3=0; int avg_interval=0; int lastchange1 = 0; int lastchange2 = 0; int lastchange3 = 0; int lastchange4 = 0; //Variables used to for velocity control float avg_speed = 0; float last_speed = 0; float stall_check = 0; float tuning_val = 1; int numInterrupts = 0; //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~ float pulsewidth = 0.2f; //0.2f int intTimMod = 0; float maxValThresh = .1; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker bool turnSpeedControl = true; //have increased PWMs when turning when true. //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Hardware periods float motorPeriod = .0018; float servoPeriod = .0025; Timer t; Timer servoTimer; Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames //End of Encoder and Motor Driver Variables ---------------------- //Function for speeding up KL25Z ADC void initADC(void){ ADC0->CFG1 = ADC0->CFG1 & ( ~( 0x80 // LDLPC = 0 ; no low-power mode | 0x60 // ADIV = 1 | 0x10 // Sample time short | 0x03 // input clock = BUS CLK ) ) ; // clkdiv <= 1 ADC0->CFG2 = ADC0->CFG2 | 0x03 ; // Logsample Time 11 = 2 extra ADCK ADC0->SC3 = ADC0->SC3 & (~(0x03)) ; // hardware avarage off } int main() { telemetry_serial.baud(115200); telemetry_obj.transmit_header(); //Alter reg values to speed up KL25Z initADC(); //Line Tracker Initializations int integrationCounter = 0; //Initial values for directions currDir = 0; prevDir = 0; // Motor Driver Initializations motor1.period(motorPeriod); motor2.period(motorPeriod); // Servo Initialization servo.period(servoPeriod); servo.pulsewidth(hardRight); wait(3); motor1.pulsewidth(motorPeriod*pulsewidth); motor2.pulsewidth(motorPeriod*pulsewidth); break1 = 0; break2 = 0; firstTime = true; t.start(); //uint16_t* data = camData.read(); while(1) { if(dataCol){ //break out of main loop if enough time has passed; if(loopCtr >= numData && dataCol){ break; } } if(integrationCounter % 151== 0){ //Send start of integration signal si = 1; clk = 1; si = 0; clk = 0; //Reset timing counter for integration integrationCounter = 0; //Reset line tracking variables maxAccum = 0; maxCount = 0; approxPos = 0; space = false; } else if (integrationCounter > 129){ //Start Timer //t.start(); tele_time_ms = t.read_ms(); for (uint16_t i=0; i<128; i++) { tele_linescan[i] = (uint8_t) (ADCdata[i] * 128); } //telemetry_obj.do_io(); if (firstTime){ maxVal = ADCdata[10]; for (int c = 11; c < 118; c++) { if (ADCdata[c] > maxVal){ maxVal = ADCdata[c]; maxLoc = c; } } for (int c = 10; c < 118; c++) { if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){ maxAccum += c; maxCount++; if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ space = true; } prevTrackLoc = c; } } //firstTime = false; } else { startWindow = prevApproxPos - trackWindow; endWindow = prevApproxPos + trackWindow; if (startWindow < 0){ startWindow = 0; } if (endWindow > 118){ endWindow = 118; } maxVal = ADCdata[10]; for (int c = startWindow; c < endWindow; c++) { if (ADCdata[c] > maxVal){ maxVal = ADCdata[c]; maxLoc = c; } } for (int c = startWindow; c < endWindow; c++) { if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){ maxAccum += c; maxCount++; if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){ space = true; } prevTrackLoc = c; } } } //Line Crossing Checks if (space) { currDir = prevDir; firstTime = true; } else { approxPos = (float)maxAccum/(float)maxCount; currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft); prevApproxPos = approxPos; } servo.pulsewidth(currDir); //Save current direction as previous direction prevDir = currDir; //Prepare to start collecting more data integrationCounter = 150; //Stop timer //t.stop(); } else{ clk = 1; wait_us(intTimMod); ADCdata[integrationCounter - 1] = camData; clk = 0; } telemetry_obj.do_io(); } }