Controlling DC-Motor from incremental Encoder pulse signals using ATmega88, C code

#include "iom88.h"
#include "main.h"

 unsigned int Test_Speed_low;       //For testing only!!
 unsigned int Test_Speed_1;       //For testing only!!
 unsigned int Test_Speed_2;       //For testing only!!
 unsigned int Test_Value_Timer_2;       //For testing only!!

int main(void)
{
  // 1 rpm    ->    1000 / 60 = 16,6 Pulse/s  -> 1000 / 16,6 = 60,25 ms/Pulse
  // 10 rpm   ->   10000 / 60 = 166  Pulse/s  -> 1000 / 166  = 6,025 ms/Pulse
  // 120 rpm  ->  120000 / 60 = 2000 Pulse/s  -> 1000 / 2000 = 0,5   ms/Pulse
  // 2000 rpm -> 2000000 / 60 = 33333 Pulse/s -> 1000 / 33333 = 0,03 ms/Pulse
  // 8 Mhz -> 0,125 us / Takt
  // Timer1 CLK/8 -> 1 us / Timer cycle -> 65535 * 1 us = 62 ms -> ~ 1 rpm
  // If pulse is faster than 0.5 ms then measure 10 im pulses with
  // Timer1 CLK/1 -> 0,125 us / Timer cycle -> 65535 * 0,125 us = 8192 us = 8 ms
  // 120 rpm  ->  120000 / 60 = 2000 Pulse/s  -> 10 Pulse = 5 ms -> 5000 us / 0,125 us = 40000 Timerticks
  // 2000 rpm -> 2000000 / 60 = 33333 Pulse/s -> 10 Pulse = 0,3 ms -> 300 us / 0,125 us = 2400 Timerticks
      
  PORTB = 0x00;           // No Pullups
  DDRB = 0x03;            // PORTB_Bit0 = 1k3, Bit1 = 0R

  PORTC = 0;
  DDRC = 0x1F;            // Out_0-4 Outputs 47k, 22k, 11k, 5k6, 2k7

  PORTD = 0x20;           // Pull-Up T1 (Bit5)
  DDRD = 0x00;                               
 
  ACSR = 0x80;            // Disable Analog Comparator
  DIDR1 = 0x03;           // Disable input register on AIN0/AIN1
  ADCSRA = 0xC7;          // Clock / 128 slower is not possible, no INT
  ADMUX = 0xC5;           // Int 1,1V Ref, ADC5
  DIDR0 = 0x20;           // Disable input register on ADC5
 
  TCCR0A = 0x02;          // T0 CRC mode
  TCCR0B = 0x06;          // T0 External clock source on falling edge
  OCR0A = 1;              // CRC int after 1 clocktick
  TIMSK0 = 0x02;          // Timer 0 Compare match A int

  TCCR1A = 0x00;          // T1 normal mode
  TCCR1B = 0x02;          // clk/8 source on T1
  TIMSK1 = 0x01;          // Timer 1 Overflow int
  Value_TCCR1B = TCCR1B;
  Value_Timer1 = 60000;
 
  Min_Timer1 = 60000;
  Max_Timer1 = 0;
  Counter_Timer1 = 0;
 
  Test_Speed_1 = 0;       //For testing only!!
  Test_Speed_2 = 0;       //For testing only!!
  Test_Speed_low = 0;       //For testing only!!
 
  EAL = 1;   
    
  while(1)
  {
    if (TCCR1B == 0x02) {
      if (Value_Timer1 <= 500) {   // < 0,5 ms / Pulse
        OCR0A = 8;                 // CRC int after 8 clocktick
        TCNT0 = 0;                // Reset timer 0 value
        TCCR1B = 0;               // stop T1
        TCNT1 = 0;                // Reset timer 1 value
        TCCR1B = 1;               // clk/1 source on T1
        Value_TCCR1B = TCCR1B;
      }
    } else {   
      if (Value_Timer1 > 40000) {  // > 0,5 ms / 10 Pulse     //Slow Mode
        OCR0A = 1;                // CRC int after 1 clocktick
        TCNT0 = 0;                // Reset timer 0 value
        TCCR1B = 0;               // stop T1
        TCNT1 = 0x1FF;
        TCCR1B = 2;               // clk/8 source on T1
        Value_TCCR1B = TCCR1B;
      }
    }
    
    if (Counter_Timer1 > 2) {
     if (Value_Timer1 < Min_Timer1) {
        Min_Timer1 = Value_Timer1;
      }
      if (Value_Timer1 > Max_Timer1) {
        Max_Timer1 = Value_Timer1;
      }
      if (Counter_Timer1 > 1000) {
        Counter_Timer1 = 0;
        Min_Timer1 = 60000;
        Max_Timer1 = 0;
      }
    }     

    if (TCCR1B == 0x02)
    {                     
      //Speed = 60000 / (Value_Timer1 / 10); // 0,1 RPM,  at 60000 uS -> 1 RPM
      
      Test_Value_Timer_2 = (Value_Timer1/10);         //For testing
      Speed = (60000/Test_Value_Timer_2); // 0,1 RPM,  at 60000 uS -> 1 RPM
      Test_Speed_low = Speed;
    }
    else
    {
      if (Value_Timer1 > 10000)
      {
        //Speed = 48000 / (Value_Timer1 / 100); // 1 RPM,  at 5000 uS -> 120 RPM
        
        Test_Value_Timer_2 = (Value_Timer1/100);      //For Testing
        Speed = (48000/Test_Value_Timer_2); // 1 RPM,  at 5000 uS -> 120 RPM
        Test_Speed_1 = Speed;           //For testing only!!
      }
      else
      {
        //Speed = 48000 / (Value_Timer1 / 10);  // 1 RPM,  at 5000 uS -> 120 RPM  ... 120RPM????
        //Speed *= 10;
        
        Test_Value_Timer_2 = (Value_Timer1/10);         //For testing
        Speed = (48000/Test_Value_Timer_2);  // 1 RPM,  at 5000 uS -> 120 RPM
        Speed *= 10;
        Test_Speed_2 = Speed;       //For testing only!!
      }
    }
  }
}

#pragma vector=INT0_vect

__interrupt void INT0_int(void)   
{
}

#pragma vector=INT1_vect

__interrupt void INT1_int(void)   
{
}

#pragma vector=TIMER0_COMPA_vect

__interrupt void TIMER0_COMPA_int(void)
{
  TCCR1B = 0;                   // Timer1 Stop
  Value_Timer1 = TCNT1;
  TCNT1 = 0;                    // Reset timer 1 value
  TCCR1B = Value_TCCR1B;
  Counter_Timer1++;   
  Counter_Int = 1;
}

#pragma vector=TIMER1_OVF_vect

__interrupt void TIMER1_OVF_int(void)
{
 if (Timer1_OVF == 2)           // Speed < 1,0 RPM
 {   
   Speed = 0;
 }
 else
 {
    Timer1_OVF++;
 }
}