[syntax=c]/*
*
* Author: marshall94
*/
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
//dioda testowa
#define LED_PIN (1<<PB2)
#define LED_OFF PORTB &= ~LED_PIN
#define LED_ON PORTB |= LED_PIN
//czujnik
#define PORT PORTC
#definePINPINC
#define DDR DDRC
#define TRIGGERPC0 //Trigger
#define ECHOPC1 //Echo
#define BLAD_POMIARU-1
#defineBRAK_PRZESZKODY-2
//serwo
#define LEWO_MAX 2280
#define PRAWO_MAX 3580
#define SRODEK 2930
//DEKLARACJA FUNKCJI------------------
//czujnik
void czujnik_init();
void trigger();
uint16_t pomiar();
//dioda testowa
void dioda_init();
//serwo
void serwo_init();
uint16_t serwo(uint8_t kat);
//regulator
uint8_t regulator(uint8_t w_zad, uint8_t pomiar);
//------------------------------------
int main(void)
{
//*********INICJALIZACJA***********
dioda_init();//dioda testowa
czujnik_init();//czujnik
serwo_init();
//*********************************
//***********ZMIENNE***************
uint16_t r;
uint8_t d;
uint8_t w_zad = 32;
uint8_t pid_out;
//*********************************
while(1)
{
trigger();//ROZPOCZECIE POMIARU
r=pomiar();//OBLICZANIE POMIARU
if (r>=0)//JEZELI NIE WYKRYTO BLEDU
{
d=(r/58.0);//KONWERSJA POMIARU NA CM
}
if (d>=0 && d<=80)
{
LED_ON;
pid_out = regulator(w_zad, d);//WYJSCIE Z REGULATORA
OCR1A = serwo(pid_out);//WEJSCIE DO SERWA
_delay_ms(50);
}
else LED_OFF;
}
}
void dioda_init()
{
DDRB |= LED_PIN;
}
void czujnik_init()
{
DDR|=(1<<TRIGGER);
}
void trigger()
{
//Impuls 10us
PORT|=(1<<TRIGGER);//WYSOKI
_delay_us(15);//Czekaj 15us
PORT&=~(1<<TRIGGER);//NISKI
}
uint16_t pomiar()
{
uint32_t i,result;
//Oczekiwanie na zbocze narastajace
for(i=0;i<600000;i++)
{
if(!(PIN & (1<<ECHO)))
continue;//Stan niski
else
break;//Wykryto zbocze narastajace
}
if(i==600000)
return BLAD_POMIARU;//Przekroczony czas oczekiwania
//Wykryto stan wysoki
//Inicjalizacja Timer1
TCCR1A=0X00;
TCCR1B=(1<<CS11);//Prescaler = Fcpu/8
TCNT1=0x00;//Init counter
//Oczekiwanie na zbocze opadajace
for(i=0;i<600000;i++)
{
if(PIN & (1<<ECHO))
{
if(TCNT1 > 60000) break; else continue;
}
else
break;
}
if(i==600000)
return BRAK_PRZESZKODY;//Przekroczony czas oczekiwania
//Wykryte zbocze narastajace
result=TCNT1;
//Stop Timer
TCCR1B=0x00;
if(result > 60000)
return BRAK_PRZESZKODY;//Brak przeszkody
else
return (result>>1);
}
uint8_t regulator(uint8_t w_zad, uint8_t pomiar)
{
//nastawy
float k = 0.25;
float k_i = 0.1;
float k_d = 0.08;
//zmienne pomocnicze
float p,i,d;//czlony regulatora
int wyjscie;
float u;//uchyb regulacji
static float u_p = 0;//poprzedni uchyb regulacji
static float su = 0;//suma minionych uchybow regulacji
//aktualny uchyb regulacji
u = w_zad - pomiar;
//skladnik proporcjonalny
p = k*u;
//skladnik calkowy
su=su+u;
i = k_i * su;
//skladnik rozniczkowych
d = k_d*(u-u_p);
u_p = u;
//OBLICZANIE PID
wyjscie = -(p+i+d)+90; //90 - rynna poziomo,
return wyjscie;
}
uint16_t serwo(uint8_t kat)
{
uint16_t wychylenie;
if(kat>121) kat=121;
if(kat<58) kat = 58;
//normalizacja
wychylenie = (int)(1300/63)*(kat-58)+2280;
return wychylenie;
}
void serwo_init()
{
DDRB |= (1 << PB1); // set output to PB1
TCCR1A = ((1 << COM1A1) | (1 << WGM11)); // clear OC1A on match + WGM mode 14
TCCR1B = ((1 << WGM12) | (1 << WGM13) | (1 << CS11)); // WGM mode 14 (Fast PWM) // and 8x prescaler
ICR1 = 40000; //set ICR1 to produce 50Hz frequency //(8000000 / 8 / 20000 = 50hz)
}[/syntax]Statystyki: Napisane przez marshall94 — 23 kwi 2017, o 02:24
]]>