Diff: Robot.h

Revision:

4:208f5279143a

Parent:

3:01b5e80d842d

Child:

5:f1613df66ceb

--- a/Robot.h	Sat Mar 09 14:27:48 2019 +0000
+++ b/Robot.h	Sat Mar 23 19:46:09 2019 +0000
@@ -3,9 +3,11 @@
     float static const distanceBetweenWheels = 0.19;
     int static const numberOfSamples = 100;
     int static const numberOfSensors = 6;
+    //attenuation factor
+    float static const AF = 1.0f;
     
-    Wheel* leftWheel;
-    Wheel* rightWheel;
+    LWheel* leftWheel;
+    RWheel* rightWheel;
     lineSensor* sensorArray[numberOfSensors];
     PID controller;
     float lineVoltages[numberOfSamples];
@@ -13,23 +15,29 @@
     int sensorNumber;
     Ticker updater;
     
+    float RoboticAngularVelocity;
+    
     public:
     
-    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(1,0,10,0.1)
+    Robot(LWheel* LW, RWheel* RW, lineSensor* SA[]) : controller(1.0f,0.0f,0.0f,0.1f)
     {
         lvIndex = 1;
+        sensorNumber = 0;
         sensorArray[0] = SA[0];
+        sensorArray[1] = SA[1];
+        sensorArray[2] = SA[2];
+        sensorArray[3] = SA[3];
+        sensorArray[4] = SA[4];
+        sensorArray[5] = SA[5];
         leftWheel = LW;
         rightWheel= RW;
         
-        lineVoltages[0] = -16.5f;
+        lineVoltages[lvIndex] = 0.0f;
         //controller will output a value between +- max speed of wheels
-        controller.setOutputLimits(-1.0f*leftWheel->returnMaxAngularVel(),rightWheel->returnMaxAngularVel());
-        controller.setInputLimits(-16.5f, 16.5f); //max limits from sensor readings
-        controller.setMode(1);
-        controller.setSetPoint(0);
-        //based on the fact that it takes 0.2 seconds to take a reading per sensor combo and so we should update our buggy every time it takes a reading from every single sensor
-        updater.attach(callback(this, &Robot::robotUpdates),1); 
+        controller.setOutputLimits(-1.0f*AF*rightWheel->returnMaxAngularVel(),AF*(leftWheel->returnMaxAngularVel()));
+        controller.setInputLimits(-1980.0f,1980.0f);
+        controller.setSetPoint(0.0f);
+        updater.attach(callback(this, &Robot::robotUpdates),0.1f);
     };
     
     
@@ -37,54 +45,71 @@
     {
         return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
     }
-        
-    float calculateAngularVelocity()
+    
+    //difference between angular velocities.
+    void dW()
     {
-        return (((leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity())/distanceBetweenWheels));
+        RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
+    }
+    
+    float returnRoboticAngularVelocity()
+    {
+        return RoboticAngularVelocity;   
     }
     
     //attempts to modify the angular velocity of the buggy
     void adjustAngularVelocity(float W)
     {
-        //if input value is greater than the maximum value the left wheel can go, go full right TURN
-        if (W > leftWheel->returnMaxAngularVel())
-        {
-            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
-            rightWheel->adjustAngularVelocity(0); 
-        }
-        else if (W < -1.0f*rightWheel->returnMaxAngularVel()) //if input value is less than the fastest the right wheel can go in reverse go full left TURN
-        {
-            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
-            leftWheel->adjustAngularVelocity(0); 
-        }
-        else if (W == 0)
-        {
-            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
-            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);  
-        }
-        else
-        {
-            //i actually have no idea what is going on here anymore
-            float temp = W - calculateAngularVelocity();
+            float temp = W - RoboticAngularVelocity;
+            //negative is a right turn
             if (W < 0)
             {
-                leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()*0.8f-rightWheel->returnAngularVelocity()) + temp);
-                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
+                //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); 
             }
             else
             {
-                rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()*0.8f-leftWheel->returnAngularVelocity()) + temp);
-                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);
-            }
-        }
-        
+                rightWheel->adjustAngularVelocity((rightWheel->returnAngularVelocity()-(temp-(leftWheel->returnMaxAngularVel()-leftWheel->returnAngularVelocity()))*AF));
+                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
+            }   
     }
     
     void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
         {
-            lineVoltages[lvIndex] += 0.5f;
-            controller.setProcessValue(lineVoltages[lvIndex]);
-            adjustAngularVelocity(controller.compute());
+            float ambientLight;
+            sensorArray[sensorNumber]->calcLineVoltage();
+            ambientLight = sensorArray[sensorNumber]->returnLineVoltage(); 
+            sensorArray[sensorNumber]->sample();
+            switch (sensorNumber)
+            {
+                case 0:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f;
+                    break;
+                case 1:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f;
+                    break;
+                case 2:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f;
+                    break;
+                case 3:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f;
+                    break;
+                case 4:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f;
+                    break;
+                case 5:
+                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f;
+                    break;
+            }
+            sensorNumber ++;
+            if (sensorNumber >= 4)
+            {
+            sensorNumber = 1;
+            adjustAngularVelocity(controller.compute(lineVoltages[lvIndex]));
+            lvIndex++;
+            }
         }
     
     void stopMovement(void)
@@ -95,7 +120,6 @@
 
 };
 
-
 /*
 Timeout timeToStop,
 void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
@@ -148,3 +172,27 @@
         }
     
 */
+//lineVoltages[lvIndex] += 0.5f;
+
+
+
+/*if input value is greater than the maximum value the left wheel can go, go full right TURN
+        if (W > leftWheel->returnMaxAngularVel()*AF)
+        {
+            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
+            rightWheel->adjustAngularVelocity(0); 
+        }
+        else if (W < (-1.0f*rightWheel->returnMaxAngularVel())) //if input value is less than the fastest the right wheel can go in reverse go full left TURN
+        {
+            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
+            leftWheel->adjustAngularVelocity(0); 
+        }
+        else if (W == 0)
+        {
+            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
+            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);  
+        }
+        else
+        {
+
+*/
\ No newline at end of file