Forum >RPM holding PID loop for Brushless DC Motor with Encoder 12V 159RPM
RPM holding PID loop for Brushless DC Motor with Encoder 12V 159RPM

Hello DFRobot,
I have a number of your brushless gear motors and I have created a PID loop Arduino sketch for RPM control for robotics projects.
This is derived from the Interrupt sketch I suggested before.
The only things required, other than an Arduino, are:
* the Circular Buffer library which is an optimized array handler: https://github.com/rlogiacco/CircularBuffer
* the Arduino PID Library: https://github.com/br3ttb/Arduino-PID-Library
I have a number of your brushless gear motors and I have created a PID loop Arduino sketch for RPM control for robotics projects.
This is derived from the Interrupt sketch I suggested before.
The only things required, other than an Arduino, are:
* the Circular Buffer library which is an optimized array handler: https://github.com/rlogiacco/CircularBuffer
* the Arduino PID Library: https://github.com/br3ttb/Arduino-PID-Library
Code: Select all
/* BrushlessPID - Input desired RPM serial plotter, PWM is calculated through PID Code Example for brushless gearmotors from Shenzhen Chihai Motor C O, Ltd. https://chihaimotor.en.alibaba.com/productgrouplist-810760854/brushless_DC_motor.html https://wiki.dfrobot.com/FIT0441_Brushless_DC_Motor_with_Encoder_12V_159RPM Uses interrupts to count pulses from the Frequency Generator (FG) signal from the motor. Uses circular buffer to record nine timestamps from pulses to make an average from eight pulse width measurements. Uses PID Library to reach desired RPM. Copyright (c) 2020 Corey McGuire. This program is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FIT NESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ // Using Circular Buffer library : https://github.com/rlogiacco/CircularBuffer #define CIRCULAR_BUFFER_INT_SAFE #include <CircularBuffer.h> //Creating an array of length 9, one more than we will average since we must //find the difference between 9 timestaps to get 8 values CircularBuffer<unsigned long, 9> timings; // Using Arduino-PID-Library : https://github.com/br3ttb/Arduino-PID-Library #include <PID_v1.h> double Setpoint ; // will be the desired RPM double Input; // Frequency Generator (FG) signal from motor double Output ; // Motor PWM signal to motor // PID parameters : adjust these to get desired speed. double Kp=.01, Ki=2, Kd=.01; PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //create PID instance void setup() { pinMode(11, OUTPUT); //PWM PIN 11 with PWM wire analogWrite(11, 0); Serial.begin(9600); Setpoint = 75; // Hardcode the target RPM myPID.SetMode(AUTOMATIC); //Turn the PID on myPID.SetTunings(Kp, Ki, Kd); //Adjust PID values //Attach interrupt handler to pin 2. Pins 2 and 3 have dedicated inturupts //More info, here : http://www.gammon.com.au/forum/?id=11488 attachInterrupt (digitalPinToInterrupt (2), pulseRead, CHANGE); } //Function called on interrupt. Using a circular buffer in a FIFO fashion, //we are storing timestamps to be calculated outside the function to keep //the interrupt as short as possible. void pulseRead() { timings.unshift(micros()); } //Function for calculating average time between pulses. We disable inturrups //for the brief time we are calculating the average because an inturrupt //durring the calculation could cause a value to be recorded twice resulting //in an errant "0" pulse length, ruining our average. There is little chance //of this measure causing errent pulse length because the calculation should //be so fast, that if the motor generates a pulse during this time, it will //still be "HIGH" by the time the calculation exits and, thus, the interrupt //is triggered. float averagePulse(){ long sum=0; noInterrupts (); for (int j=1 ; j < timings.size();j++){ sum += timings[j-1]-timings[j]; } interrupts (); return sum /(timings.size()-1.0); } void loop() { // Read the value calculated from the average pulse width from Frequency // Generator (FG) to estimate RPM. Input = 111111/averagePulse(); //PID calculation myPID.Compute(); //Write the output (PWM) as calculated by the PID function analogWrite(11,Output); //LED is set to digital 3 this is a pwm pin. //Send data by serial for plotting Serial.print(Input); Serial.print(" "); Serial.println(Output); Serial.print(" "); Serial.println(Setpoint); Serial.print(" "); if (Serial.available()) { Setpoint = Serial.parseInt(); } }
2020-03-09 23:21:01 Running the code on an Arduino UNO and with Chihai Cotor C O CHR-36GP-BLDC3525 DC 24V 12V 36mm dc planetary gear brushless motor with built-in hall drive
https://youtu.be/WMdBVukM5xw
anonymous

