Diff: main.cpp

Revision:

14:888495814f3c

Parent:

13:5be515371946

Child:

15:55e9fffc653a

--- a/main.cpp	Thu Apr 09 05:48:35 2015 +0000
+++ b/main.cpp	Thu Apr 09 21:05:01 2015 +0000
@@ -25,14 +25,16 @@
 float maxCount;
 float approxPos;
 float prevApproxPos;
-int trackWindow = 20;
+int trackWindow = 10;
+int startWindow;
+int endWindow;
 float maxVal;
 int maxLoc;
 
 bool firstTime;
 
 //Brightness accommodater
-int intTimMod = 0;
+
 
 //Data Collection
 bool dataCol = false;
@@ -40,7 +42,8 @@
 
 //Line Crossing variables
 int prevTrackLoc;
-int spaceThresh = 3;
+int spaceThresh = 2;
+int widthThresh = 10;
 bool space;
 
 //Servo turning parameters
@@ -84,9 +87,10 @@
 
 int numInterrupts = 0;
 
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`
-float pulsewidth = 0.15f;
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~PWM & Integration Time ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+float pulsewidth = 0;
+int intTimMod = 0;
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 // Hardware periods
 float motorPeriod = .0025;
 float servoPeriod = .0025;
@@ -393,21 +397,32 @@
                 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 = prevApproxPos - trackWindow; c < prevApproxPos + trackWindow; c++) {
+                for (int c = startWindow; c < endWindow; c++) {
                     if (ADCdata[c] > maxVal){
                         maxVal = ADCdata[c];
                         maxLoc = c;
                     }
                 }
                 
-                for (int c = prevApproxPos - trackWindow; c < prevApproxPos + trackWindow; c++) {
+                for (int c = startWindow; c < endWindow; c++) {
                     if (ADCdata[c] <= maxVal && maxVal - ADCdata[c] < 0.025f){
                         maxAccum += c;
                         maxCount++;
                         if (c > prevTrackLoc + spaceThresh){
                             space = true;
                         }
+                        if (maxCount > 60){
+                            firstTime = true;
+                        }
                         prevTrackLoc = c;
                     }
                 }
@@ -426,9 +441,13 @@
             //Line Crossing Checks
             if (space) {
                 currDir = prevDir;
-                firstTime = true;
+                //firstTime = true;
             } else {
-                approxPos = (float)maxAccum/(float)maxCount;
+                approxPos = (float)maxAccum/(float)maxCount - 10;
+
+                if (approxPos < 0){
+                    approxPos = 0;
+                }
 
                 if(dataCol){
                     if(loopCounterForModdingSoThatWeCanIncreaseTheRecordingTime%3==0){
@@ -442,7 +461,7 @@
                 prevApproxPos = approxPos;
 
             }
-
+/*
             //Change speed when turning at different angles
             if(approxPos > 0 && approxPos <= 45){
                     motor1.pulsewidth(motorPeriod*(pulsewidth*0.66f));
@@ -454,7 +473,7 @@
                 motor1.pulsewidth(motorPeriod*(pulsewidth*0.66f));
                 motor2.pulsewidth(motorPeriod*(pulsewidth*0.66f));
             }
-            
+           */ 
             servo.pulsewidth(currDir);
             
             //Start Velocity control after requisite number of encoder signals have been collected
@@ -477,7 +496,7 @@
         }
         else{
             clk = 1;
-            wait_us(10 + intTimMod);
+            wait_us(intTimMod);
             ADCdata[integrationCounter - 1] = camData;
             clk = 0;
         }