Revision 4:8ee76f6d32b5, committed 2015-05-11

Comitter:

cheryl_he

Date:

Mon May 11 02:00:33 2015 +0000

Parent:

3:39e6440ccd45

Child:

5:868e652f26af

Commit message:

new red rover code need to tune constants

Changed in this revision

--- a/main.cpp	Tue May 05 21:44:06 2015 +0000
+++ b/main.cpp	Mon May 11 02:00:33 2015 +0000
@@ -6,16 +6,19 @@
 #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<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 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 myled(LED1);
+DigitalOut led1(LED1);
 DigitalOut clk(PTD5);
 DigitalOut si(PTD4);
 PwmOut motor1(PTA12);
@@ -24,6 +27,7 @@
 DigitalOut break2(PTC0);
 PwmOut servo(PTA5);
 
+//Serial bt(PTA2, PTA1);
 //Serial pc(USBTX, USBRX); // tx, rx
 
 //Inputs
@@ -32,10 +36,89 @@
 //Encoder setup and variables
 InterruptIn interrupt(PTA13);
 
-Timer t;
+//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;
 
-float ADCdata [128];
+//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 & (
@@ -53,34 +136,239 @@
 }
 
 int main() {
-    
     telemetry_serial.baud(115200);
     telemetry_obj.transmit_header();
-    //uint16_t data [128] = {0};
     
     //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 = cam1.read();
     
+    if(dataCol){
+        loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime = 0;
+        printTimer.start();
+    }
+    //uint16_t* data = camData.read();
     while(1) {
-/*        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
-*/        
-        /*for (uint16_t i=0; i<128; i++) {
-            tele_linescan[i] = data[i];
+        if(dataCol){
+            //break out of main loop if enough time has passed;    
+            if(loopCtr >= numData && dataCol){
+                break;
+            }
+        }
+        if(integrationCounter % 151== 0){
+            /*
+            //Disable interrupts
+            interrupt.fall(NULL);
+            interrupt.rise(NULL);
+            */
+            
+            //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;
+
         }
-        */
-        
-        tele_time_ms = t.read_ms();
-        for (uint16_t i=0; i<128; i++) {
-            tele_linescan[i] = ADCdata[i];
+        else if (integrationCounter > 129){
+            //Start Timer
+            //t.start();
+            
+            //Enable interrupts
+            //interrupt.fall(&fallInterrupt);
+            //interrupt.rise(&riseInterrupt);
+            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;
+                    }
+                }
+            }
+            /*
+            //Check if we need to alter integration time due to brightness
+            if (maxVal < 0.15f){
+                intTimMod += 10;
+            } else if (maxVal >= 1) {
+                if (intTimMod > 0) {
+                    intTimMod -= 10;
+                }
+            }
+            */
+            
+            //Line Crossing Checks
+            if (space) {
+                currDir = prevDir;
+                firstTime = true;
+            } else {
+                approxPos = (float)maxAccum/(float)maxCount;
+
+                if(dataCol){
+                    if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
+                        lineCenters[loopCtr] = approxPos;
+                        times[loopCtr] = printTimer.read_ms();
+                        loopCtr++;
+                    }
+                }
+                
+                currDir = hardLeft + (approxPos)/((float) 118)*(hardRight-hardLeft);
+                prevApproxPos = approxPos;
+
+            }
+/*
+            if (turnSpeedControl){
+                //Change speed when turning at different angles
+                if(approxPos > 0 && approxPos <= 45){
+                        motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
+                        motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
+                } else if (approxPos > 45 && approxPos <= 55){
+                        motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
+                        motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
+                } else if (approxPos > 55 && approxPos <= 85){
+                        motor1.pulsewidth(motorPeriod*pulsewidth);
+                        motor2.pulsewidth(motorPeriod*pulsewidth);
+                } else if (approxPos > 85 && approxPos <= 95){
+                        motor1.pulsewidth(motorPeriod*pulsewidth*1.5f);
+                        motor2.pulsewidth(motorPeriod*pulsewidth*1.5f);
+                } else {
+                    motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
+                    motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
+                }
+           }
+*/           
+            servo.pulsewidth(currDir);
+            
+            //Start Velocity control after requisite number of encoder signals have been collected
+            //if(numInterrupts >= 4){
+                //velocity_control(0.1f, TUNING_CONSTANT_10);
+            //}        
+            
+            //Save current direction as previous direction
+            prevDir = currDir;
+            
+            //Prepare to start collecting more data
+            integrationCounter = 150;
+            
+            //Disable interrupts
+            //interrupt.fall(NULL);
+            //interrupt.rise(NULL);
+            
+            //Stop timer
+            //t.stop();                
         }
-        
+        else{
+            clk = 1;
+            wait_us(intTimMod);
+            ADCdata[integrationCounter - 1] = camData;
+            clk = 0;
+        }
+
+        //clk = 0;
+        integrationCounter++;
+        if(dataCol){
+            loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime++;
+        }
+        //camData.
         telemetry_obj.do_io();
+
         
     }
+/*
+    if (dataCol){
+        //print frame data
+        pc.printf("printing frame data\n\r");
+        //int frameSize = frames.size();
+        //pc.printf("%i",frameSize);
+        pc.printf("[");
+        for(int i=0; i<numData; i++){
+            if(lineCenters > 0){
+                pc.printf("%i %i,",lineCenters[i], times[i]);
+            }
+        }
+        pc.printf("]\n\r");
+    }
+    */
 }
+