Sally Brown
Control for RenBuggy
Dependencies: PID PinDetect mbed
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