Revision 5:f1613df66ceb, committed 2019-03-25

Comitter:

mazdo25

Date:

Mon Mar 25 22:42:31 2019 +0000

Parent:

4:208f5279143a

Child:

6:477382219bcf

Commit message:

LATEST WORKING VERSION;

Changed in this revision

--- a/P.h	Sat Mar 23 19:46:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-
-class P
-{
-    private:
-    float inMin;
-    float inMax;
-    
-    float control;
-    
-    float outMin;
-    float outMax;
-    
-    float gain;
-    float prevCA;
-    float controlAction;
-    
-    public:
-    P(float G)
-    {
-        gain = G;
-        outMin = 0.0f;
-        outMax = 1.0f;
-        control = 0;
-    }
-    
-    void setInputLimits(int inMin_, int inMax_)
-    {
-        if (inMin_ > inMax_) {return;} //cant be true can it
-        inMin = inMin_;
-        inMax = inMax_;
-    }
-    
-    void setInMax(float inMax_)
-    {
-        inMax = inMax_;
-    }
-    
-    float returnPrevCA()
-    {
-        return prevCA; 
-    }
-        
-    void setOutputLimits(float outMin_, float outMax_)
-    {
-        if (outMin_ > outMax_) {return;}
-        outMin = outMin_;
-        outMax = outMax_;
-    }
-    
-    float compute(float curVal_)
-    {
-        //if the measured value is greater than the measured amount, then clearly the limits are offfffffffffffffff
-        if (curVal_ > inMax) {inMax=curVal_;}
-        if (curVal_ < inMin) {inMin=curVal_;}
-        controlAction = ((control-curVal_)*gain); //amplify the error by the gain
-        if (controlAction > inMax ) {controlAction = inMax;}
-        if (controlAction < inMin) {controlAction = inMin;}
-        float prevCATemp = prevCA;
-        prevCA = controlAction;
-        controlAction += prevCATemp;
-        return (((controlAction-inMin)/(inMax - inMin))*(outMax-outMin))+outMin; //scales temp to the correct output Limits
-    }
-    
-    void setControl(float wantedVal_)
-    {
-        if (wantedVal_ > inMax) {control = inMax;}
-        else if (wantedVal_ < inMin) {control = inMin;}
-        else {control = wantedVal_;}
-    }
-    
-    float returnControl(void)
-    {
-        return control;
-    }
-    
-    float returnInMax()
-    {
-        return inMax;   
-    }
-    
-};
\ No newline at end of file

--- a/PID.h	Sat Mar 23 19:46:09 2019 +0000
+++ b/PID.h	Mon Mar 25 22:42:31 2019 +0000
@@ -3,8 +3,8 @@
     private:
     float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
     
-    float inMin_;
-    float inMax_;
+    float* inMin_;
+    float* inMax_;
     
     float outMin_;
     float outMax_;
@@ -17,8 +17,6 @@
         prevControlAction = 0;
         setPoint = 0;
         
-        inMin_ = -3.3f;
-        inMax_ = 3.3f;
         outMin_ = -1.0f;
         outMax_ = 1.0f;
         
@@ -30,23 +28,24 @@
     
     float compute (float currVal)
     {
-    if (currVal > 
     float currErr = setPoint - (currVal);
     float controlAction;
     
-    if (currErr > inMax_) {currErr = inMax_;}
-    if (currErr < inMin_) {currErr = inMin_;}
+    if (currVal > *inMax_) {*inMax_ = currVal;}
+    if (currVal < *inMin_) {*inMin_ = currVal;}
+    
+    
     
     controlAction = prevControlAction - (PrevErr*Kp_) + (Kp_*currErr)+ (Ki_*Ts*currErr) + ((Kd_/Ts)*(currErr - PrevErr - PrevErr + PrevPrevErr));
     
-    if (controlAction > inMax_) {controlAction = inMax_;}
-    if (controlAction < inMin_) {controlAction = inMin_;} 
+    if (controlAction > *inMax_) {controlAction = *inMax_;}
+    if (controlAction < *inMin_) {controlAction = *inMin_;} 
    
     prevControlAction = controlAction;
     PrevPrevErr = PrevErr;
     PrevErr = currErr;
     //scale the control Action to the correct output limits and output
-    return ((((controlAction)- inMin_)/(inMax_ - inMin_)) * (outMax_ - outMin_)) + outMin_;
+    return ((((controlAction)- *inMin_)/(*inMax_ - *inMin_)) * (outMax_ - outMin_)) + outMin_;
     }
     
     void setSetPoint(float _sP)
@@ -62,16 +61,15 @@
         outMax_ = outMax;
     }
     
-    void setInputLimits(float inMin,float inMax)
+    void assignLimitAddress(float *inMax, float *inMin)
     {
-        if (inMin > inMax) {return;}
-        //scales the previous errors and control action to correct input limits
-        PrevPrevErr = scaler(inMin_,inMax_,inMin,inMax,PrevPrevErr);
-        PrevErr = scaler(inMin_,inMax_,inMin,inMax,PrevErr);
-        prevControlAction = scaler(inMin_,inMax_,inMin,inMax,prevControlAction);
-        
+        inMax_ = inMax; 
         inMin_ = inMin;
-        inMax_ = inMax;
+    }
+    
+    float returnInMax() 
+    {
+        return  *inMax_;
     }
     
     float scaler(float prevMin, float prevMax, float newMin, float newMax, float var)
@@ -86,11 +84,6 @@
         return   prevControlAction;
     }
     
-    float returnInMax()
-    {
-        return inMax_;   
-    }
-    
 };
 
 //(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID2.h	Mon Mar 25 22:42:31 2019 +0000
@@ -0,0 +1,82 @@
+class PID2
+{
+    private:
+    float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
+    
+    float inMin_;
+    float inMax_;
+    
+    float* outMin_;
+    float* outMax_;
+    
+    public:
+    PID2 (float Kp, float Ki, float Kd, float sampleTime)
+    {
+        PrevErr = 0;
+        PrevPrevErr = 0;
+        prevControlAction = 0;
+        setPoint = 0;
+        
+        inMin_ = -1.0f;
+        inMax_ = 1.0f;
+        
+        Ts = sampleTime;
+        Kp_ = Kp;
+        Ki_ = Ki;
+        Kd_ = Kd;
+    }
+    
+    float compute (float currVal)
+    {
+    float currErr = setPoint - currVal;
+    float controlAction;
+    
+    if (currVal > inMax_) {inMax_ = currVal;}
+    if (currVal < inMin_) {inMin_ = currVal;}
+    
+    controlAction = prevControlAction - (PrevErr*Kp_) + (Kp_*currErr)+ (Ki_*Ts*currErr) + ((Kd_/Ts)*(currErr - PrevErr - PrevErr + PrevPrevErr));
+    
+    if (controlAction > inMax_) {controlAction = inMax_;}
+    if (controlAction < inMin_) {controlAction = inMin_;} 
+   
+    prevControlAction = controlAction;
+    PrevPrevErr = PrevErr;
+    PrevErr = currErr;
+    //scale the control Action to the correct output limits and output
+    return ((((controlAction)- inMin_)/(inMax_ - inMin_)) * (*outMax_ - *outMin_)) + *outMin_;
+    }
+    
+    void setSetPoint(float _sP)
+    {
+        setPoint = _sP;
+    }
+    
+    void setInputLimits(float inMin,float inMax)
+    {
+        if (inMin > inMax) {return;}
+        
+        inMin_ = inMin;
+        inMax_ = inMax;
+    }
+    
+    void assignLimitAddress(float *outMax, float *outMin)
+    {
+        outMin_ = outMax; 
+        outMax_ = outMin;
+    }
+    
+    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;
+    }
+    
+    float returnPrevCA()
+    {
+        return   prevControlAction;
+    }
+    
+};
+
+//(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin
\ No newline at end of file

--- 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()
+    {
+        
+    }
 
 };
 

--- a/SensorControl/lineSensor.h	Sat Mar 23 19:46:09 2019 +0000
+++ b/SensorControl/lineSensor.h	Mon Mar 25 22:42:31 2019 +0000
@@ -9,7 +9,7 @@
     Timeout sampler;
     
     public:
-    float static const sampleTime = 0.1f;
+    float static const sampleTime = 0.00004f;
     
     lineSensor(PinName E, PinName R):emitter(E), receiver(R)
     {   

--- a/WheelControl/LWheel.h	Sat Mar 23 19:46:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,119 +0,0 @@
-class LWheel 
-{
-    private:
-    
-    float distance; //distance traversed by wheel
-    float angularVelocity;
-    
-        float const static gain = 0.9f; //closed loop gain, (amount to amplify the difference) you have to tune this value
-    //but make sure its less than 1.5 otherwise you'll have a really sensitive motor
-    
-    PwmOut Mtr; //connect this pin to the motor driveboard pwm
-    DigitalOut direction; //connected to the direction pin of motor drive board
-    DigitalOut polarity; //connected to the bipolar of motor drive board. 0 = unipolar 1 = bipolar
-    
-    Ticker updater;
-
-    Encoder* enc;
-    
-    PID controller;
-    
-    public:
-    
-    float  maxAngularVel;
-    
-    float static const wheelDiameter = 0.18; //used in calculation of Linear velocity i.e never
-    
-    LWheel (Encoder* E, PinName M, PinName D, PinName Mode) : Mtr(M), direction(D), polarity(Mode), controller(gain,6.0f,0.0f,0.05f)
-        {
-        maxAngularVel = 0.0f;
-        enc = E;
-        polarity = 0;   
-        direction = 0;
-        distance = 0;
-        Mtr.period_us(100); //frequency of 5KHz determine this constant value based on switching losses+frequency losses
-        //higher freq -> more switching losses lower freq -> more "capacitive losses" need to find a balance
-        Mtr.write(1); //start off on the turned off state
-        
-        updater.detach();
-        
-        controller.setOutputLimits(-1.0f, 1.0f);
-        }
-    
-    void calculateAngularVelocity() //returns a float value which is the angular velocity of the WHEEL
-    {
-        float eTR = enc->encoderTickRate();
-        angularVelocity = (eTR/256.0f)*2.0f*(float)PI;
-    }
-    
-    void setFrequency(int freq) //if you want to adjust the frequency
-    {
-        Mtr.period(1/freq);
-    }
-    
-    float returnAngularVelocity() //as it says
-    {
-        return angularVelocity;
-    }
-    
-    //only called once during initialization to calculate max angular velocity 
-    //dir = direction, do opposite for each wheel just so your buggy doesn't move FORWARD but rather rotates
-    void init(int dir) 
-    {
-      enc->startTimer();
-      Mtr.write(0); //max speed
-      angularVelocity = 10.0f;
-      if (dir != 0) {direction = 1;} else {direction = 0;}
-      updater.attach(callback(this, &LWheel::init2),0.6f); //used as a wait preferably put this wait just long enough that the buggy will do a full 360 degree turn so that it hasn't moved
-    }
-    
-    void init2(void) //used as a temporarily wait command for the wheel to spin to max
-    {
-        updater.detach();
-        calculateAngularVelocity();
-        maxAngularVel = abs(angularVelocity);
-        controller.setInputLimits(-1.0f*abs(angularVelocity),abs(angularVelocity));
-        controller.setSetPoint(0.0f);
-        updater.attach(callback(this, &LWheel::wheelUpdates),0.05f);
-    }
-    
-    void startController()
-    {
-       updater.attach(callback(this, &LWheel::wheelUpdates),0.05f); 
-    }
-    
-    void stopController()
-    {
-        updater.detach();
-    }
-    
-    void wheelUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
-        {
-            calculateAngularVelocity();
-            if (controller.returnPrevCA() >= controller.returnInMax())
-            {
-                maxAngularVel = abs(angularVelocity);
-                controller.setInputLimits(-1.0f*maxAngularVel,maxAngularVel);
-            }
-            float temp2 = controller.compute(angularVelocity); //another temporary value to store the computed angular velocity
-            if (temp2 < 0) {direction = 0;} else {direction = 1;} //change direction according to the computed value
-            Mtr.write((1.0f - abs(temp2))); //write the value as a pwm
-        }
-        
-    void adjustAngularVelocity(float W) // W = angular velocity you want, obviously putting a |w| value that is > max angular velocity will set dutcy cycle to max
-        {
-        controller.setSetPoint(W);
-        if (W < 0.0f) {direction = 0;} else {direction = 1;} //obvs if you put a negative value -> will get a negative direction i.e 0;
-        };
-        
-    float getDistance(void)
-    {
-    return distance; //distance traversed by wheel
-    }
-    
-    float returnMaxAngularVel(void)
-    {
-        return  maxAngularVel;
-    }
-    
-};
\ No newline at end of file

--- a/WheelControl/RWheel.h	Sat Mar 23 19:46:09 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,109 +0,0 @@
-class RWheel 
-{
-    private:
-    
-    float distance; //distance traversed by wheel
-    float angularVelocity;
-    
-        float const static gain = 0.9f; //closed loop gain, (amount to amplify the difference) you have to tune this value
-    //but make sure its less than 1.5 otherwise you'll have a really sensitive motor
-    
-    PwmOut Mtr; //connect this pin to the motor driveboard pwm
-    DigitalOut direction; //connected to the direction pin of motor drive board
-    DigitalOut polarity; //connected to the bipolar of motor drive board. 0 = unipolar 1 = bipolar
-    
-    Ticker updater;
-
-    Encoder* enc;
-    
-    PID controller;
-    
-    public:
-    
-    float  maxAngularVel;
-    
-    float static const wheelDiameter = 0.18; //used in calculation of Linear velocity i.e never
-    
-    RWheel (Encoder* E, PinName M, PinName D, PinName Mode) : Mtr(M), direction(D), polarity(Mode), controller(gain,6.0f,0.0f,0.05f)
-        {
-        maxAngularVel = 0.0f;
-        enc = E;
-        polarity = 0;   
-        direction = 0;
-        distance = 0;
-        Mtr.period_us(100); //frequency of 5KHz determine this constant value based on switching losses+frequency losses
-        //higher freq -> more switching losses lower freq -> more "capacitive losses" need to find a balance
-        Mtr.write(1); //start off on the turned off state
-        
-        updater.detach();
-        
-        controller.setOutputLimits(-1.0f, 1.0f);
-        }
-    
-    void calculateAngularVelocity() //returns a float value which is the angular velocity of the WHEEL
-    {
-        float eTR = enc->encoderTickRate();
-        angularVelocity = (eTR/256.0f)*2.0f*(float)PI;
-    }
-    
-    void setFrequency(int freq) //if you want to adjust the frequency
-    {
-        Mtr.period(1/freq);
-    }
-    
-    float returnAngularVelocity() //as it says
-    {
-        return angularVelocity;
-    }
-    
-    //only called once during initialization to calculate max angular velocity 
-    //dir = direction, do opposite for each wheel just so your buggy doesn't move FORWARD but rather rotates
-    void init(int dir) 
-    {
-      enc->startTimer();
-      Mtr.write(0); //max speed
-      angularVelocity = 10.0f;
-      if (dir != 0) {direction = 0;}
-      updater.attach(callback(this, &RWheel::init2),0.6f); //used as a wait preferably put this wait just long enough that the buggy will do a full 360 degree turn so that it hasn't moved
-    }
-    
-    void init2(void) //used as a temporarily wait command for the wheel to spin to max
-    {
-        updater.detach();
-        calculateAngularVelocity();
-        maxAngularVel = abs(angularVelocity);
-        controller.setInputLimits(-1.0f*maxAngularVel,maxAngularVel);
-        controller.setSetPoint(0.0f);
-        updater.attach(callback(this, &RWheel::wheelUpdates),0.05f);
-    }
-    
-    void wheelUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
-        {
-            calculateAngularVelocity();
-            if (controller.returnPrevCA() >= controller.returnInMax())
-            {
-                maxAngularVel = abs(angularVelocity);
-                controller.setInputLimits(-1.0f*maxAngularVel,maxAngularVel);
-            }
-            float temp2 = controller.compute(angularVelocity); //another temporary value to store the computed angular velocity
-            if (temp2 < 0) {direction = 1;} else {direction = 0;} //change direction according to the computed value
-            Mtr.write((1.0f - abs(temp2))); //write the value as a pwm
-        }
-        
-    void adjustAngularVelocity(float W) // W = angular velocity you want, obviously putting a |w| value that is > max angular velocity will set dutcy cycle to max
-        {
-        controller.setSetPoint(W);
-        if (W < 0.0f) {direction = 1;} else {direction = 0;} //obvs if you put a negative value -> will get a negative direction i.e 0;
-        };
-        
-    float getDistance(void)
-    {
-    return distance; //distance traversed by wheel
-    }
-    
-    float returnMaxAngularVel(void)
-    {
-        return  maxAngularVel;
-    }
-    
-};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WheelControl/Wheel.h	Mon Mar 25 22:42:31 2019 +0000
@@ -0,0 +1,131 @@
+class Wheel 
+{
+    private:
+    
+    
+    
+    float distance; //distance traversed by wheel
+    float angularVelocity;
+    
+        float const static gain = 0.9f; //closed loop gain, (amount to amplify the difference) you have to tune this value
+    //but make sure its less than 1.5 otherwise you'll have a really sensitive motor
+    
+    PwmOut Mtr; //connect this pin to the motor driveboard pwm
+    DigitalOut direction; //connected to the direction pin of motor drive board
+    DigitalOut polarity; //connected to the bipolar of motor drive board. 0 = unipolar 1 = bipolar
+    
+    Ticker updater;
+
+    Encoder* enc;
+    
+    PID controller;
+    
+    public:
+    
+    float   maxAngularVel;
+    float   minAngularVel; 
+    
+    float static const wheelDiameter = 0.18; //used in calculation of Linear velocity i.e never
+    
+    Wheel (Encoder* E, PinName M, PinName D, PinName Mode) : Mtr(M), direction(D), polarity(Mode), controller(gain,2.0f,0.0f,0.0005f)
+        {
+        maxAngularVel = 0.0f;
+        enc = E;
+        polarity = 0;   
+        direction = 0;
+        distance = 0;
+        Mtr.period_us(100); //frequency of 5KHz determine this constant value based on switching losses+frequency losses
+        //higher freq -> more switching losses lower freq -> more "capacitive losses" need to find a balance
+        Mtr.write(1); //start off on the turned off state
+        
+        updater.detach();
+        
+        float *maxAVptr = &maxAngularVel;
+        float *minAVPtr = &minAngularVel;
+        controller.assignLimitAddress(maxAVptr,minAVPtr);
+        }
+    
+    void calculateAngularVelocity() //returns a float value which is the angular velocity of the WHEEL
+    {
+        float eTR = enc->encoderTickRate();
+        angularVelocity = (eTR/256.0f)*2.0f*(float)PI;
+    }
+    
+    void setFrequency(int freq) //if you want to adjust the frequency
+    {
+        Mtr.period(1/freq);
+    }
+    
+    float returnAngularVelocity() //as it says
+    {
+        return angularVelocity;
+    }
+    
+    //only called once during initialization to calculate max angular velocity 
+    //dir = direction, do opposite for each wheel just so your buggy doesn't move FORWARD but rather rotates
+    void init(int dir) 
+    {
+      enc->startTimer();
+      Mtr.write(0); //max speed
+      angularVelocity = 10.0f;
+      if (dir == 0) {direction = 0;} else {direction = 1;}
+      updater.attach(callback(this, &Wheel::init2),0.6f); //used as a wait preferably put this wait just long enough that the buggy will do a full 360 degree turn so that it hasn't moved
+    }
+    
+    void init2(void) //used as a temporarily wait command for the wheel to spin to max
+    {
+        updater.detach();
+        calculateAngularVelocity();
+        maxAngularVel = 60.0f;
+        minAngularVel = -1.0f*maxAngularVel;
+        controller.setSetPoint(0.0f);
+        updater.attach(callback(this, &Wheel::wheelUpdates),0.0005f);
+    }
+    
+    void startController()
+    {
+       updater.attach(callback(this, &Wheel::wheelUpdates),0.01f); 
+    }
+    
+    void stopController()
+    {
+        updater.detach();
+    }
+    
+    void wheelUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
+        {
+            calculateAngularVelocity();
+            float temp2 = controller.compute(angularVelocity); //another temporary value to store the computed angular velocity
+            if (temp2 < 0) {direction = 0;} else {direction = 1;} //change direction according to the computed value
+            Mtr.write((1.0f - abs(temp2))); //write the value as a pwm
+        }
+        
+    void adjustAngularVelocity(float W) // W = angular velocity you want, obviously putting a |w| value that is > max angular velocity will set dutcy cycle to max
+        {
+        controller.setSetPoint(W);
+        if (W < 0.0f) {direction = 0;} else {direction = 1;} //obvs if you put a negative value -> will get a negative direction i.e 0;
+        };
+        
+    float getDistance(void)
+    {
+    return distance; //distance traversed by wheel
+    }
+    
+    float returnMaxAngularVel(void)
+    {
+        return  maxAngularVel;
+    }
+    
+    float* returnMaxAVptr()
+    {
+        float *tempPTR = &maxAngularVel;
+        return tempPTR;
+    }
+    
+    float* returnMinAVptr()
+    {
+        float *tempPTR = &minAngularVel;
+        return tempPTR;
+    }
+    
+};
\ No newline at end of file

--- a/main.cpp	Sat Mar 23 19:46:09 2019 +0000
+++ b/main.cpp	Mon Mar 25 22:42:31 2019 +0000
@@ -4,17 +4,15 @@
 #include "mbed.h"
 #include "Encoder.h"
 #include "lineSensor.h"
-#include "P.h"
+#include "PID2.h"
 #include "PID.h"
-#include "LWheel.h"
-#include "RWheel.h"
+#include "Wheel.h"
 #include "Robot.h"
 
 
 //blue D8
 //Green D9
 //Red D5
-int i = 0;
 int main() {
     Serial pc(USBTX, USBRX);
     
@@ -23,13 +21,13 @@
     
     Encoder* RE = new Encoder(PB_3,PB_5);
     Encoder* LE = new Encoder(PB_10,PB_4);
-    LWheel* leftWheel = new LWheel(LE,PC_8,PA_9, PA_14);
-    RWheel* rightWheel = new RWheel(RE,PC_6,PA_8, PB_6);
+    Wheel* leftWheel = new Wheel(LE,PC_8,PA_9, PA_14);
+    Wheel* rightWheel = new Wheel(RE,PC_6,PA_8, PA_7);
     
     //an array of lineSensor pointers params: lineSensor(PinName emitter Pin, PinName reciever Pin, make sure it is from LEFT TO RIGHT
     lineSensor* sensorArray[6] = {new lineSensor(PB_9,A0),new lineSensor(PC_11,A1),new lineSensor(PD_2,A2),new lineSensor(PC_10,A3),new lineSensor(PB_8,A4),new lineSensor(PC_12,A5)};
 
-    leftWheel->init(0);
+    leftWheel->init(1);
     
     while(leftWheel->returnAngularVelocity()!= 0)
     {
@@ -41,16 +39,12 @@
     {
      pc.printf("Stage 2, speed %f\r\n",rightWheel->returnAngularVelocity()); 
     }
-    pc.printf("L , speed%f\r\n",leftWheel->returnMaxAngularVel());
+    pc.printf("L , speedM%f\r\n",leftWheel->returnMaxAngularVel());
     pc.printf("R , speedM %f\r\n",rightWheel->returnMaxAngularVel());
-    //Robot rbt(leftWheel, rightWheel, sensorArray);
-    leftWheel->adjustAngularVelocity(40.0f);
-    rightWheel->adjustAngularVelocity(-64.0f);
+    
+    Robot rbt(leftWheel, rightWheel, sensorArray);
     while(1)
     {
-    pc.printf("L , speed%f\r\n",leftWheel->returnAngularVelocity());
-    pc.printf("R , speedM %f\r\n",rightWheel->returnAngularVelocity());
     }
     
 }
-//lineSensor* sensorArray[6] = {new lineSensor(D11,A0),new lineSensor(D7,A1),new lineSensor(D6,A2),new lineSensor(D5,A3),new lineSensor(D4,A4),new lineSensor(D3,A5)};
\ No newline at end of file