Revision 4:208f5279143a, committed 2019-03-23

Comitter:

mazdo25

Date:

Sat Mar 23 19:46:09 2019 +0000

Parent:

3:01b5e80d842d

Child:

5:f1613df66ceb

Commit message:

latest working, but line following not;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/P.h	Sat Mar 23 19:46:09 2019 +0000
@@ -0,0 +1,81 @@
+
+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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Sat Mar 23 19:46:09 2019 +0000
@@ -0,0 +1,96 @@
+class PID
+{
+    private:
+    float Kp_, Ki_, Kd_, Ts, PrevErr, PrevPrevErr, prevControlAction, setPoint;
+    
+    float inMin_;
+    float inMax_;
+    
+    float outMin_;
+    float outMax_;
+    
+    public:
+    PID (float Kp, float Ki, float Kd, float sampleTime)
+    {
+        PrevErr = 0;
+        PrevPrevErr = 0;
+        prevControlAction = 0;
+        setPoint = 0;
+        
+        inMin_ = -3.3f;
+        inMax_ = 3.3f;
+        outMin_ = -1.0f;
+        outMax_ = 1.0f;
+        
+        Ts = sampleTime;
+        Kp_ = Kp;
+        Ki_ = Ki;
+        Kd_ = Kd;
+    }
+    
+    float compute (float currVal)
+    {
+    if (currVal > 
+    float currErr = setPoint - (currVal);
+    float controlAction;
+    
+    if (currErr > inMax_) {currErr = inMax_;}
+    if (currErr < inMin_) {currErr = inMin_;}
+    
+    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 setOutputLimits(float outMin,float outMax)
+    {
+        if (outMin > outMax) {return;}
+        
+        outMin_ = outMin;
+        outMax_ = outMax;
+    }
+    
+    void setInputLimits(float inMin,float inMax)
+    {
+        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);
+        
+        inMin_ = inMin;
+        inMax_ = inMax;
+    }
+    
+    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;
+    }
+    
+    float returnInMax()
+    {
+        return inMax_;   
+    }
+    
+};
+
+//(((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin
\ No newline at end of file

--- a/PID/PID.cpp	Sat Mar 09 14:27:48 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,324 +0,0 @@
-/**
- * @author Aaron Berk
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- * 
- * A PID controller is a widely used feedback controller commonly found in
- * industry.
- *
- * This library is a port of Brett Beauregard's Arduino PID library:
- *
- *  http://www.arduino.cc/playground/Code/PIDLibrary
- *
- * The wikipedia article on PID controllers is a good place to start on
- * understanding how they work:
- *
- *  http://en.wikipedia.org/wiki/PID_controller
- *
- * For a clear and elegant explanation of how to implement and tune a
- * controller, the controlguru website by Douglas J. Cooper (who also happened
- * to be Brett's controls professor) is an excellent reference:
- *
- *  http://www.controlguru.com/
- */
-
-/**
- * Includes
- */
-#include "PID.h"
-
-PID::PID(float Kc, float tauI, float tauD, float interval) {
-
-    usingFeedForward = false;
-    inAuto           = false;
-
-    //Default the limits to the full range of I/O: 3.3V
-    //Make sure to set these to more appropriate limits for
-    //your application.
-    setInputLimits(0.0, 3.3);
-    setOutputLimits(0.0, 3.3);
-
-    tSample_ = interval;
-
-    setTunings(Kc, tauI, tauD);
-
-    setPoint_             = 0.0;
-    processVariable_      = 0.0;
-    prevProcessVariable_  = 0.0;
-    controllerOutput_     = 0.0;
-    prevControllerOutput_ = 0.0;
-
-    accError_ = 0.0;
-    bias_     = 0.0;
-    
-    realOutput_ = 0.0;
-
-}
-
-void PID::setInputLimits(float inMin, float inMax) {
-
-    //Make sure we haven't been given impossible values.
-    if (inMin >= inMax) {
-        return;
-    }
-
-    //Rescale the working variables to reflect the changes.
-    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
-    accError_            *= (inMax - inMin) / inSpan_;
-
-    //Make sure the working variables are within the new limits.
-    if (prevProcessVariable_ > 1) {
-        prevProcessVariable_ = 1;
-    } else if (prevProcessVariable_ < 0) {
-        prevProcessVariable_ = 0;
-    }
-
-    inMin_  = inMin;
-    inMax_  = inMax;
-    inSpan_ = inMax - inMin;
-
-}
-
-void PID::setOutputLimits(float outMin, float outMax) {
-
-    //Make sure we haven't been given impossible values.
-    if (outMin >= outMax) {
-        return;
-    }
-
-    //Rescale the working variables to reflect the changes.
-    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
-
-    //Make sure the working variables are within the new limits.
-    if (prevControllerOutput_ > 1) {
-        prevControllerOutput_ = 1;
-    } else if (prevControllerOutput_ < 0) {
-        prevControllerOutput_ = 0;
-    }
-
-    outMin_  = outMin;
-    outMax_  = outMax;
-    outSpan_ = outMax - outMin;
-
-}
-
-void PID::setTunings(float Kc, float tauI, float tauD) {
-
-    //Verify that the tunings make sense.
-    if (Kc == 0.0f || tauI < 0.0f || tauD < 0.0f) {
-        return;
-    }
-
-    //Store raw values to hand back to user on request.
-    pParam_ = Kc;
-    iParam_ = tauI;
-    dParam_ = tauD;
-
-    float tempTauR;
-
-    if (tauI == 0.0f) {
-        tempTauR = 0.0f;
-    } else {
-        tempTauR = (1.0f / tauI) * tSample_;
-    }
-
-    //For "bumpless transfer" we need to rescale the accumulated error.
-    if (inAuto) {
-        if (tempTauR == 0.0f) {
-            accError_ = 0.0f;
-        } else {
-            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
-        }
-    }
-
-    Kc_   = Kc;
-    tauR_ = tempTauR;
-    tauD_ = tauD / tSample_;
-
-}
-
-void PID::reset(void) {
-
-    float scaledBias = 0.0f;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    } else {
-        scaledBias = (realOutput_ - outMin_) / outSpan_;
-    }
-
-    prevControllerOutput_ = scaledBias;
-    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
-
-    //Clear any error in the integral.
-    accError_ = 0;
-
-}
-
-void PID::setMode(int mode) {
-
-    //We were in manual, and we just got set to auto.
-    //Reset the controller internals.
-    if (mode != 0 && !inAuto) {
-        reset();
-    }
-
-    inAuto = (mode != 0);
-
-}
-
-void PID::setInterval(float interval) {
-
-    if (interval > 0) {
-        //Convert the time-based tunings to reflect this change.
-        tauR_     *= (interval / tSample_);
-        accError_ *= (tSample_ / interval);
-        tauD_     *= (interval / tSample_);
-        tSample_   = interval;
-    }
-
-}
-
-void PID::setSetPoint(float sp) {
-
-    setPoint_ = sp;
-
-}
-
-void PID::setProcessValue(float pv) {
-
-    processVariable_ = pv;
-
-}
-
-void PID::setBias(float bias){
-
-    bias_ = bias;
-    usingFeedForward = 1;
-
-}
-
-float PID::compute() {
-
-    //Pull in the input and setpoint, and scale them into percent span.
-    float scaledPV = (processVariable_ - inMin_) / inSpan_;
-
-    if (scaledPV > 1.0f) {
-        scaledPV = 1.0f;
-    } else if (scaledPV < 0.0f) {
-        scaledPV = 0.0;
-    }
-
-    float scaledSP = (setPoint_ - inMin_) / inSpan_;
-    if (scaledSP > 1.0f) {
-        scaledSP = 1;
-    } else if (scaledSP < 0.0f) {
-        scaledSP = 0;
-    }
-
-    float error = scaledSP - scaledPV;
-
-    //Check and see if the output is pegged at a limit and only
-    //integrate if it is not. This is to prevent reset-windup.
-    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
-        accError_ += error;
-    }
-
-    //Compute the current slope of the input signal.
-    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
-
-    float scaledBias = 0.0;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    }
-
-    //Perform the PID calculation.
-    controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
-
-    //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0f) {
-        controllerOutput_ = 0.0;
-    } else if (controllerOutput_ > 1.0f) {
-        controllerOutput_ = 1.0;
-    }
-
-    //Remember this output for the windup check next time.
-    prevControllerOutput_ = controllerOutput_;
-    //Remember the input for the derivative calculation next time.
-    prevProcessVariable_  = scaledPV;
-
-    //Scale the output from percent span back out to a real world number.
-    return ((controllerOutput_ * outSpan_) + outMin_);
-
-}
-
-float PID::getInMin() {
-
-    return inMin_;
-
-}
-
-float PID::getInMax() {
-
-    return inMax_;
-
-}
-
-float PID::getOutMin() {
-
-    return outMin_;
-
-}
-
-float PID::getOutMax() {
-
-    return outMax_;
-
-}
-
-float PID::getInterval() {
-
-    return tSample_;
-
-}
-
-float PID::getPParam() {
-
-    return pParam_;
-
-}
-
-float PID::getIParam() {
-
-    return iParam_;
-
-}
-
-float PID::getDParam() {
-
-    return dParam_;
-
-}

--- a/PID/PID.h	Sat Mar 09 14:27:48 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,213 +0,0 @@
-/**
- * @author Aaron Berk
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- * 
- * A PID controller is a widely used feedback controller commonly found in
- * industry.
- *
- * This library is a port of Brett Beauregard's Arduino PID library:
- *
- *  http://www.arduino.cc/playground/Code/PIDLibrary
- *
- * The wikipedia article on PID controllers is a good place to start on
- * understanding how they work:
- *
- *  http://en.wikipedia.org/wiki/PID_controller
- *
- * For a clear and elegant explanation of how to implement and tune a
- * controller, the controlguru website by Douglas J. Cooper (who also happened
- * to be Brett's controls professor) is an excellent reference:
- *
- *  http://www.controlguru.com/
- */
-
-#ifndef PID_H
-#define PID_H
-
-/**
- * Includes
- */
-#include "mbed.h"
-
-/**
- * Defines
- */
-#define MANUAL_MODE 0
-#define AUTO_MODE   1
-
-/**
- * Proportional-integral-derivative controller.
- */
-class PID {
-
-public:
-
-    /**
-     * Constructor.
-     *
-     * Sets default limits [0-3.3V], calculates tuning parameters, and sets
-     * manual mode with no bias.
-     *
-     * @param Kc - Tuning parameter
-     * @param tauI - Tuning parameter
-     * @param tauD - Tuning parameter
-     * @param interval PID calculation performed every interval seconds.
-     */
-    PID(float Kc, float tauI, float tauD, float interval);
-
-    /**
-     * Scale from inputs to 0-100%.
-     *
-     * @param InMin The real world value corresponding to 0%.
-     * @param InMax The real world value corresponding to 100%.
-     */
-    void setInputLimits(float inMin , float inMax);
-
-    /**
-     * Scale from outputs to 0-100%.
-     *
-     * @param outMin The real world value corresponding to 0%.
-     * @param outMax The real world value corresponding to 100%.
-     */
-    void setOutputLimits(float outMin, float outMax);
-
-    /**
-     * Calculate PID constants.
-     *
-     * Allows parameters to be changed on the fly without ruining calculations.
-     *
-     * @param Kc - Tuning parameter
-     * @param tauI - Tuning parameter
-     * @param tauD - Tuning parameter
-     */
-    void setTunings(float Kc, float tauI, float tauD);
-
-    /**
-     * Reinitializes controller internals. Automatically
-     * called on a manual to auto transition.
-     */
-    void reset(void);
-    
-    /**
-     * Set PID to manual or auto mode.
-     *
-     * @param mode        0 -> Manual
-     *             Non-zero -> Auto
-     */
-    void setMode(int mode);
-    
-    /**
-     * Set how fast the PID loop is run.
-     *
-     * @param interval PID calculation peformed every interval seconds.
-     */
-    void setInterval(float interval);
-    
-    /**
-     * Set the set point.
-     *
-     * @param sp The set point as a real world value.
-     */
-    void setSetPoint(float sp);
-    
-    /**
-     * Set the process value.
-     *
-     * @param pv The process value as a real world value.
-     */
-    void setProcessValue(float pv);
-    
-    /**
-     * Set the bias.
-     *
-     * @param bias The bias for the controller output.
-     */
-    void setBias(float bias);
-
-    /**
-     * PID calculation.
-     *
-     * @return The controller output as a float between outMin and outMax.
-     */
-    float compute(void);
-
-    //Getters.
-    float getInMin();
-    float getInMax();
-    float getOutMin();
-    float getOutMax();
-    float getInterval();
-    float getPParam();
-    float getIParam();
-    float getDParam();
-
-private:
-
-    bool usingFeedForward;
-    bool inAuto;
-
-    //Actual tuning parameters used in PID calculation.
-    float Kc_;
-    float tauR_;
-    float tauD_;
-    
-    //Raw tuning parameters.
-    float pParam_;
-    float iParam_;
-    float dParam_;
-    
-    //The point we want to reach.
-    float setPoint_;         
-    //The thing we measure.
-    float processVariable_;  
-    float prevProcessVariable_;
-    //The output that affects the process variable.
-    float controllerOutput_; 
-    float prevControllerOutput_;
-
-    //We work in % for calculations so these will scale from
-    //real world values to 0-100% and back again.
-    float inMin_;
-    float inMax_;
-    float inSpan_;
-    float outMin_;
-    float outMax_;
-    float outSpan_;
-
-    //The accumulated error, i.e. integral.
-    float accError_;
-    //The controller output bias.
-    float bias_;
-
-    //The interval between samples.
-    float tSample_;          
-
-    //Controller output as a real world value.
-    volatile float realOutput_;
-
-};
-
-#endif /* PID_H */

--- a/QEI/Encoder.h	Sat Mar 09 14:27:48 2019 +0000
+++ b/QEI/Encoder.h	Sat Mar 23 19:46:09 2019 +0000
@@ -28,4 +28,4 @@
      dT.start();   
     }
 
-};
\ No newline at end of file
+};

--- 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

--- a/SensorControl/lineSensor.h	Sat Mar 09 14:27:48 2019 +0000
+++ b/SensorControl/lineSensor.h	Sat Mar 23 19:46:09 2019 +0000
@@ -1,6 +1,5 @@
 class lineSensor
 {
-    
     private:
     
     DigitalOut emitter;
@@ -45,12 +44,12 @@
     {
         turnOn();
         sampler.attach(callback(this, &lineSensor::turnOff),sampleTime);
-        calcLineVoltage();
     }
     
-    void calcLineVoltage(void)
+    float calcLineVoltage(void)
     {
-        lineVoltage = receiver.read() * vREF;
+        lineVoltage = receiver.read()*vREF;
+        return lineVoltage;
     }
     
     float returnLineVoltage(void)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WheelControl/LWheel.h	Sat Mar 23 19:46:09 2019 +0000
@@ -0,0 +1,119 @@
+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/P.h	Sat Mar 09 14:27:48 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,61 +0,0 @@
-//CURRENTLY EMPTY BUT I MAY JUST MAKE A NEW CLASS FOR THE P CONTROL
-class P
-{
-    private:
-    float inMin;
-    float inMax;
-    float control;
-    
-    float outMin;
-    float outMax;
-    
-    float gain;
-    
-    
-    public:
-    P(float G)
-    {
-        gain = G;
-        inMin = 0.0f;
-        inMax = 3.3f;
-        outMin = 0.0f;
-        outMax = 1.0f;
-        control = 0;
-    }
-    
-    void setInputLimits(float inMin_, float inMax_)
-    {
-        if (inMin_ > inMax_) {return;} //cant be true can it
-        inMin = inMin_;
-        inMax = inMax_;
-    }
-    
-    void setOutputLimits(float outMin_, float outMax_)
-    {
-        if (outMin_ > outMax_) {return;}
-        outMin = outMin_;
-        outMax = outMax_;
-    }
-    
-    float compute(float curVal_)
-    {
-        float temp = ((control-curVal_)*gain)+curVal_; //amplify difference by gain and add to current value
-        temp = (((temp-inMin)/(inMax - inMin))*(outMax-outMin))+outMin; //scales temp to the correct output Limits
-        if (temp > 1.0f ) {temp = 1.0f;}
-        if (temp < -1.0f) {temp = -1.0f;}
-        return temp; //return the scaled value
-    }
-    
-    void setControl(float wantedVal_)
-    {
-        if (wantedVal_ > inMax) {control = inMax;}
-        else if (wantedVal_ <inMin) {control = inMin;}
-        else {control = wantedVal_;}
-    }
-    
-    float returnControl(void)
-    {
-        return control;
-    }
-    
-};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WheelControl/RWheel.h	Sat Mar 23 19:46:09 2019 +0000
@@ -0,0 +1,109 @@
+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

--- a/WheelControl/Wheel.h	Sat Mar 09 14:27:48 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,107 +0,0 @@
-class Wheel 
-{
-    private:
-    
-    float distance; //distance traversed by wheel
-    float angularVelocity;
-    
-        float const static gain = 1.2f; //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;
-    
-    P controller;
-    
-    public:
-    
-    float  maxAngularVel;
-    
-    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)
-        {
-        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;
-      direction = dir;
-      updater.detach();
-      updater.attach(callback(this, &Wheel::init2),2.0f); //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
-    {
-        calculateAngularVelocity();
-        maxAngularVel = abs(angularVelocity);
-        controller.setInputLimits(-1.0f*abs(angularVelocity),abs(angularVelocity));
-        controller.setControl(0.0f);
-        updater.detach();
-        updater.attach(callback(this, &Wheel::wheelUpdates),0.2f);  //attached the actual update function from now ON    
-    }
-    
-    void wheelUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytiem this function is called
-        {
-            calculateAngularVelocity();
-            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
-            temp2 = abs(temp2);
-            Mtr.write(1.0f - 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.setControl(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

--- a/main.cpp	Sat Mar 09 14:27:48 2019 +0000
+++ b/main.cpp	Sat Mar 23 19:46:09 2019 +0000
@@ -2,11 +2,12 @@
 
 #include "math.h"
 #include "mbed.h"
-#include "PID.h"
 #include "Encoder.h"
 #include "lineSensor.h"
 #include "P.h"
-#include "Wheel.h"
+#include "PID.h"
+#include "LWheel.h"
+#include "RWheel.h"
 #include "Robot.h"
 
 
@@ -22,34 +23,34 @@
     
     Encoder* RE = new Encoder(PB_3,PB_5);
     Encoder* LE = new Encoder(PB_10,PB_4);
-    Wheel* rightWheel = new Wheel(RE,PC_8,PA_9, PA_14);
-    Wheel* leftWheel = new Wheel(LE,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
+    LWheel* leftWheel = new LWheel(LE,PC_8,PA_9, PA_14);
+    RWheel* rightWheel = new RWheel(RE,PC_6,PA_8, PB_6);
     
+    //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);
     
-    pc.printf("Stage 1 speed %f\r\n", rightWheel->returnAngularVelocity());
-    
-    rightWheel->init(0);
+    while(leftWheel->returnAngularVelocity()!= 0)
+    {
+     pc.printf("Stage 1, speed %f\r\n",leftWheel->returnAngularVelocity());
+    }
+
+    rightWheel->init(1);
     while(rightWheel->returnAngularVelocity() != 0)
     {
      pc.printf("Stage 2, speed %f\r\n",rightWheel->returnAngularVelocity()); 
     }
-    leftWheel->init(0);
-    while(leftWheel->returnAngularVelocity() != 0)
-    {
-     pc.printf("Stage 2, speed %f\r\n",leftWheel->returnAngularVelocity()); 
-    }
-
+    pc.printf("L , speed%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);
     while(1)
     {
-        i++;
-        if (i >= 10)
-        {
-        i = 0;
-        pc.printf("speed %f\r\n", rightWheel->returnAngularVelocity()); 
-        pc.printf("speed %f\r\n", leftWheel->returnAngularVelocity());
-        }
+    pc.printf("L , speed%f\r\n",leftWheel->returnAngularVelocity());
+    pc.printf("R , speedM %f\r\n",rightWheel->returnAngularVelocity());
     }
+    
 }
-
-/* ineSensor* 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
+//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