worasuchad haomachai
DC motor PID position control
Fork of
QEI_HelloWorld
by 
Aaron Berk
Revision 2:916e20f507f2, committed 2018-11-07
- Comitter:
- worasuchad
- Date:
- Wed Nov 07 17:05:37 2018 +0000
- Parent:
- 1:30696e4d196b
- Commit message:
- DC motor PID position control
Changed in this revision
diff -r 30696e4d196b -r 916e20f507f2 QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Nov 07 17:05:37 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 30696e4d196b -r 916e20f507f2 QEI_lib.lib --- a/QEI_lib.lib Wed Aug 11 09:15:10 2010 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/aberk/programs/QEI_lib/latest \ No newline at end of file
diff -r 30696e4d196b -r 916e20f507f2 main.cpp
--- a/main.cpp Wed Aug 11 09:15:10 2010 +0000
+++ b/main.cpp Wed Nov 07 17:05:37 2018 +0000
@@ -1,16 +1,111 @@
-#include "QEI.h"
+
+/***************************< File comment >***************************/
+/* project: Lamborghini48 */
+/* project description : mobile robot class */
+/*--------------------------------------------------------------------*/
+/* version : 0.1 */
+/* board : NUCLEO-F411RE */
+/* detail : DC motor control */
+/*--------------------------------------------------------------------*/
+/* create : 26/10/2018 */
+/* programmer : Worasuchad Haomachai */
+/**********************************************************************/
+
+/*--------------------------------------------------------------------*/
+/* Include file */
+/*--------------------------------------------------------------------*/
+#include "math.h"
+#include "QEI.h" // https://os.mbed.com/cookbook/QEI
Serial pc(USBTX, USBRX);
+
//Use X4 encoding.
//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
//Use X2 encoding by default.
-QEI wheel (p29, p30, NC, 624);
+QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING);
+
+/*--------------------------------------------------------------------*/
+/* Global variable */
+/*--------------------------------------------------------------------*/
+PwmOut pwm(D5); // this is the PWM pin for the motor for how much we move it to correct for its error
+DigitalOut dir1(D6); //these pins are to control the direction of the motor (clockwise/counter-clockwise)
+DigitalOut dir2(D7);
+
+double angle = 0; //set the angles
-int main() {
+double setpoint = 360; // 3072; // I am setting it to move through 100 degrees
+double Kp = 0.05; // 0.015; // you can set these constants however you like depending on trial & error
+double Ki = 0.001; // 0.000071; // 0.000109;
+double Kd = 0.0; // 0.93866; // 0.1009;
+
+float last_error = 0;
+float _error = 0;
+float changeError = 0;
+float totalError = 0;
+float pidTerm = 0;
+float pidTerm_scaled = 0; // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
- while(1){
- wait(0.1);
- pc.printf("Pulses is: %i\n", wheel.getPulses());
+/*--------------------------------------------------------------------*/
+/* prototype fun */
+/*--------------------------------------------------------------------*/
+void PIDcalculation(); // find PID value
+
+/*--------------------------------------------------------------------*/
+/* main */
+/*--------------------------------------------------------------------*/
+int main() {
+ pc.baud(115200);
+ while(1)
+ {
+ PIDcalculation(); // find PID value
+
+ if (angle < setpoint)
+ {
+ dir1 = 1; // Forward motion
+ dir2 = 0;
+ }
+ else
+ {
+ dir1 = 0; // Reverse motion
+ dir2 = 1;
+ }
+
+ pwm.write(pidTerm_scaled / 255.00f);
+
+ // Serial.println("WHEEL ANGLE:");
+ pc.printf("%.3f\t\t", setpoint);
+ pc.printf("%.3f\n\r", angle);
+ //pc.printf("%d\n\r", wheel.getPulses());
}
}
+/*--------------------------------------------------------------------*/
+/* PID */
+/*--------------------------------------------------------------------*/
+void PIDcalculation(){
+ /*
+ * angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4)
+ * reduction ratio: 64:1 , pulses per revolution: 12CPR
+ */
+
+ angle = ( 0.004591 * wheel.getPulses()); // 360/(98*200*4)
+ _error = setpoint - angle;
+
+ changeError = _error - last_error; //derivative term
+ totalError += _error; //accumalate errors to find integral term
+ pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain
+
+ // pidTerm = constrain(pidTerm, -255, 255); //constraining to appropriate value
+ if(pidTerm > 255)
+ {
+ pidTerm = 255;
+ }
+ else if(pidTerm < -255)
+ {
+ pidTerm = -255;
+ }
+
+ pidTerm_scaled = abs(pidTerm); //make sure it's a positive value
+
+ last_error = _error;
+}
diff -r 30696e4d196b -r 916e20f507f2 mbed.bld --- a/mbed.bld Wed Aug 11 09:15:10 2010 +0000 +++ b/mbed.bld Wed Nov 07 17:05:37 2018 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da +https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187 \ No newline at end of file