Diff: main.cpp.orig

Revision:

8:e126c900c89d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Fri Apr 03 00:34:57 2015 +0000
@@ -0,0 +1,402 @@
+#include "mbed.h"
+#include "stdlib.h"
+
+//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 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 maxVal;
+int maxLoc;
+
+//Line Crossing variables
+int prevTrackLoc;
+int spaceThresh = 5;
+bool space;
+
+//Servo turning parameters
+float straight = 0.00155f;
+float hardLeft = 0.0010f;
+float hardRight = 0.0021f;
+
+//Servo Directions
+float currDir;
+float prevDir;
+
+//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;
+
+float pulsewidth = 0.25f;
+
+// Hardware periods
+float motorPeriod = .0025;
+float servoPeriod = .0025;
+
+Timer t;
+Timer servoTimer;
+
+//Observed average speeds for each duty cycle
+const float DESIRED_SPEED = 1;
+const float TUNING_CONSTANT_10 = 1.90;
+const float TUNING_CONSTANT_20 = 3.00;
+const float TUNING_CONSTANT_30 = 4.30;
+const float TUNING_CONSTANT_50 = 6.880;
+const float PI = 3.14159;
+const float WHEEL_CIRCUMFERENCE = .05*PI;
+
+//Velocity Control Tuning Constants
+const float TUNE_THRESH = 0.5f;
+const float TUNE_AMT = 0.1f;
+
+//Parameters specifying sample sizes and delays for small and large average speed samples
+float num_samples_small = 3.0f;
+float delay_small = 0.05f;
+float num_samples_large = 100.0f;
+float delay_large = 0.1f;
+
+//Large and small arrays used to get average velocity values
+float large_avg_speed_list [100];
+float small_avg_speed_list [10];
+
+//End of Encoder and Motor Driver Variables ----------------------
+
+//Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+int find_track(float line[]){
+    int track_location = -1;
+    float slope_threshold = .05;
+    bool downslope_indices [128] = {false};
+    bool upslope_indices [128] = {false};
+    for(int i=10; i<118; i++){
+        if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){
+            downslope_indices[i] = true;
+        }
+        if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){
+            upslope_indices[i] = true;
+        }
+    }
+    int numDownslopes = 0;
+    int numUpslopes = 0;
+    for(int i=0; i<128; i++){
+        if(downslope_indices[i] == true){
+            numDownslopes ++;
+        }
+        if(upslope_indices[i] == true){
+            numUpslopes ++;
+        }
+    }
+    int downslope_locs [numDownslopes];
+    int upslope_locs [numUpslopes];
+    int dsctr = 0;
+    int usctr = 0;
+    
+    for (int i=0; i<128; i++){
+        if(downslope_indices[i] == true){
+            downslope_locs[dsctr] = i;
+            dsctr++;
+        }
+        if(upslope_indices[i] == true){
+            upslope_locs[usctr] = i;
+            usctr++;
+        }
+    }
+    
+    for(int i=0; i<numDownslopes; i++){
+        for(int j=0; j<numUpslopes; j++){
+            if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){
+                track_location = downslope_locs[i] + 2 ;
+            }
+        }
+    }
+
+    return track_location;
+}
+
+//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
+}
+
+
+// Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+float get_speed(){
+    float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
+    float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
+    return linearSpeed;
+}
+
+float get_avg_speed(float num_samples, float delay) {
+    
+    float avg_avg_speed = 0;
+    
+    for (int c = 0; c < num_samples; c++) {
+        if (num_samples == num_samples_small){
+            small_avg_speed_list[c] = get_speed();
+        } else if (num_samples == num_samples_large){
+            large_avg_speed_list[c] = get_speed();
+        }
+        //wait(delay);
+        }
+                
+        for (int c = 1; c < num_samples; c++) {
+            if (num_samples == num_samples_small){
+                avg_avg_speed += small_avg_speed_list[c];
+            } else if (num_samples == num_samples_large){
+                avg_avg_speed += large_avg_speed_list[c];
+            }
+        }
+    return avg_avg_speed/num_samples;
+}
+
+void velocity_control(float duty_cyc, float tuning_const) {
+    
+    avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small);
+    
+    //When determined speed is infinity or 0, set the speed to the last agreeable speed
+    /*if (avg_speed > 100 || avg_speed == 0){
+        avg_speed = last_speed;
+    }*/
+    
+    pc.printf("\n\r%f", avg_speed);
+    if (avg_speed == stall_check && tuning_const != 0 && avg_speed == 0) {
+        avg_speed = 0;
+        tuning_val += TUNE_AMT;
+    } else if((avg_speed - tuning_const) > TUNE_THRESH){
+        tuning_val -= TUNE_AMT;
+        stall_check = avg_speed;
+
+    } else if (tuning_const - avg_speed > TUNE_THRESH && avg_speed != 0){
+        tuning_val += TUNE_AMT;
+        stall_check = avg_speed;
+    } else {
+        //tuning_val = 1;
+        stall_check = avg_speed;
+    }
+    
+    if (tuning_val < .5){
+        tuning_val = .5;
+    }
+    pc.printf("\n\rTuning Val: %f", tuning_val);
+    motor1.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
+    motor2.pulsewidth(motorPeriod * (duty_cyc * tuning_val));
+    
+    if (avg_speed != 0){
+        last_speed = avg_speed;
+    }
+
+}
+
+// Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
+void fallInterrupt(){
+    
+    int time = t.read_us();
+    interval1 = time - lastchange2;
+    interval2 = lastchange1-lastchange3;
+    interval3 = lastchange2 - lastchange4;
+    avg_interval = (interval1 + interval2 + interval3)/3;
+    
+    lastchange4 = lastchange3;
+    lastchange3 = lastchange2;
+    lastchange2 = lastchange1;
+    lastchange1 = time;
+    
+    numInterrupts++;
+}
+
+void riseInterrupt(){
+    int time = t.read_us();
+    interval1 = time - lastchange2;
+    interval2 = lastchange1-lastchange3;
+    interval3 = lastchange2 - lastchange4;
+    avg_interval = (interval1 + interval2 + interval3)/3;
+    
+    lastchange4 = lastchange3;
+    lastchange3 = lastchange2;
+    lastchange2 = lastchange1;
+    lastchange1 = time;
+    
+    numInterrupts++;
+}
+
+
+//End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+int main() {
+    
+    //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);
+    
+    wait(3);
+
+    motor1.pulsewidth(motorPeriod*pulsewidth);
+    motor2.pulsewidth(motorPeriod*pulsewidth);
+    break1 = 0;
+    break2 = 0;
+    
+    while(1) {
+            
+        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();
+            
+            //Enable interrupts
+            //interrupt.fall(&fallInterrupt);
+            //interrupt.rise(&riseInterrupt);
+            
+            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] < 0.07f && ADCdata[c] > 0.1f){
+                    maxAccum += c;
+                    maxCount++;
+                    if (c > prevTrackLoc + spaceThresh){
+                        space = true;
+                    }
+                    prevTrackLoc = c;
+                }
+            }
+            
+            //Line Crossing Checks
+            if (maxCount >= 15 || space) {
+                currDir = prevDir;
+            } else {
+                approxPos = (float)maxAccum/(float)maxCount;
+                //approxPos = find_track(ADCdata);
+                currDir = hardLeft + approxPos/((float) 127)*(hardRight-hardLeft);
+            }
+            
+            //Change speed when turning at different angles
+            /*if(approxPos > 0 && approxPos <= 45){
+                    motor1.pulsewidth(motorPeriod*(pulsewidth/2));
+                    motor2.pulsewidth(motorPeriod*(pulsewidth/2));
+            } else if (approxPos > 45 && approxPos <= 95){
+                    motor1.pulsewidth(motorPeriod*pulsewidth);
+                    motor2.pulsewidth(motorPeriod*pulsewidth);
+            } else {
+                motor1.pulsewidth(motorPeriod*(pulsewidth/2));
+                motor2.pulsewidth(motorPeriod*(pulsewidth/2));
+            }*/
+            
+            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(10);
+            ADCdata[integrationCounter - 1] = camData;
+            clk = 0;
+        }
+
+        //clk = 0;
+        integrationCounter++;
+        //camData.
+        
+    }
+}