Revision 0:f414c64e674f, committed 2014-03-04

Comitter:

salatron

Date:

Tue Mar 04 13:31:17 2014 +0000

Child:

1:8a2a7adb3c5d

Commit message:

Version1
; RenBuggy with PID controll

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PinDetect.lib	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RenBuggy_PID.cpp	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,293 @@
+/*******************************************************************************
+* RenBED PID Motor Control for RenBuggy                                        *
+* Copyright (c) 2014 Sally Brown                                               *
+*                                                                              *
+* 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.                                                                *
+*                                                                              *
+* PID_Controller.cpp                                                           *
+*                                                                              *
+*******************************************************************************/
+
+#ifndef _PIDCONTROLLER_C
+#define _PIDCONTROLLER_C
+
+#include "RenBuggy_PID.h"
+
+PID_Controller::PID_Controller
+(
+    PinName motorL,
+    PinName motorR,
+    PinName brakeL,
+    PinName brakeR,
+    PinName sensorL,
+    PinName sensorR
+) :
+    m_controllerL(1.0, 0.0, 0.0, RATE),  //Kc, Ti, Td, interval
+    m_controllerR(1.0, 0.0, 0.0, RATE),
+    m_motorL(motorL),
+    m_motorR(motorR),
+    m_brakeL(brakeL),
+    m_brakeR(brakeR),
+    m_senseL(sensorL),
+    m_senseR(sensorR),
+    m_numberStrips(16),                           //Default to 16 stripes
+    m_wheelCircumference(16.96),                  //Default to 16.96
+    m_stripesL(0),                                //Initialise the number of stripes to 0.
+    m_stripesR(0),
+    m_turnLeft(false),
+    m_turnRight(false),    
+    m_fProportionLeft(0.0),
+    m_fProportionRight(0.0),        
+    m_iProportionLeft(0),
+    m_iProportionRight(0)
+{  
+    m_senseL.setSampleFrequency(1000);
+    m_senseR.setSampleFrequency(1000); //If this is playing up, consider changing this to 1001?
+    
+    //It's 5 samples before it recognises it's held on.
+    m_senseL.setSamplesTillHeld(5);
+    m_senseR.setSamplesTillHeld(5);
+    
+    //Only when it's been held high and then goes low will it increment the number of counts.
+    m_senseL.attach_deasserted_held(this, &PID_Controller::countL);
+    m_senseR.attach_deasserted_held(this, &PID_Controller::countR);
+    
+    setUpControllers();
+}
+
+void PID_Controller::SetUpConstants(int numberStripes, float wheelCircumference)
+{
+    m_numberStrips = numberStripes;
+    m_wheelCircumference = wheelCircumference;
+}
+
+void PID_Controller::Forwards(int CountsForward)
+{
+    m_turnRight = false;
+    m_turnLeft = false;
+    
+    CountsForward = CountsForward * (m_numberStrips/m_wheelCircumference);
+    
+    m_rate.attach(this, &PID_Controller::doSomePID, RATE); //Attach the counter if it hasn't gone too far. Then hopefully just sit in a loop.
+    
+    m_stripesL = m_stripesR = 0;
+    m_brakeR = m_brakeL = 0;
+    
+    m_fProportionLeft = m_fProportionRight = 1.0;
+    
+    while ((m_brakeR == 0) || (m_brakeL == 0)) 
+    {
+        if (CountsForward < m_stripesL)
+        {
+            m_motorL = 0.0;
+            m_brakeL = 1;
+        }
+        if (CountsForward < m_stripesR)
+        {
+            m_motorR = 0.0;
+            m_brakeR = 1;
+            if (CountsForward < m_stripesL)
+            {
+                m_rate.detach();
+            }
+        }
+    }
+    return;
+}
+
+void PID_Controller::Left(int AngleLeft, int RadiusLeft)
+{
+    m_turnRight = false; //Turning left, NOT turning right
+    m_turnLeft = true;
+    
+    m_rate.attach(this, &PID_Controller::doSomePID, RATE);
+    
+    m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
+    m_stripesL = m_stripesR = 0;
+    
+    float m_fDistanceL = (2*pi*(RadiusLeft - 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleLeft); 
+    float m_fDistanceR = (2*pi*(RadiusLeft + 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleLeft); //gives the length of the arc over which the wheel will travel, and translates that into a number of wheel stripes
+    
+    int iDistanceL = (int) m_fDistanceL; //Cast the distance into an int
+    int iDistanceR = (int) m_fDistanceR;
+    
+    int LeftWheelDist = iDistanceL; //Set the distance the left wheel travels
+    int RightWheelDist = iDistanceR; //Set the distance the right wheel travels
+    
+    float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
+    m_iProportionLeft = floor(myfloatL); 
+    m_fProportionLeft = myfloatL;
+    
+    float myfloatR = (float)RightWheelDist/LeftWheelDist; //When turning right, you only use the left wheel's proportion
+    m_iProportionRight = ceil(myfloatR);
+    m_fProportionRight = myfloatR;
+
+    while ((m_brakeR == 0))
+    {
+        if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
+        {
+            m_motorL = 0.0;   //Stop the motor
+            m_brakeL = 1;     //Apply the brakes
+        }
+        if (RightWheelDist <= m_stripesR)
+        {
+            m_motorR = 0.0;
+            m_brakeR = 1;
+            if (LeftWheelDist <= m_stripesL)
+            {
+                m_rate.detach();
+            }
+        }
+    }
+} 
+
+void PID_Controller::Right(int AngleRight, int RadiusRight)
+{
+    m_turnRight = true;
+    m_turnLeft = false;
+    
+    m_rate.attach(this, &PID_Controller::doSomePID, RATE);
+    
+    m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.    
+    m_stripesL = m_stripesR = 0;
+    
+    float m_fDistanceL = (2*pi*(RadiusRight + 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleRight); //Forcing it to an int beforehand didn't work. It twitches instead of going argh no, but it still doesn't really work.
+    float m_fDistanceR = (2*pi*(RadiusRight - 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleRight);
+    
+    int iDistanceL = (int) m_fDistanceL; 
+    int iDistanceR = (int) m_fDistanceR;
+    
+    int LeftWheelDist = iDistanceL;  //Formula for the length of an arc, divided by circumference, multiplied by 16 stripes. 
+    int RightWheelDist = iDistanceR; //This give a distance, after which it will stop moving. These lines are causing problems... 
+    
+    float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
+    m_iProportionLeft = (int) ceil(myfloatL);
+    m_fProportionLeft = myfloatL;
+    
+    float myfloatR = (float)RightWheelDist/(float)LeftWheelDist;
+    m_iProportionRight = (int) floor(myfloatR);
+
+    while ((m_brakeL == 0))
+    {
+        if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
+        {
+            m_motorL = 0.0;   //Stop the motor
+            m_brakeL = 1;     //Apply the brakes
+        }
+        if (RightWheelDist <= m_stripesR)
+        {
+            m_motorR = 0.0;
+            m_brakeR = 1;
+            if (LeftWheelDist <= m_stripesL)
+            {
+                m_rate.detach();
+            }
+        }
+    }
+} 
+
+void PID_Controller::doSomePID()
+{
+    PIDLeft();
+    PIDRight();
+}
+
+void PID_Controller::PIDLeft()
+{
+    float fSPL = 0.0;
+    int SPL = 0;
+    
+    if(m_turnLeft)
+    {
+        fSPL = m_stripesR / m_fProportionRight;
+        SPL = (int) fSPL;
+    }
+    else if(m_turnRight)
+    {
+        fSPL = m_stripesR * m_fProportionLeft;
+        SPL = (int) fSPL;
+    }
+    else
+    {
+        SPL = m_stripesR;
+    }
+
+    m_controllerL.setProcessValue(m_stripesL); 
+    
+    m_motorL = (m_controllerL.compute()); //PWM output * some speed proportion. May slow it down or speed it up.*/
+    
+    m_controllerL.setSetPoint(SPL);
+}
+
+void PID_Controller::PIDRight()
+{
+    float fSPR = 0.0;
+    int SPR = 0;
+    
+    if(m_turnRight) //If you're turning right, the left wheel goes further, so use xProportionLeft
+    {
+        fSPR = m_stripesL / m_fProportionLeft;
+        SPR = (int) fSPR;
+    }
+    else if(m_turnLeft) //If you're turning left, the right wheel goes further, so use xProportionRight
+    {
+        fSPR = m_stripesL * m_fProportionRight;
+        SPR = (int) fSPR;
+    }
+    else
+    {
+        SPR = m_stripesL;
+    }
+    
+    m_controllerR.setProcessValue(m_stripesR); //Set the process value (what it IS).
+    
+    m_motorR = (m_controllerR.compute()); //Calculate the PWM duty cycle
+    
+    m_controllerR.setSetPoint(SPR); //SPR = set point right. this sets the set point (what it SHOULD BE).
+}
+
+void PID_Controller::countL() 
+{
+    m_stripesL++;
+}
+
+void PID_Controller::countR() 
+{
+    m_stripesR++;
+}
+
+void PID_Controller::setUpControllers()
+{
+    //m_controllerL = PID(1.0, 0.0, 0.0, RATE);  //Kc, Ti, Td, interval
+    //m_controllerR = PID(1.0, 0.0, 0.0, RATE);
+
+    m_controllerL.setInputLimits(0.0, 200);
+    m_controllerR.setInputLimits(0.0, 200);
+    //Pwm output from 0.0 to 1.0 (PWM duty cycle %)
+    m_controllerL.setOutputLimits(0.0, 1.0);
+    m_controllerR.setOutputLimits(0.0, 1.0);
+    //If there's a bias. FULL SPEED AHEAD. I don't know why this works but it does.
+    m_controllerL.setBias(1.0);
+    m_controllerR.setBias(1.0);
+    //Set it to auto mode.
+    m_controllerL.setMode(AUTO_MODE);
+    m_controllerR.setMode(AUTO_MODE);    
+}
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RenBuggy_PID.h	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,89 @@
+/*******************************************************************************
+* RenBED PID Motor Control for RenBuggy                                        *
+* Copyright (c) 2014 Sally Brown                                               *
+*                                                                              *
+* 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.                                                                *
+*                                                                              *
+* PID_Controller.h                                                             *
+*                                                                              *
+*******************************************************************************/
+
+#ifndef _PIDCONTROLLER_H
+#define _PIDCONTROLLER_H
+
+#include "mbed.h"
+#include "PID.h"
+#include "PinDetect.h"
+
+#define pi      3.1416
+#define RATE    0.001
+
+class PID_Controller
+{
+    public:
+        PID_Controller(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR);
+    
+        void SetUpConstants(int numberStripes, float wheelCircumference);
+        
+        void Forwards(int countsForward);
+        void Left(int angleLeft, int radiusLeft);
+        void Right(int angleRight, int radiusRight);
+        
+    private:
+    
+        void doSomePID();
+        void PIDLeft();
+        void PIDRight();
+        
+        void countL();
+        void countR();
+        
+        void setUpControllers();
+        
+        PID     m_controllerL;  //Kc, Ti, Td, interval
+        PID     m_controllerR;
+        
+        PwmOut  m_motorL;
+        PwmOut  m_motorR;
+        
+        DigitalOut m_brakeL;
+        DigitalOut m_brakeR;
+        
+        PinDetect   m_senseL;     //Left encoder. Pin detect type is the debouncing.
+        PinDetect   m_senseR; 
+
+        Ticker  m_rate;
+        
+        int     m_numberStrips;
+        float   m_wheelCircumference;
+
+        int     m_stripesL;
+        int     m_stripesR;
+        
+        bool    m_turnLeft;
+        bool    m_turnRight;
+        
+        float   m_fProportionLeft;
+        float   m_fProportionRight;
+        
+        int     m_iProportionLeft;
+        int     m_iProportionRight;
+};
+
+#endif    // _PIDCONTROLLER_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,12 @@
+
+#include "RenBuggy_PID.h"
+
+int main()
+{
+    PID_Controller pid(p25, p10, p8, p7, p9, p21);
+    pid.Forwards(20);
+    pid.Left(90, 45);
+    pid.Forwards(20);
+    pid.Right(90,45);
+    pid.Forwards(20);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Mar 04 13:31:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8e73be2a2ac1
\ No newline at end of file