[Atmega8][C] DC Serwomotor driver dla Mach3

#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);

   

  }

}