Diff: Robot.h

Revision:

3:01b5e80d842d

Child:

4:208f5279143a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h	Sat Mar 09 14:27:48 2019 +0000
@@ -0,0 +1,150 @@
+class Robot {
+    private:
+    float static const distanceBetweenWheels = 0.19;
+    int static const numberOfSamples = 100;
+    int static const numberOfSensors = 6;
+    
+    Wheel* leftWheel;
+    Wheel* rightWheel;
+    lineSensor* sensorArray[numberOfSensors];
+    PID controller;
+    float lineVoltages[numberOfSamples];
+    int lvIndex;
+    int sensorNumber;
+    Ticker updater;
+    
+    public:
+    
+    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(1,0,10,0.1)
+    {
+        lvIndex = 1;
+        sensorArray[0] = SA[0];
+        leftWheel = LW;
+        rightWheel= RW;
+        
+        lineVoltages[0] = -16.5f;
+        //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); 
+    };
+    
+    
+    float calculateTranslationalVelocity()
+    {
+        return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
+    }
+        
+    float calculateAngularVelocity()
+    {
+        return (((leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity())/distanceBetweenWheels));
+    }
+    
+    //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();
+            if (W < 0)
+            {
+                leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()*0.8f-rightWheel->returnAngularVelocity()) + temp);
+                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
+            }
+            else
+            {
+                rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()*0.8f-leftWheel->returnAngularVelocity()) + temp);
+                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);
+            }
+        }
+        
+    }
+    
+    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());
+        }
+    
+    void stopMovement(void)
+    {
+        leftWheel->adjustAngularVelocity(0);
+        rightWheel->adjustAngularVelocity(0);
+    }
+
+};
+
+
+/*
+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
+    {
+        timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
+    }
+    
+    void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
+        {
+            sensorArray[sensorNumber]->sample();
+            
+            if (sensorNumber < (numberOfSensors/2))
+            {
+                lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
+            }
+            else
+            }
+                lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
+            }
+            
+            sensorNumber++;
+            if (sensorNumber % numberOfSensors == 0)
+            {
+            sensorNumber = 0;
+            controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
+            adjustAngularVelocity(controller.compute());
+            lvIndex++;
+            }
+        }
+    
+*/