Qiuyang Tao / Mbed 2 deprecated Precise_Motor_Ctrl_Trial_1

PID Based Motor Controller Code

Dependencies:   QEI Servo mbed

main.cpp@0:8bded3c460c7, 2017-04-18 (annotated)

Committer:
taoqiuyang
Date:
Tue Apr 18 23:58:28 2017 +0000
Revision:
0:8bded3c460c7
PID_Based_Motor_Controller_Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taoqiuyang 0:8bded3c460c7 1 /*
taoqiuyang 0:8bded3c460c7 2 Simple PID-based Motor RPM Controller
taoqiuyang 0:8bded3c460c7 3 Purpose: This code reads motor RPM with encoder,
taoqiuyang 0:8bded3c460c7 4 generate ESC-compatible control signal with PID controller
taoqiuyang 0:8bded3c460c7 5
taoqiuyang 0:8bded3c460c7 6 Note: LED1: System Status Indicator, Blink = Main Loop is Running
taoqiuyang 0:8bded3c460c7 7 LED2: Controller Timer Indicator, Blink = Controller is Updating
taoqiuyang 0:8bded3c460c7 8 ESC is connected to Pin 26
taoqiuyang 0:8bded3c460c7 9 Encoder is connected to Pin29,Pin30, and Pin27 if has index channel
taoqiuyang 0:8bded3c460c7 10 PID gains: Kp, Ki, Kd
taoqiuyang 0:8bded3c460c7 11
taoqiuyang 0:8bded3c460c7 12 Caution: This is a simple test code, no guarantee to performance or robustness
taoqiuyang 0:8bded3c460c7 13
taoqiuyang 0:8bded3c460c7 14 Author: Qiuyang Tao
taoqiuyang 0:8bded3c460c7 15 */
taoqiuyang 0:8bded3c460c7 16
taoqiuyang 0:8bded3c460c7 17 #include "mbed.h"
taoqiuyang 0:8bded3c460c7 18 #include "Servo.h"
taoqiuyang 0:8bded3c460c7 19 #include "QEI.h"
taoqiuyang 0:8bded3c460c7 20 #define PULSE_PER_REV 200
taoqiuyang 0:8bded3c460c7 21
taoqiuyang 0:8bded3c460c7 22 Ticker ctrl_updt_timer; //Timer
taoqiuyang 0:8bded3c460c7 23 Servo motor_ESC(p26); //ESC is connected to Pin 26
taoqiuyang 0:8bded3c460c7 24 Serial pc(USBTX, USBRX); //debug serial
taoqiuyang 0:8bded3c460c7 25 QEI wheel (p29, p30, NC, PULSE_PER_REV); // <500,000 pulses per second//Use X4 encoding.
taoqiuyang 0:8bded3c460c7 26 //QEI wheel (p29, p30, p27, PULSE_PER_REV); // If encoder has index channel
taoqiuyang 0:8bded3c460c7 27
taoqiuyang 0:8bded3c460c7 28 //All LEDs Blinks at start up, indicates mbed is setting up ESC
taoqiuyang 0:8bded3c460c7 29 DigitalOut led1(LED1); //System Status Indicator, Blink = Main Loop is Running
taoqiuyang 0:8bded3c460c7 30 DigitalOut led2(LED2); //Controller Status Indicator, Blink = Controller is Updating
taoqiuyang 0:8bded3c460c7 31 DigitalOut led3(LED3); //Controller Status Indicator, On = Motor RPM < Desired RPM
taoqiuyang 0:8bded3c460c7 32 // Motor is Aaccelerating
taoqiuyang 0:8bded3c460c7 33 // Off = Motor RPM > Desired RPM
taoqiuyang 0:8bded3c460c7 34 // Motor is De-Aaccelerating
taoqiuyang 0:8bded3c460c7 35 DigitalOut led4(LED4); //Encoder Counter Overflow
taoqiuyang 0:8bded3c460c7 36
taoqiuyang 0:8bded3c460c7 37
taoqiuyang 0:8bded3c460c7 38 //PID Related Variables - User
taoqiuyang 0:8bded3c460c7 39 float setpoint_rpm=10000; //Desired RPM = 10,000
taoqiuyang 0:8bded3c460c7 40 float dt=0.02; //Update Controller at 50Hz
taoqiuyang 0:8bded3c460c7 41 float Kp=0;
taoqiuyang 0:8bded3c460c7 42 float Ki=0;
taoqiuyang 0:8bded3c460c7 43 float Kd=0;
taoqiuyang 0:8bded3c460c7 44
taoqiuyang 0:8bded3c460c7 45
taoqiuyang 0:8bded3c460c7 46 //PID Related Variables
taoqiuyang 0:8bded3c460c7 47 float error_rpm=0;
taoqiuyang 0:8bded3c460c7 48 float prev_error_rpm=0;
taoqiuyang 0:8bded3c460c7 49 float u=0; //controller output, range = [0,1]
taoqiuyang 0:8bded3c460c7 50 float u_p=0;
taoqiuyang 0:8bded3c460c7 51 float u_i=0;
taoqiuyang 0:8bded3c460c7 52 float u_d=0;
taoqiuyang 0:8bded3c460c7 53
taoqiuyang 0:8bded3c460c7 54 //Global Variables - User
taoqiuyang 0:8bded3c460c7 55 int use_index_channel=0;
taoqiuyang 0:8bded3c460c7 56
taoqiuyang 0:8bded3c460c7 57 //use Unsigned long long if overflows frequently
taoqiuyang 0:8bded3c460c7 58 //unsigned long long motor_revolutions = 0;
taoqiuyang 0:8bded3c460c7 59 int motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
taoqiuyang 0:8bded3c460c7 60 int prev_motor_revolutions=0; //number of revolutions recorded by the encoder on the index channel
taoqiuyang 0:8bded3c460c7 61 int motor_pulses=0; //Number of pulses recorded by the encoder
taoqiuyang 0:8bded3c460c7 62 int prev_motor_pulses=0; //Number of pulses recorded by the encoder
taoqiuyang 0:8bded3c460c7 63 float motor_RPM=0; //Motor angular velocity
taoqiuyang 0:8bded3c460c7 64
taoqiuyang 0:8bded3c460c7 65
taoqiuyang 0:8bded3c460c7 66 void controller_update_ISR() {
taoqiuyang 0:8bded3c460c7 67 // get motor RPM
taoqiuyang 0:8bded3c460c7 68 if (use_index_channel) {
taoqiuyang 0:8bded3c460c7 69 motor_revolutions=wheel.getRevolutions();
taoqiuyang 0:8bded3c460c7 70 motor_RPM=(motor_revolutions-prev_motor_revolutions)/dt;
taoqiuyang 0:8bded3c460c7 71 prev_motor_revolutions=motor_revolutions;
taoqiuyang 0:8bded3c460c7 72 //reset encoder counter if overflows
taoqiuyang 0:8bded3c460c7 73 if(motor_revolutions>2147483647-setpoint_rpm*dt*10){
taoqiuyang 0:8bded3c460c7 74 //18446744073709551615; for Unsigned long long
taoqiuyang 0:8bded3c460c7 75 wheel.reset();
taoqiuyang 0:8bded3c460c7 76 prev_motor_revolutions=0;
taoqiuyang 0:8bded3c460c7 77 led4 = !led4;
taoqiuyang 0:8bded3c460c7 78 }
taoqiuyang 0:8bded3c460c7 79
taoqiuyang 0:8bded3c460c7 80 }else{
taoqiuyang 0:8bded3c460c7 81 motor_pulses=wheel.getPulses();
taoqiuyang 0:8bded3c460c7 82 motor_RPM=(motor_pulses-prev_motor_pulses)/PULSE_PER_REV/dt;
taoqiuyang 0:8bded3c460c7 83 prev_motor_pulses=motor_pulses;
taoqiuyang 0:8bded3c460c7 84 //reset encoder counter if overflows
taoqiuyang 0:8bded3c460c7 85 if(motor_pulses>2147483647-setpoint_rpm*dt*PULSE_PER_REV*10){
taoqiuyang 0:8bded3c460c7 86 //18446744073709551615; for Unsigned long long
taoqiuyang 0:8bded3c460c7 87 wheel.reset();
taoqiuyang 0:8bded3c460c7 88 prev_motor_pulses=0;
taoqiuyang 0:8bded3c460c7 89 led4 = !led4;
taoqiuyang 0:8bded3c460c7 90 }
taoqiuyang 0:8bded3c460c7 91 }
taoqiuyang 0:8bded3c460c7 92
taoqiuyang 0:8bded3c460c7 93 //PID controller
taoqiuyang 0:8bded3c460c7 94 error_rpm=setpoint_rpm-motor_RPM; //e=r-u;
taoqiuyang 0:8bded3c460c7 95 if (error_rpm>0){led3=1;} else {led3=0;} //Update Motor Status
taoqiuyang 0:8bded3c460c7 96 u_p=Kp*error_rpm;
taoqiuyang 0:8bded3c460c7 97 u_i=u_i+Ki*dt*error_rpm;
taoqiuyang 0:8bded3c460c7 98 u_d=Kd*(error_rpm-prev_error_rpm)/dt;
taoqiuyang 0:8bded3c460c7 99 u= u_p + u_i + u_d;
taoqiuyang 0:8bded3c460c7 100 prev_error_rpm=error_rpm;
taoqiuyang 0:8bded3c460c7 101
taoqiuyang 0:8bded3c460c7 102 //Controller Saturation
taoqiuyang 0:8bded3c460c7 103 if (u>1){u=1;}
taoqiuyang 0:8bded3c460c7 104 if (u<0){u=0;}
taoqiuyang 0:8bded3c460c7 105
taoqiuyang 0:8bded3c460c7 106 //Output ESC signal
taoqiuyang 0:8bded3c460c7 107 motor_ESC = u;
taoqiuyang 0:8bded3c460c7 108
taoqiuyang 0:8bded3c460c7 109 //Update LED2 status, tell user PID is updating
taoqiuyang 0:8bded3c460c7 110 led2 = !led2;
taoqiuyang 0:8bded3c460c7 111
taoqiuyang 0:8bded3c460c7 112 //Watch dog can be added here in the future
taoqiuyang 0:8bded3c460c7 113 }
taoqiuyang 0:8bded3c460c7 114
taoqiuyang 0:8bded3c460c7 115 int main() {
taoqiuyang 0:8bded3c460c7 116 //pc.printf("Debug mode: \n");
taoqiuyang 0:8bded3c460c7 117 led1 = 0; //init LED1, Main Loop status
taoqiuyang 0:8bded3c460c7 118 led2 = 0; //init LED2, PID status
taoqiuyang 0:8bded3c460c7 119 led3 = 0; //init LED3, Motor status
taoqiuyang 0:8bded3c460c7 120 led4 = 0; //init LED4, Encoder Counter status
taoqiuyang 0:8bded3c460c7 121 wait(0.5);
taoqiuyang 0:8bded3c460c7 122
taoqiuyang 0:8bded3c460c7 123 //All LEDs Blinks at start up, indicates mbed is setting up ESC
taoqiuyang 0:8bded3c460c7 124 motor_ESC = 0; //Generate zero speed signal for ESC startup
taoqiuyang 0:8bded3c460c7 125 led1 = 1;led2 = 1;led3 = 1;led1 = 1;
taoqiuyang 0:8bded3c460c7 126 wait(0.5);
taoqiuyang 0:8bded3c460c7 127 led1 = 0;led2 = 0;led3 = 0;led1 = 0;
taoqiuyang 0:8bded3c460c7 128 wait(0.5);
taoqiuyang 0:8bded3c460c7 129 led1 = 1;led2 = 1;led3 = 1;led1 = 1;
taoqiuyang 0:8bded3c460c7 130 wait(0.5);
taoqiuyang 0:8bded3c460c7 131 led1 = 0;led2 = 0;led3 = 0;led1 = 0;
taoqiuyang 0:8bded3c460c7 132 wait(0.5);
taoqiuyang 0:8bded3c460c7 133
taoqiuyang 0:8bded3c460c7 134
taoqiuyang 0:8bded3c460c7 135 ctrl_updt_timer.attach(&controller_update_ISR, dt); //Update Controller at (1/dt) Hz
taoqiuyang 0:8bded3c460c7 136
taoqiuyang 0:8bded3c460c7 137 while(1) { //blink LED1
taoqiuyang 0:8bded3c460c7 138 led1 = !led1;
taoqiuyang 0:8bded3c460c7 139 wait(0.2);
taoqiuyang 0:8bded3c460c7 140 }
taoqiuyang 0:8bded3c460c7 141 }