Revision 1:8a2a7adb3c5d, committed 2014-03-05

Comitter:

salatron

Date:

Wed Mar 05 13:37:10 2014 +0000

Parent:

0:f414c64e674f

Child:

2:cd7543fdcb8c

Commit message:

Version two of RenBuggy with PID control
; Can use with two different configurations of buggys

Changed in this revision

--- a/RenBuggy_PID.cpp	Tue Mar 04 13:31:17 2014 +0000
+++ b/RenBuggy_PID.cpp	Wed Mar 05 13:37:10 2014 +0000
@@ -1,6 +1,6 @@
 /*******************************************************************************
 * RenBED PID Motor Control for RenBuggy                                        *
-* Copyright (c) 2014 Sally Brown                                               *
+* Copyright (c) 2014 Sally Brown & Liz Lloyd                                   *
 *                                                                              *
 * Permission is hereby granted, free of charge, to any person obtaining a copy *
 * of this software and associated documentation files (the "Software"), to deal*
@@ -29,14 +29,40 @@
 
 #include "RenBuggy_PID.h"
 
+PID_Stripes::PID_Stripes(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
+   PID_Controller(motorL, motorR, brakeL, brakeR),
+   m_senseL(sensorL),
+   m_senseR(sensorR)
+{
+    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);
+    
+    PID_Controller* basePointer = dynamic_cast<PID_Stripes*>(this);
+    //Only when it's been held high and then goes low will it increment the number of counts.
+    m_senseL.attach_deasserted_held(basePointer, &PID_Controller::countL);
+    m_senseR.attach_deasserted_held(basePointer, &PID_Controller::countR);
+}
+
+PID_Magnet::PID_Magnet(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
+   PID_Controller(motorL, motorR, brakeL, brakeR),
+   m_senseL(sensorL),
+   m_senseR(sensorR)
+{
+    PID_Controller* basePointer = dynamic_cast<PID_Magnet*>(this);
+    m_senseL.fall(basePointer, &PID_Controller::countL);
+    m_senseR.fall(basePointer, &PID_Controller::countR);
+}
+
 PID_Controller::PID_Controller
 (
     PinName motorL,
     PinName motorR,
     PinName brakeL,
-    PinName brakeR,
-    PinName sensorL,
-    PinName sensorR
+    PinName brakeR
 ) :
     m_controllerL(1.0, 0.0, 0.0, RATE),  //Kc, Ti, Td, interval
     m_controllerR(1.0, 0.0, 0.0, RATE),
@@ -44,45 +70,33 @@
     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_countsPerRev(16),                             //Default to 16 stripes
+    m_wheelCircumference(16.96),                    //Default to 16.96
+    m_axleLength(13),
+    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);
-    
+    m_fProportionRight(0.0)
+{
     setUpControllers();
 }
 
-void PID_Controller::SetUpConstants(int numberStripes, float wheelCircumference)
+void PID_Controller::SetUpConstants(int countsPerRev, float wheelCircumference, float axleLength)
 {
-    m_numberStrips = numberStripes;
+    m_countsPerRev = countsPerRev;
     m_wheelCircumference = wheelCircumference;
+    m_axleLength = axleLength;
 }
 
-void PID_Controller::Forwards(int CountsForward)
+void PID_Controller::Forwards(int distanceForward)
 {
     m_turnRight = false;
     m_turnLeft = false;
+    bool moving = true;
     
-    CountsForward = CountsForward * (m_numberStrips/m_wheelCircumference);
+    int CountsForward = distanceForward * (m_countsPerRev/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.
     
@@ -91,7 +105,7 @@
     
     m_fProportionLeft = m_fProportionRight = 1.0;
     
-    while ((m_brakeR == 0) || (m_brakeL == 0)) 
+    while(moving) 
     {
         if (CountsForward < m_stripesL)
         {
@@ -102,12 +116,17 @@
         {
             m_motorR = 0.0;
             m_brakeR = 1;
-            if (CountsForward < m_stripesL)
-            {
-                m_rate.detach();
-            }
+        }
+        if(CountsForward < m_stripesR &&
+           CountsForward < m_stripesL)
+        {
+            m_rate.detach();
+            moving = false;
         }
     }
+    
+    Stop();
+    
     return;
 }
 
@@ -115,30 +134,26 @@
 {
     m_turnRight = false; //Turning left, NOT turning right
     m_turnLeft = true;
+    bool turning = 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 halfAxleLength = (m_axleLength + 1)/2;
+    RadiusLeft = RadiusLeft < halfAxleLength ? halfAxleLength : RadiusLeft;
     
-    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 m_fDistanceL = (2*pi*(RadiusLeft - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleLeft); 
+    float m_fDistanceR = (2*pi*(RadiusLeft + halfAxleLength))*(m_countsPerRev/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
     
-    float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
-    m_iProportionLeft = floor(myfloatL); 
-    m_fProportionLeft = myfloatL;
+    int LeftWheelDist = (int) m_fDistanceL; //Cast the distance into an int
+    int RightWheelDist = (int) m_fDistanceR;
     
-    float myfloatR = (float)RightWheelDist/LeftWheelDist; //When turning right, you only use the left wheel's proportion
-    m_iProportionRight = ceil(myfloatR);
-    m_fProportionRight = myfloatR;
+    m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;    
+    m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist; //When turning right, you only use the left wheel's proportion
 
-    while ((m_brakeR == 0))
+    while (turning)
     {
         if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
         {
@@ -149,41 +164,43 @@
         {
             m_motorR = 0.0;
             m_brakeR = 1;
-            if (LeftWheelDist <= m_stripesL)
-            {
-                m_rate.detach();
-            }
+        }
+        if(RightWheelDist <= m_stripesR &&
+           LeftWheelDist <= m_stripesL)
+        {
+            m_rate.detach();
+            turning = false;
+            break;
         }
     }
+
+    Stop();
 } 
 
 void PID_Controller::Right(int AngleRight, int RadiusRight)
 {
     m_turnRight = true;
     m_turnLeft = false;
+    bool turning = 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*(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 halfAxleLength = (m_axleLength + 1)/2;
+    RadiusRight = RadiusRight < halfAxleLength ? halfAxleLength : RadiusRight;
     
-    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 m_fDistanceL = (2*pi*(RadiusRight + halfAxleLength))*(m_countsPerRev/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 - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleRight);
     
-    float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
-    m_iProportionLeft = (int) ceil(myfloatL);
-    m_fProportionLeft = myfloatL;
+    int LeftWheelDist = (int) m_fDistanceL; 
+    int RightWheelDist = (int) m_fDistanceR;
     
-    float myfloatR = (float)RightWheelDist/(float)LeftWheelDist;
-    m_iProportionRight = (int) floor(myfloatR);
+    m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;
+    m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist;
 
-    while ((m_brakeL == 0))
+    while (turning)
     {
         if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
         {
@@ -194,14 +211,31 @@
         {
             m_motorR = 0.0;
             m_brakeR = 1;
-            if (LeftWheelDist <= m_stripesL)
-            {
-                m_rate.detach();
-            }
+        }
+        if(RightWheelDist <= m_stripesR &&
+           LeftWheelDist <= m_stripesL)
+        {
+            m_rate.detach();
+            turning = false;
+            break;
         }
     }
+
+    Stop();
 } 
 
+void PID_Controller::Stop()
+{
+    m_stripesL = 0;
+    m_stripesR = 0;
+    m_motorL = 0.0;
+    m_motorR = 0.0;
+    m_brakeL = 1;
+    m_brakeR = 1;
+    
+    m_rate.detach();
+}
+
 void PID_Controller::doSomePID()
 {
     PIDLeft();
@@ -274,9 +308,6 @@
 
 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 %)
@@ -287,7 +318,13 @@
     m_controllerR.setBias(1.0);
     //Set it to auto mode.
     m_controllerL.setMode(AUTO_MODE);
-    m_controllerR.setMode(AUTO_MODE);    
+    m_controllerR.setMode(AUTO_MODE);  
 }
 
-#endif
\ No newline at end of file
+void PID_Controller::ConfigurePID(float p, float i, float d)
+{
+    m_controllerL.setTunings(p, i, d);
+    m_controllerR.setTunings(p, i, d);
+}
+
+#endif

--- a/RenBuggy_PID.h	Tue Mar 04 13:31:17 2014 +0000
+++ b/RenBuggy_PID.h	Wed Mar 05 13:37:10 2014 +0000
@@ -1,6 +1,6 @@
 /*******************************************************************************
 * RenBED PID Motor Control for RenBuggy                                        *
-* Copyright (c) 2014 Sally Brown                                               *
+* Copyright (c) 2014 Sally Brown & Liz Lloyd                                   *
 *                                                                              *
 * Permission is hereby granted, free of charge, to any person obtaining a copy *
 * of this software and associated documentation files (the "Software"), to deal*
@@ -36,42 +36,47 @@
 
 class PID_Controller
 {
-    public:
-        PID_Controller(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR);
-    
-        void SetUpConstants(int numberStripes, float wheelCircumference);
+    public:    
+        void SetUpConstants(int countsPerRev, float wheelCircumference, float axleLength);
         
-        void Forwards(int countsForward);
+        void Forwards(int distanceForward);
         void Left(int angleLeft, int radiusLeft);
         void Right(int angleRight, int radiusRight);
         
+        void Stop();
+        
+        void ConfigurePID(float p, float i, float d);
+        
+        void countL();
+        void countR();
+        
+    protected:
+    
+        PID_Controller(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR);
+        virtual void setUpControllers();
+        PID getLeftController() {return m_controllerL;}
+        PID getRightController() {return m_controllerR;}
+                       
     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;
+        int     m_countsPerRev;
         float   m_wheelCircumference;
+        float   m_axleLength;
 
         int     m_stripesL;
         int     m_stripesR;
@@ -81,9 +86,26 @@
         
         float   m_fProportionLeft;
         float   m_fProportionRight;
-        
-        int     m_iProportionLeft;
-        int     m_iProportionRight;
+};
+
+class PID_Stripes : public PID_Controller
+{
+    public:
+        PID_Stripes(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR);
+    
+    private:        
+        PinDetect   m_senseL;     //Left encoder. Pin detect type is the debouncing.
+        PinDetect   m_senseR; 
+};
+
+class PID_Magnet : public PID_Controller
+{
+    public:
+        PID_Magnet(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR);
+    
+    private:        
+        InterruptIn   m_senseL;     //Left encoder. 
+        InterruptIn   m_senseR; 
 };
 
 #endif    // _PIDCONTROLLER_H
\ No newline at end of file

--- a/main.cpp	Tue Mar 04 13:31:17 2014 +0000
+++ b/main.cpp	Wed Mar 05 13:37:10 2014 +0000
@@ -3,10 +3,24 @@
 
 int main()
 {
-    PID_Controller pid(p25, p10, p8, p7, p9, p21);
-    pid.Forwards(20);
-    pid.Left(90, 45);
+    //the code defaults to the striped wheel buggy configuration
+    //pins are motor l, motor r, brake l, brake r, sensor l, sensor r
+    //PID_Stripes pid(p25, p10, p8, p7, p9, p21);
+    
+    
+    //No striped wheels - remember to set up the constants
+    //i.e. counts per rev, wheel circumference, axle width
+    //pins are motor l, motor r, brake l, brake r, sensor l, sensor r
+    //PID_Magnet pid(p25, p10, p8, p7, p9, p21);
+    PID_Magnet pid(p5, p6, p7, p8, p21, p22);
+    pid.SetUpConstants(64, 11.31, 6.4);
+    
+    pid.Stop();
+    pid.Forwards(50);
+    pid.Stop();
+    pid.Left(90, 13);
+    pid.Stop();
     pid.Forwards(20);
-    pid.Right(90,45);
-    pid.Forwards(20);
-}
+    pid.Right(90,20);
+    pid.Stop();    
+}
\ No newline at end of file