Diff: Robot.h

Revision:

8:5ed6685f6edd

Parent:

7:cb07cdb35b6c

Child:

9:cefa177c1353

--- a/Robot.h	Thu Mar 28 01:07:54 2019 +0000
+++ b/Robot.h	Sun Mar 31 20:31:30 2019 +0000
@@ -1,4 +1,4 @@
-class Robot {
+ class Robot {
     private:
     float static const distanceBetweenWheels = 0.19; //currently not used may be used later
     int static const numberOfSamples = 100; //number of samples the line voltage array will hold
@@ -13,8 +13,9 @@
     int sensorNumber;
     
     Ticker updater;
+    Timeout timeToStop;
     
-    int endOfLineDetection;
+    float endOfLineDetection;
     float Reflec;
     
     float RoboticAngularVelocity;
@@ -26,10 +27,11 @@
     Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0006f) //this controller actually does nothing atm
     {
         updater.detach();
+        timeToStop.detach();
         AF  = 0.3f; //change this value to change max speedzzzzzzzzzzzzzzzzz
-        state = 'S';
-        
-        Reflec = 0;        
+        state = 'F';
+        Reflec = 0;
+          
         lvIndex = 0;
         sensorNumber = 0;
         sensorArray[0] = SA[0];
@@ -42,6 +44,9 @@
         rightWheel= RW;
         
         lineVoltages[lvIndex] = 0.0f;
+        
+        controller.setInputLimits(-300.0f,300.0f);
+        controller.setSetPoint(0.0f);
         //controller will output a value between +- max speed of wheels
         
         controller.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
@@ -64,7 +69,7 @@
     }
     
     //attempts to modify the angular velocity of the buggy
-    void adjustAngularVelocity(float W)
+    void adjustRbtAngularVelocity(float W)
     {
             //negative is a right turn
             if (W < 0)
@@ -81,64 +86,24 @@
     
     void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
         {
-
-                float ambientLight;
-                float reading;
-                ambientLight = sensorArray[sensorNumber]->calcLineVoltage();  
-                sensorArray[sensorNumber]->sample();
-                sensorArray[sensorNumber]->calcLineVoltage();
-                switch (sensorNumber)
-                {
-                case 0:
-                    lineVoltages[lvIndex%numberOfSamples] = ((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f);
-                    break;
-                case 1:
-                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f);
-                    break;
-                case 2:
-                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f);
-                    break;
-                case 3:
-                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f);
-                    break;
-                case 4: 
-                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f);
-                    break;
-                case 5:
-                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f);
+            switch (state)
+            {
+                case 'T':
+                    turn180();
                     break;
-                }
-                sensorNumber ++;
-                if (reading <= Reflec*11) {endOfLineDetection++;}
-            
-                if (sensorNumber >= numberOfSensors)
-                {
-                    if (endOfLineDetection <= 6)
-                    {
-                        AF  = 0.3f;
-                        sensorNumber = 0;
-                        adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
-                        lvIndex++;
-                    }
-                    else 
-                    {
-                        if (AF > 0) //this will Slowly grind the buggy to a HALT to overcome line breaks and overshooting in turns -> dont want it to instantly stop
-                        {
-                        AF  -= 0.1f;
-                        adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples-1]); //base your action on the previous measured value
-                        }
-                        else //well shit, you're buggy must have totally overshot the line, lets just hope while spinning it can find a line, direction not guaranteed
-                        {
-                        spin(); //spin like mad and hope you find a line
-                        }
-                    }
-                }
+                case 'S':
+                    stopMovement();   
+                    break;
+                default:
+                    followLine();
+                    break;
+            }
         }
     
     void stopMovement(void)
     {
-        leftWheel->adjustAngularVelocity(0);
-        rightWheel->adjustAngularVelocity(0);
+        leftWheel->adjustAngularVelocity(0.0f);
+        rightWheel->adjustAngularVelocity(0.0f);
     }
     
     float returnLineVoltage()
@@ -149,10 +114,9 @@
     void rbtInit()
     {
     sensorArray[0]->sample();
-    Reflec = sensorArray[0]->calcLineVoltage();
-    controller.setInputLimits(-300.0f,300.0f);
-    controller.setSetPoint(0.0f);
-    updater.attach(callback(this, &Robot::robotUpdates),0.0001f);   
+    sensorArray[0]->calcLineVoltage();
+    Reflec = sensorArray[0]->returnLineVoltage();
+    updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
     }
     
     void spin()
@@ -165,31 +129,85 @@
     {
         state = S;
     }
+    
+    void followLine(void)
+    {
+        float ambientLight;
+        float reading;
+        ambientLight = sensorArray[sensorNumber]->calcLineVoltage();  
+        sensorArray[sensorNumber]->sample();
+        sensorArray[sensorNumber]->calcLineVoltage();
+        reading = (sensorArray[sensorNumber]->returnLineVoltage()-ambientLight);
+        switch (sensorNumber)
+        {
+        case 0:
+            lineVoltages[lvIndex%numberOfSamples] = (reading * -100.0f);
+            if (reading <= Reflec*10){endOfLineDetection++;}
+            break;
+        case 1:
+            lineVoltages[lvIndex%numberOfSamples] += (reading * -50.0f);
+            if (reading <= Reflec*10) {endOfLineDetection++;}
+            break;
+        case 2:
+            lineVoltages[lvIndex%numberOfSamples] += (reading * -25.0f);
+            if (reading <= 0.5f) {endOfLineDetection++;}
+            break;
+        case 3:
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 25.0f);
+            if (reading <= 0.5f) {endOfLineDetection++;}
+            break;
+        case 4: 
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 50.0f);
+            if (reading <= Reflec*10) {endOfLineDetection++;}
+            break;
+        case 5:
+            lineVoltages[lvIndex%numberOfSamples] += (reading * 100.0f);
+            if (reading <= Reflec*10) {endOfLineDetection++;}
+            break;
+        }
+                
+        sensorNumber ++;
+            
+        if (sensorNumber >= numberOfSensors)
+        {
+            sensorNumber = 0;
+            if (endOfLineDetection < 6)
+            {
+                AF  = 0.3f;
+                adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
+                lvIndex++;
+            }
+            else if (AF > 0.1f)
+            {
+                AF -= 0.1f;
+                adjustRbtAngularVelocity(lineVoltages[lvIndex%numberOfSamples]-1);
+            }else{
+                AF = 0.0f;
+                stopMovement();
+                }
+        endOfLineDetection = 0;
+        }
+    }
+    
+    void turn180()
+    {
+        updater.detach();
+        rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
+        leftWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*-0.3f);
+        timeToStop.attach(callback(this, &Robot::reAttach),1.3f);
+        state = 'S';
+    }
+    
+    void reAttach()
+    {
+        updater.attach(callback(this, &Robot::robotUpdates),0.0001f);
+    }
 
 };
 
 /*
 Timeout timeToStop,
-void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
-    {
-        float ratioOfTurn = ((abs((int)degrees) % 360)/360); //returns a value 0 -> 0.9999999 which is how much turning it has to do
-        if (ratioOfTurn == 0.0f) {return;}
-        rightWheel->adjustAngularVelocity(0);
-        leftWheel->adjustAngularVelocity(0);
-        wait(0.1);
-        if (degrees > 0 )
-        {
-        rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
-        leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
-        }
-        else
-        {
-        rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
-        leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);    
-        } 
-            
-        timeToStop.attach(callback(this, &Robot::stopMovement),ratioOfTurn*((distanceBetweenWheels*(float)PI)/(((rightWheel->maxAngularVel)*-1.0f) * 256.0f * 2.0f * (float)PI * Wheel::wheelDiameter)));
-    }
+
     
     void travelDistance(float d)//in metres
     {