Diff: Robot.h

Revision:

5:f1613df66ceb

Parent:

4:208f5279143a

Child:

6:477382219bcf

--- a/Robot.h	Sat Mar 23 19:46:09 2019 +0000
+++ b/Robot.h	Mon Mar 25 22:42:31 2019 +0000
@@ -4,24 +4,24 @@
     int static const numberOfSamples = 100;
     int static const numberOfSensors = 6;
     //attenuation factor
-    float static const AF = 1.0f;
-    
-    LWheel* leftWheel;
-    RWheel* rightWheel;
+    float static const AF = 0.8f;
+    Wheel* leftWheel;
+    Wheel* rightWheel;
     lineSensor* sensorArray[numberOfSensors];
-    PID controller;
+    PID2 controller;
     float lineVoltages[numberOfSamples];
     int lvIndex;
     int sensorNumber;
     Ticker updater;
+    int endOfLineDetection;
     
     float RoboticAngularVelocity;
     
     public:
     
-    Robot(LWheel* LW, RWheel* RW, lineSensor* SA[]) : controller(1.0f,0.0f,0.0f,0.1f)
+    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(10000.0f, 0.0f, 0.0f, 0.0003f)
     {
-        lvIndex = 1;
+        lvIndex = 0;
         sensorNumber = 0;
         sensorArray[0] = SA[0];
         sensorArray[1] = SA[1];
@@ -34,13 +34,13 @@
         
         lineVoltages[lvIndex] = 0.0f;
         //controller will output a value between +- max speed of wheels
-        controller.setOutputLimits(-1.0f*AF*rightWheel->returnMaxAngularVel(),AF*(leftWheel->returnMaxAngularVel()));
-        controller.setInputLimits(-1980.0f,1980.0f);
+        
+        controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
+        controller.setInputLimits(-300.0f,300.0f);
         controller.setSetPoint(0.0f);
-        updater.attach(callback(this, &Robot::robotUpdates),0.1f);
+        updater.attach(callback(this, &Robot::robotUpdates),0.00005f);
     };
     
-    
     float calculateTranslationalVelocity()
     {
         return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
@@ -60,54 +60,51 @@
     //attempts to modify the angular velocity of the buggy
     void adjustAngularVelocity(float W)
     {
-            float temp = W - RoboticAngularVelocity;
             //negative is a right turn
             if (W < 0)
             {
-                //leftWheel a smaller number depending on difference
-                leftWheel->adjustAngularVelocity((leftWheel->returnAngularVelocity()+(temp+(rightWheel->returnMaxAngularVel()-rightWheel->returnAngularVelocity()))*AF));
-                //right Wheel on maximum
-                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF); 
+                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
+                leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
             }
             else
             {
-                rightWheel->adjustAngularVelocity((rightWheel->returnAngularVelocity()-(temp-(leftWheel->returnMaxAngularVel()-leftWheel->returnAngularVelocity()))*AF));
                 leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
+                rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
             }   
     }
     
     void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
         {
             float ambientLight;
+            ambientLight = sensorArray[sensorNumber]->calcLineVoltage();  
+            sensorArray[sensorNumber]->sample();
             sensorArray[sensorNumber]->calcLineVoltage();
-            ambientLight = sensorArray[sensorNumber]->returnLineVoltage(); 
-            sensorArray[sensorNumber]->sample();
             switch (sensorNumber)
             {
                 case 0:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f;
+                    lineVoltages[lvIndex%numberOfSamples] = ((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f);
                     break;
                 case 1:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f;
+                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f);
                     break;
                 case 2:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f;
+                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f);
                     break;
                 case 3:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f;
+                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f);
                     break;
-                case 4:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f;
+                case 4: 
+                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f);
                     break;
                 case 5:
-                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f;
+                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f);
                     break;
             }
             sensorNumber ++;
-            if (sensorNumber >= 4)
+            if (sensorNumber >= numberOfSensors)
             {
-            sensorNumber = 1;
-            adjustAngularVelocity(controller.compute(lineVoltages[lvIndex]));
+            sensorNumber = 0;
+            adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
             lvIndex++;
             }
         }
@@ -117,6 +114,16 @@
         leftWheel->adjustAngularVelocity(0);
         rightWheel->adjustAngularVelocity(0);
     }
+    
+    float returnLineVoltage()
+    {
+        return lineVoltages[lvIndex%numberOfSamples];
+    }
+    
+    void rbtInit()
+    {
+        
+    }
 
 };