Revision 0:8bded3c460c7, committed 2017-04-18

Comitter:

taoqiuyang

Date:

Tue Apr 18 23:58:28 2017 +0000

Commit message:

PID_Based_Motor_Controller_Code

Changed in this revision

diff -r 000000000000 -r 8bded3c460c7 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue Apr 18 23:58:28 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 000000000000 -r 8bded3c460c7 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Apr 18 23:58:28 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 000000000000 -r 8bded3c460c7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 18 23:58:28 2017 +0000
@@ -0,0 +1,141 @@
+/*
+Simple PID-based Motor RPM Controller
+Purpose:    This code reads motor RPM with encoder, 
+            generate ESC-compatible control signal with PID controller
+            
+Note:   LED1: System Status Indicator, Blink = Main Loop is Running
+        LED2: Controller Timer Indicator, Blink = Controller is Updating        
+        ESC is connected to Pin 26
+        Encoder is connected to Pin29,Pin30, and Pin27 if has index channel
+        PID gains:  Kp, Ki, Kd
+
+Caution: This is a simple test code, no guarantee to performance or robustness 
+      
+Author: Qiuyang Tao
+*/
+ 
+#include "mbed.h"
+#include "Servo.h"
+#include "QEI.h"
+#define PULSE_PER_REV 200
+  
+Ticker ctrl_updt_timer;     //Timer
+Servo motor_ESC(p26);       //ESC is connected to Pin 26
+Serial pc(USBTX, USBRX);    //debug serial
+QEI wheel (p29, p30, NC, PULSE_PER_REV);  // <500,000 pulses per second//Use X4 encoding.
+//QEI wheel (p29, p30, p27, PULSE_PER_REV); // If encoder has index channel 
+
+//All LEDs Blinks at start up, indicates mbed is setting up ESC
+DigitalOut led1(LED1); //System Status Indicator, Blink = Main Loop is Running
+DigitalOut led2(LED2); //Controller Status Indicator, Blink = Controller is Updating  
+DigitalOut led3(LED3); //Controller Status Indicator, On = Motor RPM < Desired RPM
+//                                                         Motor is Aaccelerating
+//                                                   Off = Motor RPM > Desired RPM
+//                                                         Motor is De-Aaccelerating
+DigitalOut led4(LED4); //Encoder Counter Overflow
+
+
+//PID Related Variables - User 
+float setpoint_rpm=10000;   //Desired RPM = 10,000
+float dt=0.02; //Update Controller at 50Hz
+float Kp=0;
+float Ki=0;
+float Kd=0;
+
+
+//PID Related Variables 
+float error_rpm=0;
+float prev_error_rpm=0;
+float u=0;  //controller output, range = [0,1]
+float u_p=0;
+float u_i=0;
+float u_d=0;
+
+//Global Variables - User
+int use_index_channel=0;
+
+//use Unsigned long long if overflows frequently
+//unsigned long long motor_revolutions = 0;
+int motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
+int prev_motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
+int motor_pulses=0;      //Number of pulses recorded by the encoder
+int prev_motor_pulses=0;      //Number of pulses recorded by the encoder
+float motor_RPM=0;         //Motor angular velocity
+
+
+void controller_update_ISR() {
+    // get motor RPM
+    if (use_index_channel) {
+        motor_revolutions=wheel.getRevolutions();
+        motor_RPM=(motor_revolutions-prev_motor_revolutions)/dt;
+        prev_motor_revolutions=motor_revolutions;
+        //reset encoder counter if overflows
+        if(motor_revolutions>2147483647-setpoint_rpm*dt*10){
+                           //18446744073709551615; for Unsigned long long
+            wheel.reset();    
+            prev_motor_revolutions=0;
+            led4 = !led4;
+        }
+        
+    }else{
+        motor_pulses=wheel.getPulses();     
+        motor_RPM=(motor_pulses-prev_motor_pulses)/PULSE_PER_REV/dt;
+        prev_motor_pulses=motor_pulses;
+        //reset encoder counter if overflows
+        if(motor_pulses>2147483647-setpoint_rpm*dt*PULSE_PER_REV*10){
+                           //18446744073709551615; for Unsigned long long
+            wheel.reset();    
+            prev_motor_pulses=0;
+            led4 = !led4;
+        }
+    }
+    
+    //PID controller
+    error_rpm=setpoint_rpm-motor_RPM;   //e=r-u;
+    if (error_rpm>0){led3=1;} else {led3=0;} //Update Motor Status
+    u_p=Kp*error_rpm;  
+    u_i=u_i+Ki*dt*error_rpm;
+    u_d=Kd*(error_rpm-prev_error_rpm)/dt;
+    u= u_p + u_i + u_d;
+    prev_error_rpm=error_rpm;
+    
+    //Controller Saturation
+    if (u>1){u=1;}
+    if (u<0){u=0;}
+    
+    //Output ESC signal
+    motor_ESC = u;
+    
+    //Update LED2 status, tell user PID is updating
+    led2 = !led2;   
+    
+    //Watch dog can be added here in the future
+}
+ 
+int main() {
+    //pc.printf("Debug mode: \n");
+    led1 = 0;   //init LED1, Main Loop status
+    led2 = 0;   //init LED2, PID status
+    led3 = 0;   //init LED3, Motor status
+    led4 = 0;   //init LED4, Encoder Counter status
+    wait(0.5);
+    
+    //All LEDs Blinks at start up, indicates mbed is setting up ESC
+    motor_ESC = 0; //Generate zero speed signal for ESC startup
+    led1 = 1;led2 = 1;led3 = 1;led1 = 1;
+    wait(0.5);
+    led1 = 0;led2 = 0;led3 = 0;led1 = 0;
+    wait(0.5);
+    led1 = 1;led2 = 1;led3 = 1;led1 = 1;
+    wait(0.5);
+    led1 = 0;led2 = 0;led3 = 0;led1 = 0;
+    wait(0.5);
+    
+    
+    ctrl_updt_timer.attach(&controller_update_ISR, dt); //Update Controller at (1/dt) Hz
+    
+    while(1) {  //blink LED1
+        led1 = !led1;
+        wait(0.2);
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 8bded3c460c7 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 18 23:58:28 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/97feb9bacc10
\ No newline at end of file