Software > Software

PID control DC Brushed Motors + Hall Sensors

(1/4) > >>

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: ---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;
      };
}

--- End code ---

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

Navigation

[0] Message Index

[#] Next page