Facebook
From Little Rhinoceros, 5 Months ago, written in Plain Text.
Embed
Download Paste or View Raw
Hits: 59
  1. #include <AVR/io.h>
  2. #include <AVR/interrupt.h>
  3. #include <util/delay.h>
  4.  
  5. #define P4 PC0
  6. #define P3 PC1
  7. #define P2 PC2
  8. #define P1 PC3
  9. #define L1 PC4
  10. #define L2 PC5
  11. #define L3 20U
  12. #define L4 21U
  13. #define L_PWM PD6
  14. #define P_PWM PB2
  15. #define PM PD0
  16. #define LM PD5
  17. int8_t Tablica_czujnikow[8] = { P4,P3,P2,P1,L1,L2,L3,L4 };
  18. int8_t Tablica_wag[8] = { 10,7,3,1,-1,-3,-7,10 };
  19. int8_t analog2digital(int8_t Pin);
  20. float wartosc_odczytana(int8_t tab_czuj[8], int8_t tab_wag[8])
  21. {
  22.         int8_t odczyt = 0;
  23.         int8_t il_czuj = 0;
  24.         for (int8_t i = 0; i < 8; i++)
  25.         {
  26.                 if (analog2digital(tab_czuj[i]) < 150)
  27.                 {
  28.                         il_czuj++;
  29.                         odczyt += tab_wag[i];
  30.                 }
  31.         }
  32.         if (il_czuj = 0) return 0;
  33.         else
  34.         {
  35.                 return odczyt / il_czuj;
  36.         }
  37. }
  38. volatile int16_t var_kierunkowe = 130;
  39. ISR(TIMER0_COMPA_vect) // LEWY PWM // tutaj mozecie wrzucic pida dla lewgo pwm
  40. {
  41.  
  42. }
  43. ISR(TIMER2_OVF_vect) // PRAWY PWM // tutaj mozecie wrzucic pida dla prawego pwm
  44. {
  45.  
  46. }
  47. int main(void)
  48. {
  49.         Serial.begin(9600);
  50.         DDRC &= ~(1 << P4);
  51.         DDRC &= ~(1 << P3);
  52.         DDRC &= ~(1 << P2);
  53.         DDRC &= ~(1 << P1);
  54.         DDRC &= ~(1 << L1);
  55.         DDRC &= ~(1 << L2);
  56.         DDRC &= ~(1 << L3);
  57.         DDRC &= ~(1 << L4);
  58.         // Silnik
  59.         DDRD |= (1 << L_PWM); // lewy pwm
  60.         DDRB |= (1 << P_PWM); // prawy pwm
  61.         DDRB |= (1 << PM); // prawy silnik przod
  62.         DDRD |= (1 << LM); // lewy silnik przod
  63.         // ADC
  64.         ADCSRA |= ((1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0));
  65.         ADMUX |= (1 << REFS0); // poczytaj note
  66.         ADCSRA |= (1 << ADEN);
  67.         ADCSRA |= (1 << ADSC);
  68.         //PWM
  69.         TCCR0A |= (1 << COM0A1) | (1 << WGM01) | (1 << WGM00);
  70.         TIMSK0 |= (1 << TOIE0);
  71.         TCCR2A |= (1 << COM2B1) | (1 << WGM21) | (1 << WGM20);
  72.         TIMSK2 |= (1 << TOIE2);
  73.         OCR2B = 0; //prawy pwm
  74.         OCR0A = 0; //lewy pwm
  75.         sei();
  76.         TCCR0B |= (1 << CS00);
  77.         TCCR2B |= (1 << CS20);
  78.  
  79.         while (1)
  80.         {
  81.                 float kierunek = wartosc_odczytana(Tablica_czujnikow, Tablica_wag);
  82.                 if (kierunek<5 && kierunek>-5)
  83.                 {
  84.                         PORTB|=(1 << PM);
  85.                         PORTD |= (1 << LM);
  86.                         OCR0A = var_kierunkowe;
  87.                         OCR2B = var_kierunkowe;
  88.  
  89.                 }
  90.                 if (kierunek > 5)
  91.                 {
  92.                         PORTB |= (1 << PM);
  93.                         PORTD |= (0 << LM);
  94.                         OCR0A = var_kierunkowe+30;
  95.                         OCR2B = 30;
  96.                 }
  97.                 if (kierunek < -5)
  98.                 {
  99.                         PORTB |= (0 << PM);
  100.                         PORTD |= (1 << LM);
  101.                         OCR0A = 30;
  102.                         OCR2B = var_kierunkowe + 30;
  103.                 }
  104.  
  105.  
  106.         }
  107. }
  108. int8_t analog2digital(int8_t Pin)
  109. {
  110.         ADMUX &= 0xF0;
  111.         ADMUX |= Pin;
  112.         ADCSRA |= (1 << ADSC);
  113.         ADCSRA |= (1 << ADSC);
  114.         while (ADCSRA & (1 << ADSC));
  115.         return ADCW;
  116. }
  117.  
  118.