Podłączę się do tematu.
Konstruuję sterownik silnika prądu stałego - serwonapęd.
W tym momencie mam gotowe podprogramy:
- odczyt pozycji z enkodera
- regulator PID (nie działa tak jakbym chciał, albo potrzebny jest tuning parametrów albo mam gdzieś błąd w kodzie)
W tym momencie silnik lubi oscylować wokół pozycji zadanej.
W tym momencie stanąłem na funkcjach generowania ruchu trapezoidalnego. Prawdę mówiąc nie wiem od czego zacząć. Domyślam się, że potrzebna w programie będzie prędkość aby określić max V i a.
Nigdy wcześniej nie programowałem, więc chyba i tak daleko z tym zaszedłem...
Uprzejmie proszę o jakieś wskazówki jak zrobić pomiar prędkości. (coś próbowałem z sprawdzaniem różnicy stanu rejestru pozycji w parzystych i nieparzystych cyklach przerwania regulatora PID, ale nie działało to za dobrze).
Timer0 działa jako przerwanie 1kHz z pętlą sprawdzania pozycji i liczenia regulatora PID.
Timer1 działa jako PWM z f=15kHz.
Poszukuję też kogoś z Bydgoszczy chętnego udzielić korepetycji w tym temacie. Oczywiście odpłatnie. Czekam na PM od chętnych.
Kod źródłowy:
#define F_CPU 16000000L
#include <avr/io.h>
#include <stdint.h>
#include <stddef.h>
#include <util/delay.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include "rprintf.h"
#include "pid.h"
#include <math.h>
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define ENC_PORT PIND
#define SET_IN3 PORTC |= _BV(0)
#define SET_IN4 PORTC |= _BV(1)
#define CLR_IN3 PORTC &= ~_BV(0)
#define CLR_IN4 PORTC &= ~_BV(1)
/*************** PID variables *******************/
#define K_P 160
#define K_I 0.01
#define K_D 0
struct PID_DATA pidData;
volatile int16_t motorReference=0x0010;
volatile int16_t measurementValue, inputValue;
/**************** Encoder variables ***************/
int16_t en_pos, en_pos_prev, change, speed;
int i;
int temp;
unsigned char en_dir;
unsigned char en_now=0;
unsigned char en_prev;
/**************** PWM variables *******************/
int16_t pwm_value;
void init_IO(void)
{
DDRD = 0x20; // Ustawia PORTD jako wyjscie/wejście
PORTD = 0xFF; // Podciąga PORTD do 1
DDRA = 0xF0; // Ustawia PORTA jako wejscie/wyjscie
PORTA = 0xFF; // Podciaga PORTA do 1
DDRB = 0xFF;
PORTB = 0xFF;
DDRC = 0xFF;
PORTC = 0xFF;
/**************************************************/
TIMSK = 0x02;
/***************** Config T0 **********************/
//ustawienie timer do przerwania z f 1kHz
OCR0 = 0x75;
TCCR0 = 0x0B;
/**************** PWM Config T1 *******************/
TCCR1A = 0x83; // Konfiguracja z CodeVision
TCCR1B = 0x09;
TCNT1H = 0x00;
TCNT1L = 0x00;
ICR1H = 0x00;
ICR1L = 0x00;
OCR1AH = 0x00;
OCR1AL = 0x00;
OCR1BH = 0x00;
OCR1BL = 0x00;
/***************** Config T2 **********************
ASSR=0x00;
TCCR2=0x07;
TCNT2=0x00;
OCR2=0xFF;
/**************** LCD Init ************************/
LCDinit();
LCDcursorOFF();
/**************** PID Init ************************/
pid_Init(K_P * SCALING_FACTOR, K_I * SCALING_FACTOR , K_D * SCALING_FACTOR , &pidData);
}
void encoder_init(void)
{
en_pos = 0;
sbi(MCUCR,ISC00); // Ustawienie INT0
cbi(MCUCR,ISC01); // na zbocze rosnace i opadające
sbi(GICR,INT0); // Wlaczenie przerwania INT0
sei();
}
void encoder_off(void)
{
cbi(GICR,INT0);
}
void encoder_on(void)
{
sbi(GICR,INT0);
}
void set_encoder_position(int position)
{
en_pos = position;
}
void go_right(void)
{
SET_IN3;
CLR_IN4;
PORTA |= _BV(5);
PORTA &= ~_BV(4);
}
void go_left(void)
{
SET_IN4;
CLR_IN3;
PORTA |= _BV(4);
PORTA &= ~_BV(5);
}
void stop_motor(void)
{
CLR_IN3;
CLR_IN4;
PORTA |= _BV(4);
PORTA |= _BV(5);
}
void print_byte_to_lcd(uint8_t dana)
{
LCDsendChar(dana); // funkcja wypisujaca
}
void Set_Input(int16_t inputValue)
{
pwm_value = inputValue; // Skalowanie wyniku -
if(pwm_value >= 0x0240) // Ograniczenie mocy silnika
{
pwm_value = 0x0240; // Jeśli przekroczono wartość maks, to ustaw maks
}
if(pwm_value < 0x00) //Wykluczenie pwm < od skutecznej wartości, ktora powoduje ruch silnika
{
pwm_value = 0x00; // Jeśli przekroczono wartość maks, to ustaw 0
}
OCR1A = pwm_value; // Wpisanie wart. PWM do rejestru porównującego
}
SIGNAL (INT0_vect) //obsługa przerwania
{
en_now = (ENC_PORT & (3 << 2)) >> 2; // odczyt portu i przesuniecie bitow na prawo
en_dir = (en_prev & 1) ^ ((en_now & 2) >> 1); // sprawdzenie kierunku obrotu
if(en_dir == 0)
{
en_pos++; // Kierunek obrotu w prawo: dodaj
}
else
{
en_pos--; // Kierunek obrotu w lewo: odejmij
}
en_prev = en_now; // Zapamiętaj ostatnią wartosc do porownania nast. razem
}
ISR (TIMER0_COMP_vect)
{
/*i++;
if(i % 2)
{
en_pos_prev = en_pos;
}
else
{
change = (en_pos - en_pos_prev) * 10;
}*/
measurementValue = en_pos;
// zmiana kierunkow pracy
if(motorReference < measurementValue)
{
go_left();
}
else
{
go_right();
}
if(abs(motorReference - measurementValue) < 1) //wylacz silnik w pozycji zadanej
{
stop_motor();
}
inputValue = pid_Controller(motorReference, measurementValue, &pidData);
Set_Input(inputValue);
PORTA ^= _BV(6); // Mruganie diodą w celu sprawdzenia predkosci programu przerwania
}
/*
ISR (TIMER2_COMP_vect)
{
i++;
if(i % 2)
{
en_pos_prev = en_pos;
}
else
{
change = en_pos - en_pos_prev;
}
PORTA ^= _BV(6); // Mruganie diodą w celu sprawdzenia predkosci programu
}
*/
int main (void)
{
init_IO();
encoder_init();
rprintfInit(print_byte_to_lcd);
LCDclr();
while(1)
{
LCDhome();
if((PIND & 1) == 0)
{
motorReference += 1; //Sterowanie silnikiem w prawo
}
if((PIND & 2) == 0)
{
motorReference -= 1; //w lewo
}
if((PINA & 1) == 0)
{
stop_motor(); //stop
}
if((PINA & 2) == 0)
{
go_right(); //w prawo
}
rprintf("M:%x", measurementValue);
LCDGotoXY(0,1);
rprintf("I:%x", inputValue);
LCDGotoXY(7,0);
rprintf("E:%x", motorReference);
LCDGotoXY(7,1);
rprintf("P:%x", change);
}
}
Regulator PID:
Standardowy regulator PID z noty katalogowej Atmela AVR221.