Subversion Repositories BL-Ctrl

/*############################################################################
 + Regler für Brushless-Motoren
 + ATMEGA8 mit 8MHz
 + (c) 01.2007 Holger Buss
 + Nur für den privaten Gebrauch
 + Keine Garantie auf Fehlerfreiheit
 + Kommerzielle Nutzung nur mit meiner Zustimmung
 + Der Code ist für die Hardware BL_Ctrl V1.0 entwickelt worden
 + www.mikrocontroller.com
/############################################################################*/

#include "main.h"

unsigned int  PWM = 0;
unsigned int  Strom = 0; //ca. in 0,1A
unsigned char Strom_max = 0;
unsigned char Mittelstrom = 0;
unsigned int  Drehzahl = 0;  // in 100UPM  60 = 6000
unsigned int  KommutierDelay = 10;
unsigned int  I2C_Timeout = 0;
unsigned int SIO_Timeout = 0;
unsigned int  SollDrehzahl = 0;
unsigned int  IstDrehzahl = 0;
unsigned int  DrehZahlTabelle[256];//vorberechnete Werte zur Drehzahlerfassung
unsigned char ZeitFuerBerechnungen = 1;
unsigned char MotorAnwerfen = 0;
unsigned char MotorGestoppt = 1;
unsigned char MaxPWM = MAX_PWM;
unsigned int  CntKommutierungen = 0;
unsigned int  SIO_Drehzahl = 0;
unsigned char ZeitZumAdWandeln = 1;
unsigned char MotorAdresse = 1;
unsigned char PPM_Betrieb = 1;

//############################################################################
//
void SetPWM(void)
//############################################################################
{
    unsigned char tmp_pwm;
    tmp_pwm = PWM;
    if(tmp_pwm > MaxPWM)    // Strombegrenzung
        {
        tmp_pwm = MaxPWM;
        PORTC |= ROT;
        }
    if(Strom > MAX_STROM)   // Strombegrenzung
        {
        OCR1A = 0; OCR1B = 0; OCR2  = 0;
        PORTC |= ROT;
        Strom--;
        }
    else
        {
        #ifdef  _32KHZ
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
        #endif

        #ifdef  _16KHZ
        //OCR1A = 2 * (int)tmp_pwm; OCR1B = 2 * (int)tmp_pwm; OCR2  = tmp_pwm;
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
        #endif
        }
}

//############################################################################
//
void PWM_Init(void)
//############################################################################
{
    PWM_OFF;
    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (0 << WGM12) |
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
/*    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (1 << WGM12) |
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
*/
}

//############################################################################
//
void Wait(unsigned char dauer)
//############################################################################
{
    dauer = (unsigned char)TCNT0 + dauer;
    while((TCNT0 - dauer) & 0x80);
}

//############################################################################
//
void Anwerfen(unsigned char pwm)
//############################################################################
{
    unsigned long timer = 300,i;
    DISABLE_SENSE_INT;
    PWM = 5;
    SetPWM();
    Manuell();
    Delay_ms(200);
    PWM = pwm;
    while(1)
        {
        for(i=0;i<timer; i++)
            {
            if(!UebertragungAbgeschlossen)  SendUart();
            else DatenUebertragung();
            Wait(100);  // warten
            }
        timer-= timer/15+1;
        if(timer < 25) { if(TEST_MANUELL) timer = 25; else return; }

        Manuell();
        Phase++;
        Phase %= 6;
        AdConvert();
        PWM = pwm;
        SetPWM();
        if(SENSE)
            {
            PORTD ^= GRUEN;
            }
        }
}

/*
#define SENSE_A ADMUX = 0;
#define SENSE_B ADMUX = 1;
#define SENSE_C ADMUX = 2;

#define ClrSENSE            ACSR |= 0x10
#define SENSE               ((ACSR & 0x10))
#define SENSE_L             (!(ACSR & 0x20))
#define SENSE_H             ((ACSR & 0x20))
*/

void RotBlink(unsigned char anz)
{
sei(); // Interrupts ein
 while(anz--)
  {
   PORTC |= ROT;
   Delay_ms(300);    
   PORTC &= ~ROT;
   Delay_ms(300);    
  }
   Delay_ms(1000);    
}

#define TEST_STROMGRENZE 100
unsigned char DelayM(unsigned int timer)
{
 while(timer--)
  {
   FastADConvert();
   if(Strom > TEST_STROMGRENZE)
       {
        STEUER_OFF;
        return(1);
       }
  }
 return(0);  
}

unsigned char Delay(unsigned int timer)
{
 while(timer--)
  {
  }
 return(0);  
}


void MotorTon(void)
//############################################################################
{
    unsigned int timer = 300,i;
    unsigned int t;
    unsigned char MosfetKurzschluss = 0, MosfetOkay = 0,anz = 0;
    DISABLE_SENSE_INT;
    cli();//Globale Interrupts Ausschalten
    PORTC &= ~ROT;
    PWM = 0;
    SetPWM();
    uart_putchar('\n');
    STEUER_OFF;
//#define  STEUER_A_L {PORTD &= ~0x30; PORTD |= 0x08;}
//#define  STEUER_B_L {PORTD &= ~0x28; PORTD |= 0x10;}
//#define  STEUER_C_L {PORTD &= ~0x18; PORTD |= 0x20;}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ High-Mosfets auf Kurzschluss testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Strom = 0;
    PORTD |= 0x08; // Low  A ein
    PORTB |= 0x04; // High B ein; und A gegen B
//PORTB |= 0x08; // Test A
    if(DelayM(3))
       {
        MosfetKurzschluss = 0x01;
        anz = 1;
        uart_putchar('A');
       }
    STEUER_OFF;
    DelayM(100);
    Strom = 0;
    PORTD |= 0x10; // Low B ein
    PORTB |= 0x02; // High C ein
//PORTB |= 0x04; // Test B
    if(DelayM(3))
       {
        anz = 2;
        MosfetKurzschluss = 0x02;
        uart_putchar('B');
       }
    STEUER_OFF;
    DelayM(100);
    Strom = 0;
    PORTD |= 0x20; // Low C ein
    PORTB |= 0x04; // High B ein
    //PORTB |= 0x0C; // High A+B ein
//PORTB |= 0x02; // Test C
    if(DelayM(3))
       {
        anz = 3;
        MosfetKurzschluss = 0x04;
        uart_putchar('C');
       }
    STEUER_OFF;
    DelayM(100);
    PORTD |= 0x08; // Low  A ein; und A gegen C
    PORTB |= 0x02; // High C ein
//PORTB |= 0x08; // Test A
    if(DelayM(3))
       {
        anz = 3;
        MosfetKurzschluss = 0x01;
        uart_putchar('C');
       }
    STEUER_OFF;
    DelayM(100);
 if(anz) while(1) RotBlink(anz);
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Low-Mosfets auf Kurzschluss testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    Strom = 0;
    PORTB |= 0x08; // Test A
//PORTD |= 0x08; // Low A ein  
    if(DelayM(3))
       {
        anz = 4;
        MosfetKurzschluss = 0x01;
        uart_putchar('a');
       }
    STEUER_OFF;
    Strom = 0;
    PORTB |= 0x04; // Test B
//PORTD |= 0x10; // Low B ein
    if(DelayM(3))
       {
        anz = 5;
        MosfetKurzschluss = 0x02;
        uart_putchar('b');
       }
    STEUER_OFF;
    Strom = 0;
    PORTB |= 0x02; // Test C
//PORTD |= 0x20; // Low C ein
    if(DelayM(3))
       {
        anz = 6;
        MosfetKurzschluss = 0x04;
        uart_putchar('c');
       }
    STEUER_OFF;

 if(anz) while(1) RotBlink(anz);

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ High-Mosfets auf Schalten testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    SENSE_A;
    STEUER_OFF;
    PORTD |= 0x10; // Low B ein
    PORTD |= 0x20; // Low C ein
    Strom = 0;
#define TONDAUER  100000    
#define SOUND_E 5
#define SOUND2_A 500
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
     {
      PORTB |= 0x08; // Test A
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x01; } else { MosfetOkay &= ~0x01;}
      PORTB = 0;
      Delay(SOUND2_A);
     }
    STEUER_OFF;
    SENSE_B;
    PORTD |= 0x08; // Low A ein
    PORTD |= 0x20; // Low C ein
#define SOUND1_A 600
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
     {
      PORTB |= 0x04; // Test B
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x02; } else { MosfetOkay &= ~0x02;}
      PORTB = 0;
      Delay(SOUND1_A);
     }

    STEUER_OFF;
    SENSE_C;
    PORTD |= 0x08; // Low A ein
    PORTD |= 0x10; // Low B ein
#define SOUND3_A  700
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
     {
      PORTB |= 0x02; // Test C
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x04; } else { MosfetOkay &= ~0x04;}
      PORTB = 0;
      Delay(SOUND3_A);
     }
    STEUER_OFF;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Low-Mosfets auf Schalten testen
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    SENSE_B;
    PORTD |= 0x08; // Low A ein
#define SOUND2_A 500
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
     {
      PORTB |= 0x04; // Test B
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x08; } else { MosfetOkay &= ~0x08;}    
      PORTB = 0;
      Delay(SOUND2_A);
     }
 
    STEUER_OFF;
    SENSE_C;
    PORTD |= 0x10; // Low B ein
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
     {
      PORTB |= 0x02; // Test C
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x10; } else { MosfetOkay &= ~0x10;}    
      PORTB = 0;
      Delay(SOUND3_A);
     }

    STEUER_OFF;
    SENSE_B;
    PORTD |= 0x20; // Low C ein
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
     {
      PORTB |= 0x04; // Test B
      Delay(SOUND_E);
      if(SENSE_L) { MosfetOkay |= 0x20; } else { MosfetOkay &= ~0x20;}      
      PORTB = 0;
      Delay(SOUND1_A);
     }

    sei();//Globale Interrupts Einschalten
    Delay_ms(250 * MotorAdresse);    

#define SOUND8_E 7
#define SOUND8_A 650
    for(i=0; i<(TONDAUER / SOUND8_A); i++)
     {
      for(timer = 0; timer < SOUND8_E; timer++) PORTB |= 0x04; // Test B
      for(timer = 0; timer < SOUND8_A; timer++) PORTB = 0;
     }

 if(!(MosfetOkay & 0x01))  { anz = 1; uart_putchar('A'); } else
 if(!(MosfetOkay & 0x02))  { anz = 2; uart_putchar('B'); } else
 if(!(MosfetOkay & 0x04))  { anz = 3; uart_putchar('C'); } else
 if(!(MosfetOkay & 0x08))  { anz = 4; uart_putchar('a'); } else
 if(!(MosfetOkay & 0x10))  { anz = 5; uart_putchar('b'); } else
 if(!(MosfetOkay & 0x20))  { anz = 6; uart_putchar('c'); }  
 Delay_ms(250 * (4-MotorAdresse));    

 if(MosfetOkay != 0x3f) Delay_ms(1000);

#define TONDAUER  80000    
#define SOUND9_E 20
#define SOUND9_A 500

 while(anz--)
  {
   PORTC |= ROT;
    if(!(MosfetOkay & 0x04) || !(MosfetOkay & 0x20))  // Schalter C defekt
     {
     PORTD |= 0x10; // Low B ein
     for(i=0; i< (TONDAUER / SOUND9_A) ; i++)
      {
       for(timer = 0; timer < SOUND9_E; timer++) PORTB |= 0x08; // Test A
       for(timer = 0; timer < SOUND9_A; timer++) PORTB = 0;
      }
     }
    else
    if(!(MosfetOkay & 0x02) || !(MosfetOkay & 0x10)) // Schalter B defekt
     {
     PORTD |= 0x20; // Low C ein
     for(i=0; i< (TONDAUER / SOUND9_A) ; i++)
      {
       for(timer = 0; timer < SOUND9_E; timer++) PORTB |= 0x08; // Test A
       for(timer = 0; timer < SOUND9_A; timer++) PORTB = 0;
      }
     }
    else
    if(!(MosfetOkay & 0x01) || !(MosfetOkay & 0x08)) // Schalter A defekt
     {
     PORTD |= 0x20; // Low C ein
     for(i=0; i< (TONDAUER / SOUND9_A) ; i++)
      {
       for(timer = 0; timer < SOUND9_E; timer++) PORTB |= 0x04; // Test B
       for(timer = 0; timer < SOUND9_A; timer++) PORTB = 0;
      }
     }
   PORTC &= ~ROT;
   Delay_ms(300);    
  }
}

//############################################################################
//
unsigned char SollwertErmittlung(void)
//############################################################################
{
    static unsigned int sollwert = 0;
    unsigned int ppm;
    if(!I2C_Timeout)   // bei Erreichen von 0 ist der Wert ungültig
        {
        if(SIO_Timeout)  // es gibt gültige SIO-Daten
            {
             sollwert =  (MAX_PWM * (unsigned int) SIO_Sollwert) / 200;  // skalieren auf 0-200 = 0-255
             PPM_Betrieb = 0;
             ICP_INT_DISABLE;
             PORTC &= ~ROT;
            }
        else
            if(anz_ppm_werte > 20)  // es gibt gültige PPM-Daten
                {
                PPM_Betrieb = 1;
                ppm = PPM_Signal;
                if(ppm > 300) ppm =   0;  // ungültiges Signal
                if(ppm > 200) ppm = 200;
                if(ppm <= MIN_PPM) sollwert = 0;
                else
                    {
                    sollwert = (int) MIN_PWM + ((MAX_PWM - MIN_PWM) * (ppm - MIN_PPM)) / (190 - MIN_PPM);
                    }
                PORTC &= ~ROT;
                }
            else   // Kein gültiger Sollwert
                {
                 if(!TEST_SCHUB) { if(sollwert) sollwert--; }  
                 PORTC |= ROT;
                }
        }
    else // I2C-Daten sind gültig
        {
        sollwert = I2C_RXBuffer;
        PPM_Betrieb = 0;
        PORTC &= ~ROT;
        ICP_INT_DISABLE;
        }
    if(sollwert > MAX_PWM) sollwert = MAX_PWM;
    return(sollwert);
}

void DebugAusgaben(void)
{
    DebugOut.Analog[0] = Strom;
    DebugOut.Analog[1] = Mittelstrom;
    DebugOut.Analog[2] = SIO_Drehzahl;
    DebugOut.Analog[3] = PPM_Signal;
}


//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
    char altPhase = 0;
    int test = 0;
    unsigned int MinUpmPulse,Blink,TestschubTimer;
    unsigned int Blink2,MittelstromTimer,DrehzahlMessTimer,MotorGestopptTimer;

    DDRC  = 0x08;
    PORTC = 0x08;
    DDRD  = 0xBA;
    PORTD = 0x00;
    DDRB  = 0x0E;
    PORTB = 0x31;
       
#if (MOTORADRESSE == 0)
    PORTB |= (ADR1 + ADR2);   // Pullups für Adresswahl
    for(test=0;test<500;test++);
    if (PINB & ADR1)
         {
           if (PINB & ADR2) MotorAdresse = 1;
            else MotorAdresse = 2;
         }
         else
         {
           if (PINB & ADR2) MotorAdresse = 3;
            else MotorAdresse = 4;
         }
#else
    MotorAdresse  = MOTORADRESSE;
#endif
   
    UART_Init();
    Timer0_Init();
    sei();//Globale Interrupts Einschalten
   
    // Am Blinken erkennt man die richtige Motoradresse
/*
    for(test=0;test<5;test++)
        {
        if(test == MotorAdresse) PORTD |= GRUEN;
        Delay_ms(150);
        PORTD &= ~GRUEN;
        Delay_ms(250);
        }      

    Delay_ms(500);
*/  
   // UART_Init();  // war doppelt
    PWM_Init();

    InitIC2_Slave(0x50);                           
    InitPPM();

    Blink             = SetDelay(101);    
    Blink2            = SetDelay(102);
    MinUpmPulse       = SetDelay(103);
    MittelstromTimer  = SetDelay(254);
    DrehzahlMessTimer = SetDelay(1005);
    TestschubTimer    = SetDelay(1006);
    while(!CheckDelay(MinUpmPulse))
    {
     if(SollwertErmittlung()) break;
    }
    ;
    PORTD |= GRUEN;
    PWM = 0;

    SetPWM();

    SFIOR = 0x08;  // Analog Comperator ein
    ADMUX = 1;

    MinUpmPulse = SetDelay(10);
    DebugOut.Analog[1] = 1;
    PPM_Signal = 0;

    if(!SollwertErmittlung()) MotorTon();

    // zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
    if(TEST_MANUELL)    Anwerfen(TEST_MANUELL);  // kommt von dort nicht wieder

    while (1)
        {
        if(!TEST_SCHUB)   PWM = SollwertErmittlung();
        //I2C_TXBuffer = PWM; // Antwort über I2C-Bus
        if(MANUELL_PWM)   PWM = MANUELL_PWM;

        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(Phase != altPhase)   // es gab eine Kommutierung im Interrupt
            {
            MotorGestoppt = 0;
            ZeitFuerBerechnungen = 0;    // direkt nach einer Kommutierung ist Zeit
            MinUpmPulse = SetDelay(50);  // Timeout, falls ein Motor stehen bleibt
            altPhase = Phase;
            }
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(!PWM)    // Sollwert == 0
            {
            MotorAnwerfen = 0;      // kein Startversuch
            ZeitFuerBerechnungen = 0;
            // nach 1,5 Sekunden den Motor als gestoppt betrachten
            if(CheckDelay(MotorGestopptTimer))
                {
                DISABLE_SENSE_INT;
                MotorGestoppt = 1;  
                STEUER_OFF;
                }
            }
        else
            {
            if(MotorGestoppt) MotorAnwerfen = 1;        // Startversuch
            MotorGestopptTimer = SetDelay(1500);
            }

        if(MotorGestoppt && !TEST_SCHUB) PWM = 0;
        SetPWM();
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        if(!ZeitFuerBerechnungen++)
            {
            if(MotorGestoppt) PORTD |= GRUEN; //else PORTD &= ~GRUEN;
            if(SIO_DEBUG)
                {
                DebugAusgaben();  // welche Werte sollen angezeigt werden?
                if(!UebertragungAbgeschlossen)  SendUart();
                else DatenUebertragung();
                }
            // Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
            if(CheckDelay(MittelstromTimer))  
                {
                MittelstromTimer = SetDelay(50); // alle 50ms
                if(Mittelstrom <  Strom) Mittelstrom++;// Mittelwert des Stroms bilden
                else if(Mittelstrom >  Strom) Mittelstrom--;
       
                if(Mittelstrom > LIMIT_STROM)// Strom am Limit?
                    {
                    MaxPWM--;// dann die Maximale PWM herunterfahren
                    PORTC |= ROT;
                    }
                else
                    {
                    if(MaxPWM < MAX_PWM) MaxPWM++;
                    }
                }

            if(CheckDelay(DrehzahlMessTimer))   // Ist-Drehzahl bestimmen
                {
                DrehzahlMessTimer = SetDelay(10);
                SIO_Drehzahl = (6 * CntKommutierungen) / (POLANZAHL / 2);
                CntKommutierungen = 0;
                if(PPM_Timeout == 0) // keine PPM-Signale
                ZeitZumAdWandeln = 1;
                }


            if(CheckDelay(TestschubTimer))  
                {
                TestschubTimer = SetDelay(1500);
                if(TEST_SCHUB)
                    {
                    switch(test)
                        {
                        case 0: PWM = 50; test++; break;
                        case 1: PWM = 130; test++; break;
                        case 2: PWM = 60;  test++; break;
                        case 3: PWM = 140; test++; break;
                        case 4: PWM = 150; test = 0; break;
                        default: test = 0;
                        }
                    }
                }
            // Motor Stehen geblieben
            if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen)
                {
                MotorGestoppt = 1;    
                DISABLE_SENSE_INT;
                MinUpmPulse = SetDelay(100);        
                if(MotorAnwerfen)
                    {
                    PORTC &= ~ROT;
                    MotorAnwerfen = 0;
                    Anwerfen(10);
                    PORTD |= GRUEN;
                    MotorGestoppt = 0;    
                    Phase--;
                    PWM = 1;
                    SetPWM();
                    SENSE_TOGGLE_INT;
                    ENABLE_SENSE_INT;
                    MinUpmPulse = SetDelay(20);
                    while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
                    PWM = 15;
                    SetPWM();
                    MinUpmPulse = SetDelay(300);
                    while(!CheckDelay(MinUpmPulse)); // kurz Durchstarten
                   
                                    // Drehzahlmessung wieder aufsetzen
                    DrehzahlMessTimer = SetDelay(50);
                    altPhase = 7;
                    }
                }
            } // ZeitFuerBerechnungen
        } // while(1) - Hauptschleife
}