Subversion Repositories BL-Ctrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
66 jan-hendri 1
/*#######################################################################################
2
Flight Control
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Regler für Brushless-Motoren
6
// + ATMEGA8 mit 8MHz
7
// + Nur für den privaten Gebrauch
8
// + Copyright (c) 12.2007 Holger Buss
9
// + www.MikroKopter.com
10
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), 
12
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. 
13
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt 
14
// + bzgl. der Nutzungsbedingungen aufzunehmen. 
15
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
16
// + Verkauf von Luftbildaufnahmen, usw.
17
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, 
19
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
22
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
23
// + eindeutig als Ursprung verlinkt werden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
26
// + Benutzung auf eigene Gefahr
27
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur 
30
// + mit unserer Zustimmung zulässig
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, 
35
// + this list of conditions and the following disclaimer.
36
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
37
// +     from this software without specific prior written permission.
38
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet 
39
// +     for non-commercial use (directly or indirectly)
40
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted 
41
// +     with our written permission
42
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be 
43
// +     clearly linked as origin 
44
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
53
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
54
// +  POSSIBILITY OF SUCH DAMAGE. 
55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56
 
57
#include "main.h"
58
 
59
unsigned int  PWM = 0;
60
unsigned int  Strom = 0,RuheStrom; //ca. in 0,1A
61
unsigned char Strom_max = 0;
62
unsigned char Mittelstrom = 0;
63
unsigned int  Drehzahl = 0;  // in 100UPM  60 = 6000
64
unsigned int  KommutierDelay = 10;
65
unsigned int  I2C_Timeout = 0;
66
unsigned int SIO_Timeout = 0;
67
unsigned int  SollDrehzahl = 0;
68
unsigned int  IstDrehzahl = 0;
69
unsigned int  DrehZahlTabelle[256];//vorberechnete Werte zur Drehzahlerfassung
70
unsigned char ZeitFuerBerechnungen = 1;
71
unsigned char MotorAnwerfen = 0;
72
unsigned char MotorGestoppt = 1;
73
unsigned char MaxPWM = MAX_PWM;
74
unsigned int  CntKommutierungen = 0;
75
unsigned int  SIO_Drehzahl = 0;
76
unsigned char ZeitZumAdWandeln = 1;
77
unsigned char MotorAdresse = 1;
78
unsigned char PPM_Betrieb = 1;
79
 
80
//############################################################################
81
//
82
void SetPWM(void)
83
//############################################################################
84
{
85
    unsigned char tmp_pwm;
86
    tmp_pwm = PWM;
87
    if(tmp_pwm > MaxPWM)    // Strombegrenzung
88
        {
89
        tmp_pwm = MaxPWM;
90
        PORTC |= ROT;
91
        }
92
    if(Strom > MAX_STROM)   // Strombegrenzung
93
        {
94
        OCR1A = 0; OCR1B = 0; OCR2  = 0;
95
        PORTC |= ROT;
96
        Strom--;
97
        }
98
    else
99
        {
100
        #ifdef  _32KHZ 
101
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
102
        #endif 
103
 
104
        #ifdef  _16KHZ 
105
        //OCR1A = 2 * (int)tmp_pwm; OCR1B = 2 * (int)tmp_pwm; OCR2  = tmp_pwm;
106
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
107
        #endif 
108
        }
109
}
110
 
111
//############################################################################
112
//
113
void PWM_Init(void)
114
//############################################################################
115
{
116
    PWM_OFF;
117
    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (0 << WGM12) |
118
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
119
/*    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (1 << WGM12) |
120
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
121
*/
122
}
123
 
124
//############################################################################
125
//
126
void Wait(unsigned char dauer)
127
//############################################################################
128
{
129
    dauer = (unsigned char)TCNT0 + dauer;
130
    while((TCNT0 - dauer) & 0x80);
131
}
132
 
133
//############################################################################
134
//
135
void Anwerfen(unsigned char pwm)
136
//############################################################################
137
{
138
    unsigned long timer = 300,i;
139
    DISABLE_SENSE_INT;
140
    PWM = 5;
141
    SetPWM();
142
    Manuell();
143
    Delay_ms(200);
144
    PWM = pwm;
145
    while(1)
146
        {
147
        for(i=0;i<timer; i++)
148
            {
149
            if(!UebertragungAbgeschlossen)  SendUart();
150
            else DatenUebertragung();
151
            Wait(100);  // warten
152
            }
153
        timer-= timer/15+1;
154
        if(timer < 25) { if(TEST_MANUELL) timer = 25; else return; }
155
 
156
        Manuell();
157
        Phase++;
158
        Phase %= 6;
159
        AdConvert();
160
        PWM = pwm;
161
        SetPWM();
162
        if(SENSE)
163
            {
164
            PORTD ^= GRUEN;
165
            }
166
        }
167
}
168
 
169
/*
170
#define SENSE_A ADMUX = 0;
171
#define SENSE_B ADMUX = 1;
172
#define SENSE_C ADMUX = 2;
173
 
174
#define ClrSENSE            ACSR |= 0x10
175
#define SENSE               ((ACSR & 0x10))
176
#define SENSE_L             (!(ACSR & 0x20))
177
#define SENSE_H             ((ACSR & 0x20))
178
*/
179
 
180
void RotBlink(unsigned char anz)
181
{
182
sei(); // Interrupts ein
183
 while(anz--)
184
  {
185
   PORTC |= ROT;
186
   Delay_ms(300);    
187
   PORTC &= ~ROT;
188
   Delay_ms(300);    
189
  }
190
   Delay_ms(1000);    
191
}
192
 
193
#define TEST_STROMGRENZE 120
194
unsigned char DelayM(unsigned int timer)
195
{
196
 while(timer--)
197
  {
198
   FastADConvert();
199
   if(Strom > (TEST_STROMGRENZE + RuheStrom))
200
       {
201
        FETS_OFF;
202
        return(1);
203
       }
204
  }
205
 return(0);  
206
}
207
 
208
unsigned char Delay(unsigned int timer)
209
{
210
 while(timer--)
211
  {
212
//   if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
213
  }
214
 return(0);  
215
}
216
 
217
/*
218
void ShowSense(void)
219
{
220
 if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
221
 
222
}
223
*/
224
#define HIGH_A_EIN PORTB |= 0x08
225
#define HIGH_B_EIN PORTB |= 0x04
226
#define HIGH_C_EIN PORTB |= 0x02
227
#define LOW_A_EIN  PORTD |= 0x08
228
#define LOW_B_EIN  PORTD |= 0x10
229
#define LOW_C_EIN  PORTD |= 0x20
230
 
231
void MotorTon(void)
232
//############################################################################
233
{
234
    unsigned char ADR_TAB[5] = {0,0,2,1,3};
235
    unsigned int timer = 300,i;
236
    unsigned int t = 0;
237
    unsigned char anz = 0,MosfetOkay = 0, grenze = 50;
238
 
239
    PORTC &= ~ROT;
240
    Delay_ms(300 * ADR_TAB[MotorAdresse]);    
241
    DISABLE_SENSE_INT;
242
    cli();//Globale Interrupts Ausschalten
243
    uart_putchar('\n');
244
    STEUER_OFF;
245
    Strom_max = 0;
246
    DelayM(50);
247
    RuheStrom = Strom_max;
248
//    uart_putchar(RuheStrom + 'A');
249
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
250
//+ High-Mosfets auf Kurzschluss testen
251
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
252
    Strom = 0;
253
    LOW_B_EIN;
254
    HIGH_A_EIN;
255
    if(DelayM(3))
256
       {
257
        anz = 1;
258
        uart_putchar('1');
259
       }
260
    FETS_OFF;
261
    Delay(1000);
262
    Strom = 0;
263
    LOW_A_EIN;
264
    HIGH_B_EIN;
265
    if(DelayM(3))
266
       {
267
        anz = 2;
268
        uart_putchar('2');
269
       }
270
    FETS_OFF;
271
    Delay(1000);
272
    Strom = 0;
273
    LOW_B_EIN; // Low C ein
274
    HIGH_C_EIN; // High B ein
275
    if(DelayM(3))
276
       {
277
        anz = 3;
278
        uart_putchar('3');
279
       }
280
    FETS_OFF;
281
    Delay(1000);
282
    LOW_A_EIN; // Low  A ein; und A gegen C
283
    HIGH_C_EIN; // High C ein
284
    if(DelayM(3))
285
       {
286
        anz = 3;
287
        uart_putchar('7');
288
       }
289
    FETS_OFF;
290
    DelayM(10000);
291
if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten
292
 
293
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
294
//+ LOW-Mosfets auf Schalten und Kurzschluss testen
295
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
296
 if(UDR == ' ') {t = 65535; grenze = 40; uart_putchar('_');} else t = 1000; // Ausführlicher Test
297
 Strom = 0;
298
 for(i=0;i<t;i++)
299
 {
300
  LOW_A_EIN;
301
  DelayM(1);
302
  FETS_OFF;
303
  Delay(5);
304
  HIGH_A_EIN;
305
  DelayM(1);
306
  FETS_OFF;
307
  if(Strom > grenze + RuheStrom) {anz = 4; uart_putchar('4'); break;}
308
  Delay(5);
309
 }
310
 Delay(10000);
311
 
312
 Strom = 0;
313
 for(i=0;i<t;i++)
314
 {
315
  LOW_B_EIN;
316
  DelayM(1);
317
  FETS_OFF;
318
  Delay(5);
319
  HIGH_B_EIN;
320
  DelayM(1);
321
  FETS_OFF;
322
  if(Strom > grenze + RuheStrom) {anz = 5; uart_putchar('5'); break;}
323
  Delay(5);
324
 }
325
 
326
 Strom = 0;
327
 Delay(10000);
328
 
329
 for(i=0;i<t;i++)
330
 {
331
  LOW_C_EIN;
332
  DelayM(1);
333
  FETS_OFF;
334
  Delay(5);
335
  HIGH_C_EIN;
336
  DelayM(1);
337
  FETS_OFF;
338
  if(Strom > grenze + RuheStrom) {anz = 6; uart_putchar('6'); break;}
339
  Delay(5);
340
 }
341
 
342
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
343
//+ High-Mosfets auf Schalten testen
344
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
345
    SENSE_A;
346
    FETS_OFF;
347
    LOW_B_EIN; // Low B ein
348
    LOW_C_EIN; // Low C ein
349
    Strom = 0;
350
#define TONDAUER  40000    
351
#define SOUND_E 2
352
#define SOUND1_A 300
353
#define SOUND2_A 330
354
#define SOUND3_A 360
355
 
356
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
357
     {
358
      HIGH_A_EIN; // Test A
359
      Delay(SOUND_E);
360
      if(MessAD(0) > 50) { MosfetOkay |= 0x01; } else { MosfetOkay &= ~0x01;};
361
      PORTB = 0;
362
      Delay(SOUND1_A);
363
     }
364
    FETS_OFF;
365
 
366
    LOW_A_EIN; // Low A ein
367
    LOW_C_EIN; // Low C ein
368
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
369
     {
370
      HIGH_B_EIN; // Test B
371
      Delay(SOUND_E);
372
      if(MessAD(1) > 50) { MosfetOkay |= 0x02; } else { MosfetOkay &= ~0x02;};
373
      PORTB = 0;
374
      Delay(SOUND1_A);
375
     }
376
 
377
    FETS_OFF;
378
    LOW_A_EIN; // Low A ein
379
    LOW_B_EIN; // Low B ein
380
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
381
     {
382
      HIGH_C_EIN; // Test C
383
      Delay(SOUND_E);
384
      if(MessAD(2) > 50) { MosfetOkay |= 0x04; } else { MosfetOkay &= ~0x04;};
385
      PORTB = 0;
386
      Delay(SOUND2_A);
387
     }
388
    FETS_OFF;
389
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
390
//+ Low-Mosfets auf Schalten testen
391
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
392
    SENSE_B;
393
    LOW_A_EIN; // Low A ein
394
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
395
     {
396
      HIGH_B_EIN; // Test B
397
      Delay(SOUND_E);
398
      if(MessAD(0) > 50) { MosfetOkay &= ~0x08;} else { MosfetOkay |= 0x08;};
399
      PORTB = 0;
400
      Delay(SOUND2_A);
401
     }
402
 
403
//++++++++++++++++++++++++++++++++++++
404
    LOW_C_EIN; // Low C ein
405
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
406
     {
407
      HIGH_B_EIN; // Test B
408
      Delay(SOUND_E);
409
      if(MessAD(2) > 50) { MosfetOkay &= ~0x20;} else { MosfetOkay |= 0x20;};
410
      PORTB = 0;
411
      Delay(SOUND3_A);
412
     }
413
    FETS_OFF;
414
//++++++++++++++++++++++++++++++++++++
415
    FETS_OFF;
416
    LOW_B_EIN; // Low B ein
417
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
418
     {
419
      HIGH_C_EIN; // Test C
420
      Delay(SOUND_E);
421
      if(MessAD(1) > 50) { MosfetOkay &= ~0x10;} else { MosfetOkay |= 0x10;};
422
      PORTB = 0;
423
      Delay(SOUND3_A);
424
     }
425
    FETS_OFF;
426
//++++++++++++++++++++++++++++++++++++
427
 
428
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
429
    sei();//Globale Interrupts Einschalten
430
//    Delay_ms(250 * MotorAdresse);    
431
/*
432
    LOW_A_EIN; // Low B ein
433
#define SOUND8_A 650
434
    for(i=0; i<(TONDAUER / SOUND8_A); i++)
435
     {
436
      HIGH_B_EIN; // Test B
437
      Delay(SOUND_E);
438
      PORTB = 0;
439
      Delay(SOUND8_A);
440
     }
441
*/
442
 Delay_ms(300 * (4-ADR_TAB[MotorAdresse]));    
443
 if(!(MosfetOkay & 0x01))  { anz = 1; UDR='A'; } else
444
 if(!(MosfetOkay & 0x02))  { anz = 2; UDR='B'; } else
445
 if(!(MosfetOkay & 0x04))  { anz = 3; UDR='C'; } else
446
 if(!(MosfetOkay & 0x08))  { anz = 4; UDR='a'; } else
447
 if(!(MosfetOkay & 0x10))  { anz = 5; UDR='b'; } else
448
 if(!(MosfetOkay & 0x20))  { anz = 6; UDR='c'; }  
449
 
450
 if(anz) Delay_ms(1000);
451
 
452
 RotBlink(anz);
453
 uart_putchar('.');
454
}
455
 
456
//############################################################################
457
//
458
unsigned char SollwertErmittlung(void)
459
//############################################################################
460
{
461
    static unsigned int sollwert = 0;
462
    unsigned int ppm;
463
    if(!I2C_Timeout)   // bei Erreichen von 0 ist der Wert ungültig
464
        {
465
        if(SIO_Timeout)  // es gibt gültige SIO-Daten
466
            {
467
             sollwert =  (MAX_PWM * (unsigned int) SIO_Sollwert) / 200;  // skalieren auf 0-200 = 0-255
468
             PPM_Betrieb = 0;
469
             ICP_INT_DISABLE;
470
             PORTC &= ~ROT;
471
            }
472
        else
473
            if(anz_ppm_werte > 20)  // es gibt gültige PPM-Daten
474
                {
475
                PPM_Betrieb = 1;
476
                ppm = PPM_Signal;
477
                if(ppm > 300) ppm =   0;  // ungültiges Signal
478
                if(ppm > 200) ppm = 200;
479
                if(ppm <= MIN_PPM) sollwert = 0;
480
                else
481
                    {
482
                    sollwert = (int) MIN_PWM + ((MAX_PWM - MIN_PWM) * (ppm - MIN_PPM)) / (190 - MIN_PPM);
483
                    }
484
                PORTC &= ~ROT;
485
                }
486
            else   // Kein gültiger Sollwert
487
                {
488
                 if(!TEST_SCHUB) { if(sollwert) sollwert--; }  
489
                 PORTC |= ROT;
490
                }
491
        }
492
    else // I2C-Daten sind gültig
493
        {
494
        sollwert = I2C_RXBuffer;
495
        PPM_Betrieb = 0;
496
        PORTC &= ~ROT;
497
        ICP_INT_DISABLE;
498
        }
499
    if(sollwert > MAX_PWM) sollwert = MAX_PWM;
500
    return(sollwert);
501
}
502
 
503
void DebugAusgaben(void)
504
{
505
    DebugOut.Analog[0] = Strom;
506
    DebugOut.Analog[1] = Mittelstrom;
507
    DebugOut.Analog[2] = SIO_Drehzahl;
508
    DebugOut.Analog[3] = PPM_Signal;
509
}
510
 
511
 
512
//############################################################################
513
//Hauptprogramm
514
int main (void)
515
//############################################################################
516
{
517
    char altPhase = 0;
518
    int test = 0;
519
    unsigned int MinUpmPulse,Blink,TestschubTimer;
520
    unsigned int Blink2,MittelstromTimer,DrehzahlMessTimer,MotorGestopptTimer;
521
 
522
    DDRC  = 0x08;
523
    PORTC = 0x08;
524
    DDRD  = 0xBA;
525
    PORTD = 0x00;
526
    DDRB  = 0x0E;
527
    PORTB = 0x31;
528
 
529
#if (MOTORADRESSE == 0)
530
    PORTB |= (ADR1 + ADR2);   // Pullups für Adresswahl
531
    for(test=0;test<500;test++);
532
    if (PINB & ADR1)
533
         {
534
           if (PINB & ADR2) MotorAdresse = 1;
535
            else MotorAdresse = 2;
536
         }
537
         else
538
         {
539
           if (PINB & ADR2) MotorAdresse = 3;
540
            else MotorAdresse = 4;
541
         }
542
#else
543
    MotorAdresse  = MOTORADRESSE;
544
#endif
545
 
546
    UART_Init();
547
    Timer0_Init();
548
    sei();//Globale Interrupts Einschalten
549
 
550
    // Am Blinken erkennt man die richtige Motoradresse
551
/*
552
    for(test=0;test<5;test++)
553
        {
554
        if(test == MotorAdresse) PORTD |= GRUEN;
555
        Delay_ms(150);
556
        PORTD &= ~GRUEN;
557
        Delay_ms(250);
558
        }      
559
 
560
    Delay_ms(500);
561
*/  
562
   // UART_Init();  // war doppelt
563
    PWM_Init();
564
 
565
    InitIC2_Slave(0x50);                           
566
    InitPPM();
567
 
568
    Blink             = SetDelay(101);    
569
    Blink2            = SetDelay(102);
570
    MinUpmPulse       = SetDelay(103);
571
    MittelstromTimer  = SetDelay(254);
572
    DrehzahlMessTimer = SetDelay(1005);
573
    TestschubTimer    = SetDelay(1006);
574
    while(!CheckDelay(MinUpmPulse))
575
    {
576
     if(SollwertErmittlung()) break;
577
    }
578
    ;
579
    PORTD |= GRUEN;
580
    PWM = 0;
581
 
582
    SetPWM();
583
 
584
    SFIOR = 0x08;  // Analog Comperator ein
585
    ADMUX = 1;
586
 
587
    MinUpmPulse = SetDelay(10);
588
    DebugOut.Analog[1] = 1;
589
    PPM_Signal = 0;
590
 
591
    if(!SollwertErmittlung()) MotorTon();
592
//MotorTon();    
593
    PORTB = 0x31; // Pullups wieder einschalten
594
 
595
    // zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
596
    if(TEST_MANUELL)    Anwerfen(TEST_MANUELL);  // kommt von dort nicht wieder
597
 
598
    while (1)
599
        {
600
//ShowSense();
601
 
602
        if(!TEST_SCHUB)   PWM = SollwertErmittlung();
603
        //I2C_TXBuffer = PWM; // Antwort über I2C-Bus
604
        if(MANUELL_PWM)   PWM = MANUELL_PWM;
605
 
606
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
607
        if(Phase != altPhase)   // es gab eine Kommutierung im Interrupt
608
            {
609
            MotorGestoppt = 0;
610
            ZeitFuerBerechnungen = 0;    // direkt nach einer Kommutierung ist Zeit 
611
            MinUpmPulse = SetDelay(250);  // Timeout, falls ein Motor stehen bleibt
612
            altPhase = Phase;
613
            }
614
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
615
        if(!PWM)    // Sollwert == 0
616
            {
617
            MotorAnwerfen = 0;      // kein Startversuch
618
            ZeitFuerBerechnungen = 0;
619
            // nach 1,5 Sekunden den Motor als gestoppt betrachten 
620
            if(CheckDelay(MotorGestopptTimer))
621
                {
622
                DISABLE_SENSE_INT;
623
                MotorGestoppt = 1;  
624
                STEUER_OFF;
625
                }
626
            }
627
        else
628
            {
629
            if(MotorGestoppt) MotorAnwerfen = 1;        // Startversuch
630
            MotorGestopptTimer = SetDelay(1500);
631
            }
632
 
633
        if(MotorGestoppt && !TEST_SCHUB) PWM = 0;
634
        SetPWM();
635
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
636
        if(!ZeitFuerBerechnungen++)
637
            {
638
            if(MotorGestoppt) PORTD |= GRUEN; //else PORTD &= ~GRUEN;
639
            if(SIO_DEBUG)
640
                {
641
                DebugAusgaben();  // welche Werte sollen angezeigt werden?
642
                if(!UebertragungAbgeschlossen)  SendUart();
643
                else DatenUebertragung();
644
                }
645
            // Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
646
            if(CheckDelay(MittelstromTimer))  
647
                {
648
                MittelstromTimer = SetDelay(50); // alle 50ms
649
                if(Mittelstrom <  Strom) Mittelstrom++;// Mittelwert des Stroms bilden
650
                else if(Mittelstrom >  Strom) Mittelstrom--;
651
                if(Strom > MAX_STROM) MaxPWM -= MaxPWM / 32;              
652
                if((Mittelstrom > LIMIT_STROM))// Strom am Limit?
653
                    {
654
                    if(MaxPWM) MaxPWM--;// dann die Maximale PWM herunterfahren
655
                    PORTC |= ROT;
656
                    }
657
                else
658
                    {
659
                    if(MaxPWM < MAX_PWM) MaxPWM++;
660
                    }
661
                }
662
 
663
            if(CheckDelay(DrehzahlMessTimer))   // Ist-Drehzahl bestimmen
664
                {
665
                DrehzahlMessTimer = SetDelay(10);
666
                SIO_Drehzahl = CntKommutierungen;//(6 * CntKommutierungen) / (POLANZAHL / 2);
667
                CntKommutierungen = 0;
668
                if(PPM_Timeout == 0) // keine PPM-Signale
669
                ZeitZumAdWandeln = 1;
670
                }
671
 
672
          if(TEST_SCHUB)
673
           {
674
            if(CheckDelay(TestschubTimer))  
675
                {
676
                TestschubTimer = SetDelay(1500);
677
                    switch(test)
678
                        {
679
                        case 0: PWM = 50; test++; break;
680
                        case 1: PWM = 130; test++; break;
681
                        case 2: PWM = 60;  test++; break;
682
                        case 3: PWM = 140; test++; break;
683
                        case 4: PWM = 150; test = 0; break;
684
                        default: test = 0;
685
                        }
686
                }
687
            }  
688
          // Motor Stehen geblieben
689
            if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen)
690
                {
691
                MotorGestoppt = 1;    
692
                DISABLE_SENSE_INT;
693
                MinUpmPulse = SetDelay(100);        
694
                if(MotorAnwerfen)
695
                    {
696
                    PORTC &= ~ROT;
697
                    MotorAnwerfen = 0;
698
                    Anwerfen(10);
699
                    PORTD |= GRUEN;
700
                    MotorGestoppt = 0;    
701
                    Phase--;
702
                    PWM = 1;
703
                    SetPWM();
704
                    SENSE_TOGGLE_INT;
705
                    ENABLE_SENSE_INT;
706
                    MinUpmPulse = SetDelay(20);
707
                    while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
708
                    PWM = 15;
709
                    SetPWM();
710
                    MinUpmPulse = SetDelay(300);
711
                    while(!CheckDelay(MinUpmPulse)); // kurz Durchstarten
712
 
713
                                    // Drehzahlmessung wieder aufsetzen
714
                    DrehzahlMessTimer = SetDelay(50);
715
                    altPhase = 7;
716
                    }
717
                }
718
            } // ZeitFuerBerechnungen
719
        } // while(1) - Hauptschleife
720
}
721