Diff: main.cpp

Revision:

0:ad375c052b4c

Child:

1:55e0aaf71bda

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 20 08:59:05 2015 +0000
@@ -0,0 +1,273 @@
+#include "mbed.h"
+#include "stdlib.h"
+
+//Outputs
+DigitalOut led1(LED1);
+DigitalOut clk(PTD5);
+DigitalOut si(PTD4);
+PwmOut motor1(PTA4);
+PwmOut motor2(PTA12);
+DigitalOut break1(PTC12);
+DigitalOut break2(PTC13);
+PwmOut servo(PTA5);
+
+//Inputs
+AnalogIn camData(PTC2);
+
+//Encoder setup and variables
+InterruptIn interrupt(PTA13);
+
+//Line Tracking Variables --------------------------------
+float ADCdata [128];
+float minAccum;
+float minCount;
+float approxPos;
+float minVal;
+int minLoc;
+
+//Servo turning parameters
+float straight = 0.00155f;
+float hardLeft = 0.0013f;
+float slightLeft = 0.00145f;
+float hardRight = 0.0018f;
+float slightRight = 0.00165f;
+
+//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 stall_check = 0;
+float tuning_val = 1;
+
+Timer t;
+Timer servoTimer;
+
+//Observed average speeds for each duty cycle
+const float DESIRED_SPEED = 0.5;
+const float TUNING_CONSTANT_10 = 1.10;
+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 ----------------------
+
+// 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);
+    
+    if (avg_speed == stall_check) {
+        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){
+        //tuning_val += TUNE_AMT;
+        stall_check = avg_speed;
+    } else {
+        //tuning_val = 1;
+        stall_check = avg_speed;
+    }
+    motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
+    motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
+    
+}
+
+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;
+}
+
+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;
+}
+
+
+//End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+int main() {
+    //Line Tracker Initializations
+    int integrationCounter = 0;
+    
+    // Motor Driver Initializations
+    motor1.period(.0025);
+    motor2.period(.0025);
+    interrupt.fall(&fallInterrupt);
+    interrupt.rise(&riseInterrupt);
+
+    //wait(5);
+
+    motor1.pulsewidth(.0025*.1);
+    motor2.pulsewidth(.0025*.1);
+    break1 = 0;
+    break2 = 0;
+    
+    t.start();
+    
+    wait(5);
+    
+    while(1) {
+            
+        if(integrationCounter % 151== 0){
+            //Disable interrupts
+            //__disable_irq();
+            //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
+            minAccum = 0;
+            minCount = 0;
+            approxPos = 0;
+            
+            //Reset Encoder and Motor Driver Variables
+            interval1=0;
+            interval2=0;
+            interval3=0;
+            avg_interval=0;
+            lastchange1 = 0;
+            lastchange2 = 0;
+            lastchange3 = 0;
+            lastchange4 = 0;
+            
+            t.reset();
+                
+        }
+        else if (integrationCounter > 129){
+            //Enable interrupts
+            //__enable_irq();
+            //interrupt.fall(&fallInterrupt);
+            //interrupt.rise(&riseInterrupt);
+            
+            minVal = ADCdata[15];
+            for (int c = 15; c < 118; c++) {
+                if (ADCdata[c] < minVal){
+                    minVal = ADCdata[c];
+                    minLoc = c;
+                }
+            }
+            
+            for (int c = 15; c < 118; c++) {
+                if (ADCdata[c] >= minVal && ADCdata[c] - minVal < 0.04f && ADCdata[c] > 0.1f){
+                    minAccum += c;
+                    minCount++;
+                }
+            }
+            
+            approxPos = (float)minAccum/(float)minCount;
+            
+            if(approxPos > 0 && approxPos <= 20){
+                    servo.pulsewidth(hardLeft);
+            } else if (approxPos > 20 && approxPos <= 45){
+                    servo.pulsewidth(slightLeft);
+            } else if (approxPos > 45 && approxPos <= 90){
+                servo.pulsewidth(straight);
+            } else if (approxPos > 90 && approxPos <= 105){
+                servo.pulsewidth(slightRight);
+            } else if (approxPos > 105 && approxPos <= 128){
+                    servo.pulsewidth(hardRight);
+            }
+            
+            velocity_control(0.05f, DESIRED_SPEED);
+            
+            integrationCounter = 150;
+        }
+        else{
+            clk = 1;
+            wait_us(70);
+            ADCdata[integrationCounter - 1] = camData;
+            clk = 0;
+        }
+
+        //clk = 0;
+        integrationCounter++;
+        //camData.
+        
+    }
+}