PID control DC Brushed Motors + Hall Sensors

[xkyve]

What I have done so far:

PWM module: values can be set from 0 (0%) to 3750 (100%). Any values above 3750 will be set as 100%.

Digital Hall sensors mounted and functioning (I used Timer Capture facility to measure the time between pulses). Maximum angular speed read is about 115 Hz. This means:

-the motor speed values read will be updated with a maximum rate of 115 times per second
-the minimum, which i can configure, is set at about 20 times per second (for now)

In other words my sensor data will update in the interval of 20-115 times per second. The bottom value i can configure.
This means I have a decently accurate value every 50 miliseconds (20 times per second) expressed in Hz, even miliHertz if I want to.

Now I have also created a buffer, or a filter, where I store every x miliseconds (configured 50 ms for now) the current angular speed, so that I can have an average motor speed computed when requested (dont know if necessary).

I want to maintain motor speed at a given value.

I have hooked up the microcontroller to Matlab (using RS232) so I can graph the motor speed. I never created a PI or PID digital control so what should I do next?

I'm guessing I have to create a function that will execute every x miliseconds where I compute the PWM future value. Is this correct? What value should x take?

I read this great tutorial http://igor.chudov.com/manuals/Servo-Tuning/PID-without-a-PhD.pdf
but it leaves out some implementation details. Can you help me?

thank you

in case it will be needed, project code: https://github.com/xkyve/mobility

[ahes]

I've made my own servo about 3 months ago.

I'm building my own Grad project (robot arm )

I didn't use encoder as a feedback only simple rotary pot. ( buying 11 encoders for two robots is a little bit expensive )

The motor is brushed DC 2A rated at rated speed 24 V.

here are the videos for the experiment

servo mechanism.avi

wrist servo mechanism slow mode.avi.avi

the controller is a P controller ( position servo is 3rd order system you might lose stability if you want to use pi controller during the normal operation) and then it'll switch to PI to hold the motor at it's final position

If you're interested I can post you the schematic and the control program "C for AVR micro"

[xkyve]

Very nice project!

I would like to look into the digital control program, if you're so kind to post it.

I managed to implement a control algorithm, also, for my DC motors and it's looking pretty good on the graphs, but i have to tune the gains. It's weird cause I have the best results with P=24 I=12 (proportional and integral gain). I must've made a mistake somewhere.

https://github.com/xkyve/mobility/blob/master/Pid.c

thank you

[ahes]

Code: [Select]

Chip type               : ATmega8
Program type            : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model            : Small
External RAM size       : 0
Data Stack size         : 256
*****************************************************/

#include <mega8.h>

#include <delay.h>

#ifndef RXB8
#define RXB8 1
#endif

#ifndef TXB8
#define TXB8 0
#endif

#ifndef UPE
#define UPE 2
#endif

#ifndef DOR
#define DOR 3
#endif

#ifndef FE
#define FE 4
#endif

#ifndef UDRE
#define UDRE 5
#endif

#ifndef RXC
#define RXC 7
#endif

#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<DOR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)

// USART Receiver buffer
#define RX_BUFFER_SIZE 8
char rx_buffer[RX_BUFFER_SIZE];

#if RX_BUFFER_SIZE<256
unsigned char rx_wr_index,rx_rd_index,rx_counter;
#else
unsigned int rx_wr_index,rx_rd_index,rx_counter;
#endif

// This flag is set on USART Receiver buffer overflow
bit rx_buffer_overflow;

// USART Receiver interrupt service routine
interrupt [USART_RXC] void usart_rx_isr(void)
{
char status,data;
status=UCSRA;
data=UDR;
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
   {
   rx_buffer[rx_wr_index]=data;
   if (++rx_wr_index == RX_BUFFER_SIZE) rx_wr_index=0;
   if (++rx_counter == RX_BUFFER_SIZE)
      {
      rx_counter=0;
      rx_buffer_overflow=1;
      };
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Get a character from the USART Receiver buffer
#define _ALTERNATE_GETCHAR_
#pragma used+
char getchar(void)
{
char data;
while (rx_counter==0);
data=rx_buffer[rx_rd_index];
if (++rx_rd_index == RX_BUFFER_SIZE) rx_rd_index=0;
#asm("cli")
--rx_counter;
#asm("sei")
return data;
}
#pragma used-
#endif

// USART Transmitter buffer
#define TX_BUFFER_SIZE 8
char tx_buffer[TX_BUFFER_SIZE];

#if TX_BUFFER_SIZE<256
unsigned char tx_wr_index,tx_rd_index,tx_counter;
#else
unsigned int tx_wr_index,tx_rd_index,tx_counter;
#endif

// USART Transmitter interrupt service routine
interrupt [USART_TXC] void usart_tx_isr(void)
{
if (tx_counter)
   {
   --tx_counter;
   UDR=tx_buffer[tx_rd_index];
   if (++tx_rd_index == TX_BUFFER_SIZE) tx_rd_index=0;
   };
}

#ifndef _DEBUG_TERMINAL_IO_
// Write a character to the USART Transmitter buffer
#define _ALTERNATE_PUTCHAR_
#pragma used+
void putchar(char c)
{
while (tx_counter == TX_BUFFER_SIZE);
#asm("cli")
if (tx_counter || ((UCSRA & DATA_REGISTER_EMPTY)==0))
   {
   tx_buffer[tx_wr_index]=c;
   if (++tx_wr_index == TX_BUFFER_SIZE) tx_wr_index=0;
   ++tx_counter;
   }
else
   UDR=c;
#asm("sei")
}
#pragma used-
#endif

// Standard Input/Output functions
#include <stdio.h>



#define FIRST_ADC_INPUT 5
#define LAST_ADC_INPUT 5
int adc_data[LAST_ADC_INPUT-FIRST_ADC_INPUT+1];
#define ADC_VREF_TYPE 0x00

// ADC interrupt service routine
// with auto input scanning
interrupt [ADC_INT] void adc_isr(void)
{
static unsigned char input_index=0;
// Read the AD conversion result
adc_data[input_index]=ADCW;
// Select next ADC input
if (++input_index > (LAST_ADC_INPUT-FIRST_ADC_INPUT))
   input_index=0;
ADMUX=(FIRST_ADC_INPUT | (ADC_VREF_TYPE & 0xff))+input_index;
// Delay needed for the stabilization of the ADC input voltage
delay_us(10);
// Start the AD conversion
ADCSRA|=0x40;
}

// MOtor output handls motor pins except PWM
// direction and fast decay included

void M_out(char state)
{
   if(state==0)
   {
      PORTB=0;
   }
   else if(state==2)
   {
        PORTB=7;
        PORTD=0x00;
   }
   else if(state==1){
        PORTB=6;
        PORTD=0xf0;
   }   
}
// Timer2 output compare interrupt service routine
// This timer is the RTC timer for the control loop
// this varialble carries the desired position

// variable that carries the desired pos for loop function
int desired_pos;
int i_error;
interrupt [TIM2_COMP] void timer2_comp_isr(void)
{
// Place your code here
    // Holds the position error
    int p_error;
    p_error = desired_pos-adc_data[0];
    // now perform PID control
    //asume first it's a P controller
    //assume P =1
         
    if(p_error<0){
        M_out(2);
        p_error*=-1;
    }
    else
        M_out(1);
    if (p_error<=1){
         p_error=0;
         i_error=0;
    }
    else{
        if(p_error<=40){
        // add PI controller
            if(i_error<450)
                i_error+=p_error;
            else i_error=450;
            p_error*=40;
            p_error+=i_error*4;       
        //else if(p_error<=20)
       //     p_error*=50;
       }
        else if(p_error<=50)
            p_error*=20;
        else
            p_error=1023;
    }
    //if(p_error<=6)
      //  p_error=0;
    OCR1AH=p_error/256;
    OCR1AL=p_error;
}
// Declare your global variables here

void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=Out Func1=Out Func0=Out
// State7=T State6=T State5=T State4=T State3=T State2=1 State1=1 State0=1
PORTB=0x07;
DDRB=0x07;

// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=Out Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=0 State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x80;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
TCCR0=0x00;
TCNT0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 8000.000 kHz
// Mode: Fast PWM top=03FFh
// OC1A output: Inverted
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xC3;
TCCR1B=0x09;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: 7.813 kHz
// Mode: CTC top=OCR2
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x0F;
TCNT2=0x00;
OCR2=0x0F;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
MCUCR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x80;

// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud Rate: 2400 (Double Speed Mode)
UCSRA=0x02;
UCSRB=0xD8;
UCSRC=0x86;
UBRRH=0x01;
UBRRL=0xA0;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC Clock frequency: 500.000 kHz
// ADC Voltage Reference: AREF pin
ADMUX=FIRST_ADC_INPUT | (ADC_VREF_TYPE & 0xff);
ADCSRA=0xCC;

// Global enable interrupts
#asm("sei")
//initialize control loop variables
delay_ms(10);
desired_pos=adc_data[0];
//test statement
//desired_pos=500;
while (1)
      {
      // Place your code here
           desired_pos=getchar()*4;
      };
}

here's the code for the control algorithm

feel free to ask me about any thing ambiguous in the code

I'm sorry I will look into your code later, I'm exhausted right now from a stupid exam.

[xkyve]

From what i understood:

Depending if your error is negative/positive you set up some GPIO pins.
If your error is small enough (<= 1) you set the error and integral state error as if it was 0.

Otherwise, if it's a considerable error, you set up 3 cases of error: below 40, 40-50, above 50. In each of these 3 cases you have different values of gains.

Only in the first case (below 40) you activate PI control. You limit the integral state error with "if(i_error<450)".
In the second case you have proportional gain only (value 20) and in the last case you set the error (or better said in this case output control value) to 1023 [for limiting, or saturation?].

In the end you write the calculated output to the PWM timers? Did I understand correctly?
Why do you have so many stages or cases?
 
When you are bored and have nothing better to do could you look into my PID algorithm (link above Pid.c) to see if I have slipped a mistake somewhere and also what could I improve? [function is called pid_execute]

Thank you

[ahes]

Depending if your error is negative/positive you set up some GPIO pins.

GPIO pins are for controlling the H-Bridge

If your error is small enough (<= 1) you set the error and integral state error as if it was 0.

Otherwise, if it's a considerable error, you set up 3 cases of error: below 40, 40-50, above 50. In each of these 3 cases you have different values of gains.

Only in the first case (below 40) you activate PI control. You limit the integral state error with "if(i_error<450)".
In the second case you have proportional gain only (value 20) and in the last case you set the error (or better said in this case output control value) to 1023 [for limiting, or saturation?].

In the end you write the calculated output to the PWM timers? Did I understand correctly?

exactly

Why do you have so many stages or cases?

in order to get the maximum performance of the motor you have to maximize your controller's gain but  you will lose stability eventually

using a digital controller offers you a great flexibility in your control scheme why not to use it?

if your error is large, you command the motor to work in it's maximum power.

as the error decreases, at certain point you need to limit the rate of change of your error signal to avoid oscillations

example of using higher gain

wrist servo mechanism fast mode.avi

also part of it because I want to save some of the computing power of the controller "if you know that at this certain value of error and gain the controller will saturate, why do you bother calculating it instead of writing the final value.

When you are bored and have nothing better to do could you look into my PID algorithm (link above Pid.c) to see if I have slipped a mistake somewhere and also what could I improve? [function is called pid_execute]

sure and I don't have to be bored   

[ahes]

I've looked into your code

why assigning a minimum value for your "I" controller It means that the output of the controller wont reach zero ever. what if you want your motor to stand still without any movements.

the second note, using too much types and structs
depending on the efficiency of your compiler this will take forever execution time if you have high sampling rate.

[xkyve]

Thank you for you response

The minimum and maximum value I have assigned for the "I" controller is the minimum and maximum of what the drive can actually accept. I quote from this tutorial http://igor.chudov.com/manuals/Servo-Tuning/PID-without-a-PhD.pdf

"The easiest and most direct way to deal with integrator windup is to limit the integrator state"
"Usually you can just set the integrator minimum and maximum so that the integrator output matches the drive minimum and maximum."

I'm not sure if I need this or not, but this is my first digital controller and I've taken all the code from that tutorial and I try to experiment with it.

My processor can handle those types, it's running at 60Mhz and the sampling rate is not very tight.

I have one more question. At which frequency do you collect or sample the data from the system output and at which frequency do you execute the control algorithm? Is the rate the same? Should it be?

I sample at a maximum rate of 40 ms (depends on the motor speed) and the controller executes every 60 milliseconds.

[ahes]

I understand the maximum value for anti windup but minimum ?

yes I can use the minimum as the initial value for the integrator ( as not to make the integrator to start from zero every time to speed up the response ) but what if the command signal is zero

but at least tell me what is this servo is for "speed , position or both"

for the sampling rate the higher the better

but you have many constraints

the most obvious is your controller speed but you mentioned that it's 60 MHz then you don't have a real problem on that.

the feed back device's resolution will limit the sampling rate

assuming you're using 500 Pulse/rev rotary encoder

you calculate the speed by subtracting the previous value of position from the current value

what will happen if your sampling rate is greater than the encoder resolution multiplied by the speed in rad/sec, in some intermediate sample the estimated speed will appear as zero (quantization error ).


in my case it was a position servo and feed back device was giving me an absolute value ( pot ) I had no constraint on sampling rate except the controller.

[xkyve]

I'm controlling the speed of a DC brushed motor. I'm using Hall sensors to measure the speed (rotations / second).

My program stores the current speed into a global variable. The speed info is updated at a rate which depends on the actual motor speed: the hall sensor generates a digital square wave with the frequency equal with that of the motor speed. The square waves go into the Timer Capture Input module. I can calculate the speed this way.

In other words the speed data is updated with a frequency of a maximum 110 Hz (maximum motor speed) and a minimum of 25 Hz (which is the time out). Why did I implement this time out? Because if the motor were to run at 100Hz and then I would suddenly stop it, no more impulses would be generated to the Timer Capture. The speed data would stay wrongly at a non zero value. Time-out solves this problem.

Therefore I can say that my sample rate, or data collection rate is between 25Hz - 110Hz.
My control algorithm runs every 60 milliseconds (16.67 Hz). This values can be configured and I don't know if I should let them be this way or not.

Is it weird I have a proportional gain of 14 and an integral gain of 10? Here is the system response:

Controlul digital al turatiei al unui motor

[ahes]

I thought that the Hall sensor is to measure the current in the motor winding

now you're using it as a speed counter, in this case the resolution is crap."only one pulse/rev"

for the proportional gain and the integral, no it is not weird at all

those values depends upon the poles in your system and the response you want.

for the response video it's great I guess except when the speed was 0, the motor started to fluctuate around the zero speed.
is that the actual response on your motor shaft.

[ahes]

before I forget the values of Gains in your system controller can you put them in this form :

14(1+0.7142 S)

it means you have a mechanical time constant (inertia / friction ) around 0.7 sec. (it's pretty high if the motor is unloaded )

but if the response is satisfying for you, then who cares.

[xkyve]

Yes, that is the actual response on my motor axis. As you said, the resolution is crap - for a single motor shaft revolution I get a single impulse from the Hall sensor. The motor stalls at 0 in the beginning when a command is sent.

Kp = 14
where does 0.7142 come from?

[ahes]

Kp = 14
where does 0.7142 come from?

before I explain, have you toke an introductory course in automatic control, or at least know the basics of advanced mathematics especially laplace transform.

[ahes]

for the stopping problem you can use another technique to stop the motor

if you are using  H-bridge you can stop the PID controller after receiving the 0 speed value

search for regenerative breaking.

[xkyve]

I took an introductory course in automatic system theory and I know about Laplace transform. I apologize because it was a long time and I can't remember a lot of stuff.

My Pi controller looks like this: 14 + 10 * 1/s
So I thought you referred to this.

I will research more before asking further questions.

[ahes]

My Pi controller looks like this: 14 + 10 * 1/s
So I thought you referred to this.

yes my apologies, I confused it with PD controller

Ok then I assume you know the mathematical model of the DC machines

K/(te *s +1)(tm *s +1)

where te is the electrical time constant =l/r (inductance / resistance )
and tm is the mechanical time constant = J/b (inertia / friction coefficient )

in a typical  DC motor the mechanical time constant is much longer than the "te" making the mechanical pole to be the dominant pole in the system

putting the PI controller transfer function in the form of k(1+t *s)/s as zero and a pole at origin

where changing the integrator gain will change this zero location

Ideally what you always try to do is to place the zero of the PI controller over the mechanical pole this will cancel the effect of the mechanical pole

the multiplication of the motor and PI controller transfer function will produce 2nd order system of type 1 (one integrator)

then the remaining term is the proportional gain K

then you tune k to get the fastest response of the system (try to make it critically damped ) if you increased  k beyond that value you will get ringing response.