Diff: Robot.h

Revision:

9:cefa177c1353

Parent:

8:5ed6685f6edd

Child:

10:ccf6155e3f97

diff -r 5ed6685f6edd -r cefa177c1353 Robot.h
--- a/Robot.h	Sun Mar 31 20:31:30 2019 +0000
+++ b/Robot.h	Fri Apr 05 16:11:28 2019 +0000
@@ -24,11 +24,11 @@
     
     public:
     
-    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0006f) //this controller actually does nothing atm
+    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0003f) //this controller actually does nothing atm
     {
         updater.detach();
         timeToStop.detach();
-        AF  = 0.3f; //change this value to change max speedzzzzzzzzzzzzzzzzz
+        AF  = 0.7f; //change this value to change max speedzzzzzzzzzzzzzzzzz
         state = 'F';
         Reflec = 0;
           
@@ -45,7 +45,7 @@
         
         lineVoltages[lvIndex] = 0.0f;
         
-        controller.setInputLimits(-300.0f,300.0f);
+        controller.setInputLimits(-180.0f,180.0f);
         controller.setSetPoint(0.0f);
         //controller will output a value between +- max speed of wheels
         
@@ -141,28 +141,29 @@
         switch (sensorNumber)
         {
         case 0:
-            lineVoltages[lvIndex%numberOfSamples] = (reading * -100.0f);
-            if (reading <= Reflec*10){endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] = (reading * -500.0f); 
+            if (reading <= Reflec*4){endOfLineDetection++;}
+            
             break;
         case 1:
-            lineVoltages[lvIndex%numberOfSamples] += (reading * -50.0f);
-            if (reading <= Reflec*10) {endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] += (reading * -180.0f);
+            if (reading <= Reflec*4) {endOfLineDetection++;}
             break;
         case 2:
-            lineVoltages[lvIndex%numberOfSamples] += (reading * -25.0f);
-            if (reading <= 0.5f) {endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] += (reading * -50.0f);
+            if (reading <= Reflec*4) {endOfLineDetection++;}
             break;
         case 3:
-            lineVoltages[lvIndex%numberOfSamples] += (reading * 25.0f);
-            if (reading <= 0.5f) {endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 50.0f);
+            if (reading <= Reflec*4) {endOfLineDetection++;}
             break;
         case 4: 
-            lineVoltages[lvIndex%numberOfSamples] += (reading * 50.0f);
-            if (reading <= Reflec*10) {endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 150.0f);
+            if (reading <= Reflec*4) {endOfLineDetection++;}
             break;
         case 5:
-            lineVoltages[lvIndex%numberOfSamples] += (reading * 100.0f);
-            if (reading <= Reflec*10) {endOfLineDetection++;}
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 250.0f);
+            if (reading <= Reflec*4) {endOfLineDetection++;}
             break;
         }
                 
@@ -171,16 +172,16 @@
         if (sensorNumber >= numberOfSensors)
         {
             sensorNumber = 0;
-            if (endOfLineDetection < 6)
+            if (endOfLineDetection < 8)
             {
-                AF  = 0.3f;
+                AF = scaler(-180.0f,180.0f, 0.2f, 1.0f, lineVoltages[lvIndex%numberOfSamples]);
                 adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
                 lvIndex++;
             }
             else if (AF > 0.1f)
             {
-                AF -= 0.1f;
-                adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]-1);
+                AF -= AF/2.0f;
+                adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples-1]);
             }else{
                 AF = 0.0f;
                 stopMovement();
@@ -200,7 +201,14 @@
     
     void reAttach()
     {
-        updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
+        updater.attach(callback(this, &Robot::robotUpdates),0.00005f);
+    }
+    
+    float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
+    {
+        if (var > prevMax) {var = prevMax;}
+        if (var < prevMin) {var = prevMin;}
+        return (((var-prevMin)/(prevMax - prevMin))*(newMax-newMin))+newMin;
     }
 
 };