Hi guys. I'm having problems with my project about using arduino for DC motor speed control. I have wrote a program mostly based on PID library and struggled with the interrupt pin. Can you guys help me please, Im new so please go easy on me. I'm kinda desperate right now, 3 days and I haven't made any progress.
P/s: i tried to add few lines about enabling and disabling interrupt, but it didnt work also
/*working variables*/
#define PWM 6
#define sensorA 2
#define sensorB 8
//====
int lastTime;
float Input, Output, Setpoint=0;
float ITerm, lastInput;
float kp, ki, kd;
int SampleTime = 100; //0.1 sec
float outMin =40, outMax;
//=====
volatile int pulse =0;
//=====
const byte numChars = 32;
char receivedChars[numChars];
char tempChars[numChars];
boolean newData = false;
float Kp,Ki,Kd;
//=====
void setup() {
pinMode(PWM,OUTPUT);
pinMode(sensorA,INPUT);
pinMode(sensorB,INPUT);
digitalWrite(sensorA,HIGH);
digitalWrite(sensorB,HIGH);
attachInterrupt(0,rpm,RISING);
Serial.begin(9600);
}
void loop() {
recvWithStartEndMarkers();//nhan du lieu
if (newData == true) {
strcpy(tempChars, receivedChars);
parseData();
showNewData();
newData = false;
}
SetTunings(Kp,Ki,Kd);//dat chi so kp ki kd
sei();
delay(100);
cli();
getMotorData();//tinh so vong quay
Compute();//tinh output
motorForward(Output);//viet output
Serial.println(Input);
}
void Compute()
{
unsigned long now = millis();
int timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/
float error = Setpoint - Input;
ITerm+= (ki * error);
if(ITerm> outMax) ITerm= outMax;
else if(ITerm< outMin) ITerm= outMin;
float dInput = (Input - lastInput);
/*Compute PID Output*/
Output = kp * error + ITerm- kd * dInput;
if(Output > outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
/*Remember some variables for next time*/
lastInput = Input;
lastTime = now;
}
}
void SetTunings(float Kp, float Ki, float Kd)
{
float SampleTimeInSec = ((float)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
}
void SetOutputLimits(float Min, float Max)
{
if(Min > Max) return;
outMin = Min;
outMax = Max;
if(Output > outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
if(ITerm> outMax) ITerm= outMax;
else if(ITerm< outMin) ITerm= outMin;
}
//============
void getMotorData() {
static int pulseAnt = 0;
Input = (pulse - pulseAnt)/(SampleTime*334/1000);
pulseAnt = pulse;
}
void rpm() {
if (digitalRead(sensorB),LOW) pulse++;
else pulse--;
}
void motorForward(int Output) {
analogWrite(PWM, Output);
}
//========
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
void parseData() {
// split the data into its parts
char * strtokIndx; // this is used by strtok() as an index
strtokIndx = strtok(tempChars,",");
Setpoint=atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Kp = atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Ki = atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Kd = atof(strtokIndx);
}
//=====
void showNewData() {
if (newData == true) {
Serial.println(Setpoint);
Serial.println(Kp);
Serial.println(kp);
Serial.println(pulse);
Serial.println(Input);
Serial.println(Ki);
Serial.println(ki);
}
}
Thank you sir, for replying. I use pin 2 as interrupt pin, however, im not sure whether or not i need to enable and disable global interrupt to make it work. (I loaded the code and my motor stood still). And can you explain why my "rpm" wouldnt work?
I think my code is choking some where. I have checked, it received my parameters right. However, for every loop, the "Input" is always 0 and the Compute function doesnt seem to produce any result.
UPDATE: I have changed all my function into PID lib. The problem is my "Input" is constantly 0, so the "Output" always tends to Max. Im quite sure that there is no problem with my optical encoder. Can anyone spot out the fault in my code please. Many thanks
#include <PID_v1.h>
/*working variables*/
#define PWM1 6
#define sensorA 2
//====
unsigned long time;
double Setpoint, Input, Output;
volatile double pulse;
int SampleTime=50;//0.05s
//=====
const byte numChars = 32;
char receivedChars[numChars];
char tempChars[numChars];
boolean newData = false;
double Kp,Ki,Kd;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
//=====
void setup() {
pinMode(PWM1,OUTPUT);
pinMode(sensorA,INPUT);
digitalWrite(sensorA,HIGH);
attachInterrupt(digitalPinToInterrupt(sensorA),rpm,RISING);
Serial.begin(9600);
myPID.SetMode(AUTOMATIC);
}
void loop() {
recvWithStartEndMarkers();//nhan du lieu
if (newData == true) {
strcpy(tempChars, receivedChars);
parseData();
newData = false;
}
sei();
myPID.SetTunings(Kp,Ki,Kd);//dat chi so kp ki kd
getMotorData();//tinh so vong quay
myPID.Compute();//tinh output
analogWrite(PWM1, Output);
}
//============
void getMotorData() {
//calculate velocity
Input = (pulse/(SampleTime*41/1000));
Serial.println(Input);
pulse=0;
}
void rpm() { //pulse counter
pulse++;
}
//========
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = '<';
char endMarker = '>';
char rc;
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
void parseData() {
// split the data into its parts
char * strtokIndx; // this is used by strtok() as an index
strtokIndx = strtok(tempChars,",");
Setpoint=atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Kp = atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Ki = atof(strtokIndx);
strtokIndx = strtok(NULL, ",");
Kd = atof(strtokIndx);
}
//=====
This time, I tried to separate blocks, still, focusing on counting. I tried this simple version of pulse counting and realize that my time interval is far more longer than it should. The result is approx. 2.5secs where as in my calculation, it should be less than 5ms. Im using pin 2 as interrupt pin, checked pin 3 as well, result still not improved. Any suggestion what should I do next to check pin interrupt will be appreciated
#define sensorA 2
volatile double pulse;
double rpm;
unsigned long timeold;
unsigned long now=millis();
void setup()
{ pinMode(sensorA,INPUT);
digitalWrite(sensorA,HIGH);
Serial.begin(9600);
attachInterrupt(digitalPinToInterrupt(sensorA), counter, FALLING);
pulse = 0;
rpm = 0;
timeold = 0;
}
void loop()
{
unsigned long now=millis();
if (pulse >= 100) {
//Update RPM every 20 counts, increase this for better RPM resolution,
//decrease for faster update
rpm = (60.0*1000.0*pulse)/(334.0*(now-timeold));
Serial.println((now-timeold),DEC);
timeold = now;
pulse=0;
}
}
void counter()
{
pulse++;
}
I am assuming your motor only turns in one direction.
You can calculate you RPM with only the duration between pulses
you only need the last instance of RPM to successfully control the motor
Try something like this:
void rpm() {
unsigned long PulseTime = micros();
PulseDuration = PulseTime - LastPulseTime;
LastPulseTime = PulseTime;
pulse++;
}
// do the conversion to RPM later when you need to know
// I'm rusty at the math but you should be able to easily convert Microseconds per revolution into RPM
unsigned long MicrosecondsPerRevolution = (PulseDuration * PulsesPerRevolution);
int RPM = 1/(MicrosecondsPerRevolution * .000001) * 60;
you can use the pulse++ to tell if the motor is running.
Thank you guys for replying.
I finally get my motor to work, the problem was with my encoder indeed, its pins were loose. Now, I'm going back to the calculation algorithm. I.e if I set parameters as <60,2,0,0> (<setpoint,Kp,Ki,Kd>), my speed will only reach about 45rpm and it stays there. My assumption is: the program needs the error to differ from 0 to produce a meaning output (what I mean is: if the error equals to 0, duty of PWM will turn to 0 and my motor stop). Therefore at 45 rpm, the duty of PWM and my speed is balanced and the program will continuously produce the same Output for PWM. Can anyone confirm?
Thanks
The output of your P regulator is proportional to the error, i.e. a non-zero error will persist even in balanced state. That's why Ki should be added (become non-zero), so that this remaining error is almost eliminated after some (short) time.
You must adopt (increase) Kp, if the desired effect is not reached. In theory Kp can become arbitrarily high, but if you make it too high I suspect that the loop can start oscillating due to the motor response.
This is a tachometer program that I used with a optical encoder with 400 steps per revolution. It uses PID to control a motor with direct drive to the encoder at 2000 RPM
This requires the PID_v2 library I created to maintain stable PID control (PID_v1 is flawed).
I Posted earlier with an easier way to calculate RPM I seen this from another example I haven't adapted this RPM calculation to the new method but it works none the less.
Enjoy
Z
Test setup:
(The board is an atmega328p (UNO) arduino at heart motor controller I created)
zhomeslice:
This is a tachometer program that I used with a optical encoder with 400 steps per revolution. It uses PID to control a motor with direct drive to the encoder at 2000 RPM
More interesting stuff. Thank you.
I have taken a copy of your program for study.
Do you know if it will work (without any code changes) if the setpoint speed is controllable by the user.
And what about if the load on the motor changes?
These lines
myPID.Compute();
analogWrite(PWMpin, Output);
suggest that the compute function produces a non-zero value when there is no error. But in your robot-balancer it seemed to produce a zero output when there was no error.
Do you know if it will work (without any code changes) if the setpoint speed is controllable by the user.
this toggled between 2 speeds I have it fixed now at 2100 but when testing i would switch it between 800 and 2100 rpm
put drag on the shaft and more below 800 for my setup was less than what the motor would stay running.
The PID is quite responsive and loads would power up the motor to 100% quite quickly
************ Note about my (1 line) blink without delay() timer ************
for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 100; QTimer = millis() ) { // one line Spam Delay at 100 miliseconds
// stuff
}
// translates to
static long QTimer = millis();
if((long)( millis() - QTimer ) >= 100){
QTimer = millis();
// stuff
}
I was scolded a while back on this forum for suggesting this by AARG who thinks this is bad coding because it is confusing. ( Sorry if your were confused about this...)
These lines
myPID.Compute();
analogWrite(PWMpin, Output);
suggest that the compute function produces a non-zero value when there is no error. But in your robot-balancer it seemed to produce a zero output when there was no error.
...R
Yes and I have thought about this I make a change to the PID_v2 which allows us to prime the integral portion of the PID
PrimeIntegral(double DesiredOutput)
if the motor is stopped you can prime the integral to a starting point high enough to cause the motor to run and start receiving RPM data allowing for additional adjustments to take place. I haven't tested this yet I may need a day or so but here's the altered PID_v2 code
Robin2:
Many thanks @zhomeslice. More food for thought - but I won't get much chance to think about it for a couple of days.
...R
Thanks Robin2 you gave me an idea that improved my PID code just added and tested the PrimeIntegral(X); function with excellent results Take a look:
Motor starts quickly and seamlessly with this easy startup sequence:
Just made some basic changes to the encoder code and WOW with a more powerful 12V 2 amp supply I got these readings with the setpoint at 7100 RPM! My calculations are crazy! With my encoder at 400 pulses per revolution at 118.3 revolutions per second we are counting 47300+ pulses per second! and it maintained setpoint!
Parts:
using UNO with rotary encoder LPD3806-400bw-g5-24c and small 12V dc brushed motor
connections:
only using 1 encoder wheel wire colors red +5V, black GND white to Pin2 with a 2K ohm resister between Red and White
output using PWM on pin 3 (pin 4 enables my H-Bridge) a simple mosfet switch would suffice for single direction testing
Attaching all files used:
Thanks again. I won't have time to study it today.
Here are a couple of ideas to give you something to think about.
First, something relatively trivial. I find that my motors don't always start at the same PWM so I have been thinking of a system that simply updates the value from zero in several steps if the motor has stalled (or stopped deliberately) then when pulses start being produced it uses them in the usual feedback mode.
I think it is not your basic design (so I presume you won't be offended) but it seems to me to be very lazy programming to use floats and doubles in a microprocessor. I am impressed by the performance you have posted but I suspect it would be much less trouble for the Arduino if all the PID calcs were done with integers. I wonder, with a bit of care with the maths, if it might even be possible to do it with 16 bit ints rather than 32 bit longs.
As a separate issue, one of my "problems" is that my present "encoder" only produces 1 pulse per revolution (ppr). I may be able to increase that to 3 ppr If I can find a suitable source of hex bar. I don't feel able to paint three (or even 2) spots on a round bar accurately at 180 deg spacing. Either way, I think this will make it harder to maintain an even speed because there is more time for acceleration (or deceleration) between speed measurements.
And, of course, I want my Arduino (Attiny1634) to run an nRF24 wireless transceiver as well as controlling the speed.
