Diff: main.cpp

Revision:

6:f1d948d2d6c1

Parent:

5:20223464f7aa

Child:

8:e126c900c89d

--- a/main.cpp	Mon Mar 30 20:07:39 2015 +0000
+++ b/main.cpp	Tue Mar 31 22:56:01 2015 +0000
@@ -29,14 +29,13 @@
 
 //Line Crossing variables
 int prevTrackLoc;
+int spaceThresh = 5;
 bool space;
 
 //Servo turning parameters
 float straight = 0.00155f;
 float hardLeft = 0.0010f;
-float slightLeft = 0.00145f;
 float hardRight = 0.0021f;
-float slightRight = 0.00165f;
 
 //Servo Directions
 float currDir;
@@ -63,8 +62,9 @@
 float stall_check = 0;
 float tuning_val = 1;
 
+int numInterrupts = 0;
 
-float pulsewidth = 0.18f;
+float pulsewidth = 0.25f;
 
 // Hardware periods
 float motorPeriod = .0025;
@@ -150,6 +150,23 @@
     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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
@@ -187,9 +204,9 @@
     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){
+    /*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) {
@@ -220,6 +237,7 @@
 
 }
 
+// Interrupt Functions for Encoder ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``
 void fallInterrupt(){
     
     int time = t.read_us();
@@ -232,6 +250,8 @@
     lastchange3 = lastchange2;
     lastchange2 = lastchange1;
     lastchange1 = time;
+    
+    numInterrupts++;
 }
 
 void riseInterrupt(){
@@ -245,16 +265,22 @@
     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 (meaningless currently)
+    //Initial values for directions
     currDir = 0;
     prevDir = 0;
     
@@ -272,17 +298,9 @@
     break1 = 0;
     break2 = 0;
     
-    t.start();
-    
-    //wait(5);
-    
     while(1) {
             
-        if(integrationCounter % 151== 0){
-            //Disable interrupts
-            interrupt.fall(NULL);
-            interrupt.rise(NULL);
-            
+        if(integrationCounter % 151== 0){            
             //Send start of integration signal
             si = 1;
             clk = 1;
@@ -299,27 +317,15 @@
             approxPos = 0;
             
             space = false;
-            
-            /* Velocity control junk
-            //Reset Encoder and Motor Driver Variables
-            interval1=0;
-            interval2=0;
-            interval3=0;
-            //avg_interval=0;
-            lastchange1 = 0;
-            lastchange2 = 0;
-            lastchange3 = 0;
-            lastchange4 = 0;
-            //stall_check = 0;
-            */
+
         }
         else if (integrationCounter > 129){
             //Start Timer
-            t.start();
+            //t.start();
             
             //Enable interrupts
-            interrupt.fall(&fallInterrupt);
-            interrupt.rise(&riseInterrupt);
+            //interrupt.fall(&fallInterrupt);
+            //interrupt.rise(&riseInterrupt);
             
             maxVal = ADCdata[10];
             for (int c = 11; c < 118; c++) {
@@ -333,50 +339,53 @@
                 if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){
                     maxAccum += c;
                     maxCount++;
-                    if (c != prevTrackLoc + 1){
+                    if (c > prevTrackLoc + spaceThresh){
                         space = true;
                     }
                     prevTrackLoc = c;
                 }
             }
             
-            if (maxCount >= 25) {
+            //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);
             }
             
-            //approxPos = find_track(ADCdata);
-            
+            //Change speed when turning at different angles
             /*if(approxPos > 0 && approxPos <= 45){
-                    motor1.pulsewidth(motorPeriod*(pulsewidth-.015));
-                    motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
+                    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-.015));
-                motor2.pulsewidth(motorPeriod*(pulsewidth-.015));
+                motor1.pulsewidth(motorPeriod*(pulsewidth/2));
+                motor2.pulsewidth(motorPeriod*(pulsewidth/2));
             }*/
             
             servo.pulsewidth(currDir);
             
-            //delay for velocity control
-            //wait_ms(10);
-            
+            //Start Velocity control after requisite number of encoder signals have been collected
+            //if(numInterrupts >= 4){
+                //velocity_control(0.1f, TUNING_CONSTANT_10);
+            //}        
             
-            //Stop Timer
-            //t.stop();
-            
-            //Reset Timer
-            //t.reset();
-            
-            //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;