Diff: main.cpp

Revision:

19:d9746cc2ec80

Parent:

18:7941524e0d28

Child:

22:21ec3ac3b8ea

Child:

23:bf38d7d2255a

--- a/main.cpp	Fri Apr 24 00:29:45 2015 +0000
+++ b/main.cpp	Tue Apr 28 17:13:33 2015 +0000
@@ -84,11 +84,12 @@
 
 int numInterrupts = 0;
 
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-float pulsewidth = 0.20f;
-int intTimMod = 20;
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Tuning Values that can make it or break it ~~~~~~~~~~~~~~~~~~~~~~~~
+float pulsewidth = 0.14f;
+int intTimMod = 0;
+int maxValThresh = .1; //~.1 for earlier in the day, reduce it (maybe something like .05 - .01 or something) as it gets darker 
+bool turnSpeedControl = false; //have increased PWMs when turning when true.
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-bool turnSpeedControl = true;
 
 // Hardware periods
 float motorPeriod = .0025;
@@ -98,83 +99,8 @@
 Timer servoTimer;
 Timer printTimer; //after printTimer reaches a certain value the main loop will terminate and print the frames
 
-//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){
  
@@ -192,111 +118,6 @@
         & (~(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
@@ -383,7 +204,7 @@
                 }
                 
                 for (int c = 10; c < 118; c++) {
-                        if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < (maxVal*0.04)){
+                        if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
                             maxAccum += c;
                             maxCount++;
                             if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
@@ -413,7 +234,7 @@
                 }
                 
                 for (int c = startWindow; c < endWindow; c++) {
-                    if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < (maxVal*0.04)){
+                    if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < maxValThresh){
                         maxAccum += c;
                         maxCount++;
                         if (c > prevTrackLoc + spaceThresh || maxCount > widthThresh){
@@ -457,14 +278,20 @@
             if (turnSpeedControl){
                 //Change speed when turning at different angles
                 if(approxPos > 0 && approxPos <= 45){
-                        motor1.pulsewidth(motorPeriod*(pulsewidth*0.95f));
-                        motor2.pulsewidth(motorPeriod*(pulsewidth*0.95f));
-                } else if (approxPos > 45 && approxPos <= 95){
+                        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*0.95f));
-                    motor2.pulsewidth(motorPeriod*(pulsewidth*0.95f));
+                    motor1.pulsewidth(motorPeriod*(pulsewidth*2.0f));
+                    motor2.pulsewidth(motorPeriod*(pulsewidth*2.0f));
                 }
            }