Mosfet Driver

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
HMFK03LST1
Date:
Sun Jul 21 19:56:34 2013 +0000
Parent:
5:d814001b8aae
Commit message:
fasten

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r d814001b8aae -r 4af29761b1f0 main.cpp
--- a/main.cpp	Tue May 14 23:10:35 2013 +0000
+++ b/main.cpp	Sun Jul 21 19:56:34 2013 +0000
@@ -2,13 +2,14 @@
 
 LocalFileSystem local("local");                  //init Flashdrive for reading config file
 FILE *fp;                                        //Pointer to local Config File
+FILE *lp;
 
 #define USR_POWERDOWN    (0x104)                 //Power Down Mbed Interface (save 50% or 45 mA)
 
-float version = 0.9;                             //Program Version  
+float version = 1.1;                             //Program Version  
 
-bool mosfet_open  = true ;                      //Mosfet1 geschlossen
-bool mosfet_close = false;                      //Mosfet1 offen
+bool mosfet_open  = false;                      //Mosfet1 geschlossen
+bool mosfet_close = true;                       //Mosfet1 offen
 
 
 DigitalOut myled  (LED1);
@@ -19,8 +20,9 @@
 DigitalOut pump   (p36);
 DigitalOut purge  (p35);
 DigitalOut h2_blow(p34);
-DigitalOut waterp (p33);
+DigitalOut out4   (p33);
 DigitalOut mosfet (p21);
+DigitalOut xbee_rst(p11);
 
 DigitalIn  In1    (p22);                      // Caps Down
 DigitalIn  In2    (p23);                      // Power UP
@@ -36,10 +38,10 @@
 AnalogIn   AI_5   (p19);                      // AI 5 Faktor * 2
 AnalogIn   cur_in (p20);                      // AI 6 Faktor * 2 zum Strommesser
 
-Serial pc(USBTX, USBRX);
+// Serial pc(USBTX, USBRX);
+Serial pc(p9, p10);           // XBee
 Ticker PC_OUT_timer;          // Output Monitoring to Serial
-Ticker LED_timer;             // Set Status LED´s
-
+ 
 Timer t;
 
 // Brennstoffzellen Parameter
@@ -53,14 +55,16 @@
 float bz_c_i_max  =  20.0;    //Strom Integrale Reglung Max %
 float bz_c_i_min  = -10.0;    //Strom Integrale Reglung Min %
 float bz_temp_max =  55.0;    //Maximale Wassertemperatur °C 
-int   load_fail   =     0;    //Kein Strom trotz Imax
+float bz_faktor   =     0;    //Temp Variable
 
 // SuperCap Parameter
-float cap_max     =  25.0;    //CAP Spannung max. (Abschaltung)
+float cap_max     =  35.0;    //CAP Spannung max. (Abschaltung)
 float cap_p_max   =  90.0;    //CAP Prozent Load bei 0V
-float cap_min     =  20.0;    //CAP Spannung min. (Zelle an)
-float cap_p_min   =   5.0;    //CAP Prozent Load bei 0V
+float cap_min     =  30.0;    //CAP Spannung min. (Zelle an)
+float cap_p_min   =  15.0;    //CAP Prozent Load bei 0V
 float cap_delta   =   1.5;    //Absenkung der Spannung mit Din
+float cap_low     =  26.0;    //Grenze zur Volleinkopplung                                  ***
+float cap_up      =  29.5;    //Grenze high current                                         ***
 
 // Pump & Purge Parameter
 float purge_start =   3.0;    //s before starting purch
@@ -70,21 +74,25 @@
 float pwm_cycle   =  20.0;    //ms für PWM Period
 float pwm_on      =  12.0;    //ms für PWM high
 float pwm_pro     =     0;    //PWM Taktung
+float pow_up_on   =  28.5;    //Einschalten Boostpumpe V unter Cap_min                      ***
+float pow_up_off  =  29.5;    //Ausschalten Boostpumpe V unter Cap_min                      ***
+
 
 // Monitoring Parameter
 int   debug       =     1;    //Serial Output on (1)
 float sample      =     5;    //Serial Output Samples per Second
 char  com_in[8]          ;    //Serial input
-
+float mos_t_max   =    85;
 
 // Mosfet Parameter
 int   Cel_Level   =     0;    //% Load aus Bz Spannung
 int   Cap_Level   =     0;    //% Load aus Cap Spannung
-int   Cur_Level   =     0;
-int   Load_Level  =     0;    //% Load aus Bz und Cap
+float Cur_Level   =     0;    //% Load aus BZ Strom
+float Load_Level  =     0;    //% Load aus Bz und Cap
 float mos_temp    =     0;    //°C Mosfet
 
 // Temp Variable
+bool  pwm_mode    = false;
 bool  Load        = false;    //Laderegler aktiv
 bool  pump_on     = false;    //Pumpenzustand
 int   boost       =     0;    //Number of PWM-Cycles for Pump runup boost calc in load_cfg
@@ -96,26 +104,97 @@
 float bz_temp     =     0;    //BZ Temperatur
 bool  temp_1_2    = false;    //Auswahl Temperaturkanal
 int   pump_block  =     2;  
+int   mos_block   =  1000;
+int   purge_h2_on =     0;
+bool  power_up    =     0; 
+int   h2purge     =   100;
+int   h2purge_count =   0;   
+int   msg_cnt     =     0;
+bool  mos_over    = false;
+bool  pump_2      = true;
 
 unsigned int counter_cycle = 0;      //Counter PWM-Cycles for Pump-Purge
 unsigned int counter_takt  = 0;      //Counter for PWM Pump
-
+unsigned int counter_send  = 0;
+unsigned int temp_takt     = 0;
+float pwm_cycle_count      = 0;
+float zahler               = 0;
 
 float Regler_Hz = 1000.0;  
 
 
+void load_cfg();
+void LED();
+void led_beep();
+void do_command();
+void get_input();
+int semihost_powerdown();
+int port(int);
+void SEND();
+void TEMP();
+void PUMPE();
+void control_block();
+bool LOAD_STATE();
 
+void mos_pwm()
+{
+ int temp = 0;
+  temp = int((Load_Level * pwm_cycle_count) / 100.0);                                  
+  while(t.read_us() <= temp){ mosfet = mosfet_close;};                           // %Load PWM zu Timer vergleich            {}                                        
+  if   (Load_Level < 100.0) { mosfet = mosfet_open ;};         // %Load PWM erreicht Mosfet aus oder dauer an                                                                               
+}
+
+int main() 
+{ 
+    
+    mosfet = mosfet_open;                           //Mosfet auf
+    pc.baud(115200);                                //config Serial Port
+
+    load_cfg();                                     //init config File               
+    t.start();                                      //Timer für PWM starten     
+
+   while(1) 
+    {
+     while (t.read_us() <= pwm_cycle_count){};      // Time Sync
+     SEND();                                        // Send MSG
+     get_input();                                   // Serial Command input
+     t.reset();                                     // Timer reset
+     
+     if (LOAD_STATE() == true) 
+      {
+       if (Load_Level > 50)      {
+                                  mosfet = mosfet_close;                     // Mosfet close
+                                  control_block();                           // PID Block
+                                  mos_pwm();                                 // Mosfet PWM
+                                 }
+        else                     {
+                                  mos_pwm();                                 // Mosfet PWM
+                                  control_block();                           // PID Block 
+                                 }; 
+       }
+      else
+       {
+        control_block();                                                      // PID Block
+       }    
+    }
+}
+
+
+void xbee_rs()
+ {
+  xbee_rst = 0; wait_ms(1); xbee_rst = 1;
+ }
 
 void load_cfg()
 {
- char read[25][5];
+ char read[27][5];
 
  char  i = 0;
  char  j = 0;
  int   c = 0;
  float  temp;
  
- for (j = 0; j<26; j++)
+ for (j = 0; j<28; j++)
   {
    for (i = 0; i<6; i++)
     {
@@ -164,10 +243,17 @@
        
        sscanf(&read[21][1], "%f", &temp); Regler_Hz   = temp;
        sscanf(&read[22][1], "%f", &temp); debug       = temp;
-       sscanf(&read[23][1], "%f", &temp); sample      = temp;   
+       sscanf(&read[23][1], "%f", &temp); sample      = temp;
+       
+       sscanf(&read[24][1], "%f", &temp); mos_t_max   = temp;   
        
-       boost = boost_time * 1000 / pwm_cycle;
-       pump_block = 5 / purge_end;      
+       boost           = boost_time * 1000 / pwm_cycle;                 //boost in Pumpen PWM-Cycle
+       pump_block      = 5 / purge_end;                                 //5s Pumpen aus
+       mos_block       = 2000000.0/Regler_Hz ;                          //2s Mosfet aus
+       pwm_cycle_count = 1000000.0/Regler_Hz ;                          //ns pro Mosfet PWM-Cycle
+       bz_faktor       = (bz_p_oben - bz_p_unten)/(bz_max - bz_min);    //
+       h2purge         = 600 / purge_end;
+       xbee_rs();
      }
     
     pc.printf("\n\r"                                            );
@@ -190,6 +276,8 @@
     pc.printf(" CAP max        [%c] : %5.1f \n\r",37,cap_p_max  );
     pc.printf(" CAP min        [V] : %5.1f \n\r",cap_min        );
     pc.printf(" CAP min        [%c] : %5.1f \n\r",37,cap_p_min  );
+    pc.printf(" CAP up  add A  [V] : %5.1f \n\r",cap_up         );
+    pc.printf(" CAP low Mos on [V] : %5.1f \n\r",cap_low        );
     pc.printf(" CAP lo on Din  [-V]: %5.1f \n\r",cap_delta      );
     pc.printf("----------Pump & Purge---------- \n\r"           );
     pc.printf(" Purge on       [s] : %5.1f \n\r",purge_start    );
@@ -198,7 +286,10 @@
     pc.printf(" Pumpe abregeln [%c] : %5.1f \n\r",37,pump_red   );
     pc.printf(" PWM cycle      [ms]: %5.0f \n\r"  ,pwm_cycle    );
     pc.printf(" PWM on         [ms]: %5.0f \n\r"  ,pwm_on       );
+    pc.printf(" All Pumps on   [V] : %5.1f \n\r"  ,pow_up_on    );
+    pc.printf(" All Pumps off  [V] : %5.1f \n\r"  ,pow_up_off   );
     pc.printf("------------Monitor------------- \n\r"           );
+    pc.printf(" Mosfet Temp max[%cC]  :%4.0f \n\r",176,mos_t_max);
     pc.printf(" Regler Frequenz[Hz]  :%4.0f \n\r"  ,Regler_Hz   );
     pc.printf(" Serial output  [bool]: %3d \n\r"  ,debug        );
     pc.printf(" Samplerate     [Hz]  : %3.0f \n\r",sample       );
@@ -208,29 +299,24 @@
 
 void LED()
 {
-  if (bz  < bz_min )            myled  = 1; else myled  = 0;                //LED = Spannung an der BZ IO
-  if (cap > cap_min)            myled1 = 1; else myled1 = 0;                //LED = Spannung an den Cap´s IO 
-  if ((mosfet == mosfet_close)
-     &&(Load == true))          myled2 = 1; else myled2 = 0;                //LED = Gate Zustand Mosfet 1
-  if (pump_on == 1)             myled3 = 1; else myled3 = 0;                //LED = Pumpe an
+  if (bz      <  bz_min )            myled  = 1; else myled  = 0;                //LED = Spannung an der BZ IO
+  if (cap     >  cap_min)            myled1 = 1; else myled1 = 0;                //LED = Spannung an den Cap´s IO 
+  if (Load    == true   )            myled2 = 1; else myled2 = 0;                //LED = Gate Zustand Mosfet 1
+  if (pump_on == 1      )            myled3 = 1; else myled3 = 0;                //LED = Pumpe an
 }
 
 void led_beep()
 {
- LED_timer.detach();
- mosfet = mosfet_open;
  myled = 0; myled1 = 0; myled2 = 0; myled3 = 0;
- wait(0.1);
+ wait(0.05);
  myled = 1; 
- wait(0.1);
+ wait(0.05);
  myled1 = 1; 
- wait(0.1);
+ wait(0.05);
  myled2 = 1;
- wait(0.1);
+ wait(0.05);
  myled3 = 1;
- wait(0.1);
- LED_timer.attach   (&LED   , 0.200);
- myled3 = 1; 
+ wait(0.05);
 }
 
 
@@ -238,9 +324,40 @@
 {
 switch(com_in[0])
             {        
-                case 'r':  load_cfg();
+                case 'r':  mosfet = mosfet_open;
+                           load_cfg();
                            led_beep();
                            break;       
+            
+                case 'm':  
+                           mosfet = mosfet_open;
+                           debug = 5;
+                           break;
+                
+                case 'n':  
+                           mosfet = mosfet_open;
+                           debug = 0;
+                           break;
+                
+                
+                
+                case 'l':  debug = 6;
+                           break;
+                           
+                case 'u':  pump_on = true;
+                           break;
+                           
+                case '0':  mos_over = false;
+                           break;
+                
+                case '1':  mos_over = true;
+                           break;        
+                
+                case '2':  pump_2 = true;
+                           break;
+                
+                case '3':  pump_2 = false;
+                           break;
             }
 }
 
@@ -251,7 +368,7 @@
 
     while (pc.readable() == true)     
     { 
-          wert = pc.getc();                                       //Char lesen
+          wert = pc.getc();                                           //Char lesen
           if ((wert == 'c') && ((input_c == 0))) {input_c = 1;}       //Command Char 1 prüfen
           if ( wert == 10) {input_c = 0; do_command(); com_in[0] =' ';}   //Command End prüfen
           
@@ -265,9 +382,6 @@
 }
 
 
-
-
-
 int semihost_powerdown() 
 {
  uint32_t arg;
@@ -283,7 +397,7 @@
  if (number == 4) {if (In4 == 1) return 1; else return 0;}
  if (number == 5) {if (In5 == 1) return 1; else return 0;}
  if (number == 6) {if (In6 == 1) return 1; else return 0;}
-return 2;
+return -1;
 }
 
 void SEND()
@@ -294,56 +408,60 @@
  float temp4 = 0;
  float temp5 = 0;
  char  temp6 = 0;
- 
- bool status= false;
+
+
 
-switch (debug)
-{
- case 1:     
-             if (mosfet == mosfet_close) status = true;
-             if (Load == true) mosfet = mosfet_open;
-             pc.printf("BZ: %4.1f/%4.1f-%4.1f+%4.1f  CAP: %4.1f/%4.1f-%4.1f  Pump : %2d Purge: %3.1f/%3.1f-%3.1f  Load: %02d/%02d-%02d Current: %4.2f Temp BZ: %3.1f Temp Mos: %3.1f\n\r"
-             ,bz,bz_min,bz_on,bz_max,cap,cap_min,cap_max,int(pwm_pro*100),float(counter_cycle*pwm_cycle/1000),
+if (counter_send >= int(Regler_Hz / sample)) 
+ { 
+  LED();
+  counter_send = 0;
+  
+  switch (debug)
+   {
+    case 1:           
+             mosfet = mosfet_open;
+             pc.printf("BZ: %4.1f/%4.1f-%4.1f+%4.1f  CAP: %4.1f/%4.1f-%4.1f  Pump : %2d Purge: %3.1f/%3.1f-%3.1f  Load: %02d/%02d-%02d Current: %4.2f Temp BZ: %3.1f Temp Mos: %3.1f H2purge: %03d\n\r"
+             ,bz,bz_min,bz_on,bz_max,cap,cap_min,cap_max,int(pwm_pro*100),float(counter_cycle * pwm_cycle/1000),
              purge_start,purge_end,int(Load_Level),int((Cap_Level + Cel_Level) + bz_c_i_min), 
-             int((Cap_Level + Cel_Level) + bz_c_i_max), current, bz_temp, mos_temp);
-             if (status == true) mosfet = mosfet_close; 
+             int((Cap_Level + Cel_Level) + bz_c_i_max), current, bz_temp, mos_temp, h2purge_count);
              break;
 
- case 2:
-             if (mosfet == mosfet_close) status = true;
-             if (Load == true) mosfet = mosfet_open;
+    case 2:
+             mosfet = mosfet_open;
              pc.printf("In1: %2d In2: %2d In3: %2d In4: %2d In5: %2d In6: %2d \n\r",port(1),port(2),port(3),port(4),port(5),port(6));      
-             if (status == true) mosfet = mosfet_close; 
              break;
  
- case 3:
-             if (mosfet == mosfet_close) status = true;
-             if (Load == true) mosfet = mosfet_open;
-             pc.printf("AI1: %3.2f AI2: %3.2f AI3: %3.2f AI4: %3.2f AI5: %3.2f AI6: %3.2f \n\r",cap_in.read(),bz_in.read(),mos_in.read(),temp_in.read(),AI_5.read(),cur_in.read());       
-             if (status == true) mosfet = mosfet_close; 
+    case 3:         
+             mosfet = mosfet_open;
+             pc.printf("AI1: %3.2f AI2: %3.2f AI3: %3.2f AI4: %3.2f AI5: %3.2f AI6: %3.2f \n\r",cap_in.read(),bz_in.read(),mos_in.read(),temp_in.read(),AI_5.read(),cur_in.read());
              break;
 
- case 4:
+    case 4:
              pc.printf("Change \n\r");       
              pump    = !pump ;
              purge   = !purge;
              h2_blow = !h2_blow;
-             waterp  = !waterp;
+             out4    = !out4;
              mosfet  = !mosfet;
              break;   
                
 
 
- case 5:   
+    case 5:   
+             mosfet = mosfet_open;
+             
+             if (msg_cnt > 1000) {xbee_rs(); msg_cnt = 0;} else {msg_cnt++; xbee_rst = 1;};
              
              if (((Cap_Level + Cel_Level) + bz_c_i_min) > 0) {temp1 = ((Cap_Level + Cel_Level) + bz_c_i_min);}
              else temp1 = 0;
-             
+             if (temp1 > 100) temp1 = 100;
              if (((Cap_Level + Cel_Level) + bz_c_i_max) > 0) {temp2 = ((Cap_Level + Cel_Level) + bz_c_i_max);}
              else temp2 = 0;
+             if (temp2 > 100) temp2 = 100;
              
              if (current > 0) {temp3 = current;}
              else temp3 = 0;
+             if (current > 10){temp3 = 10;};
              
              if (bz_temp > 0) {temp4 = bz_temp;}
              else temp4 = 0;
@@ -353,8 +471,9 @@
              
              temp6 = (purge<<7)+(pump_on<<6)+(In1<<5) + (In2<<4) + (In3<<3) + (In4<<2) + (In5<<1) + In6;
              
-             pc.printf("%c%c%c%c%c%c%c%c%c%c%c%c",255,128,                                                            //0,1
-                                           char(int(bz*6)),                                      //0-35   zu 0-250    2     0
+             pc.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c",
+                                           255,128,                                                            //0,1
+                                           char(int(bz*6)),                                      //0-42   zu 0-250    2     0
                                            char(int(cap*7)),                                     //0-38   zu 0-250    3     1
                                            char(int(Load_Level * 2.5)),                          //0-100  zu 0-250    4     2
                                            char(int(temp1      * 2.5)),                          //0-100  zu 0-250    5     3
@@ -363,189 +482,179 @@
                                            char(int(temp4      * 2.5)),                          //0-100  zu 0-250    8     6
                                            char(int(temp5      * 2.5)),                          //0-100  zu 0-250    9     7
                                            char(int(pwm_pro    * 250)),                          //0-100  zu 0-250   10     8
-                                           temp6                                                 //Statusbits Din/Pump/Purge
+                                           char(h2purge_count        ),                          //counter 0 -       11     9 
+                                           temp6                                                 //Statusbits        12     10
                       );
              break;
+ 
+    case 6:   
+            mosfet = mosfet_open;      
+            break;
+   }
  }
-
 }
 
 
-
-
-
-
 void TEMP() 
 {
-if (temp_1_2 == true) bz_temp  = (temp_in.read()/0.00407)- 48.3;
-else                  mos_temp = ( mos_in.read()/0.00407)- 50.3;
-temp_1_2 = !temp_1_2;
+ if (temp_1_2 == true) bz_temp  = (temp_in.read()/0.00407)- 53.6;
+ else                  mos_temp = ( mos_in.read()/0.00407)- 54.1;
+ 
+ temp_1_2 = !temp_1_2;
+ 
+ if (  (mos_temp >  mos_t_max  )   
+     ||(bz_temp  >  bz_temp_max)){power_up = 0; pump_block = (10 / purge_end);}                 ;
 }
 
 
-
 void PUMPE()
-{
- float regler = 1;
- 
- counter_takt++;                                        //Aufrufzähler 
- 
+{                                                     
+ temp_takt++;                                        //Aufrufzähler
+  if (temp_takt >= 500){TEMP();temp_takt = 0;}       //alle 500ms
  
- 
- if (cap > (cap_max - (!In1 * cap_delta)))
- {
-  pump_on = false;
-  pwm_pro = 0;
- }
+//*** Autopurge
+
+ if (    (h2purge    == 0)                            // Purge Sperre aus
+      && (Cur_Level  == bz_c_i_max)                   // Current Integral auf maximum     
+      && (pump_on    == 1)                            // Pumpe an
+      && (cap < (0.95 * cap_low))                     // Capspannung niedrig  
+      
+      ) {mosfet = mosfet_open; h2_blow = 1; wait_ms(40); h2_blow = 0; h2purge = 495 * Regler_Hz/pwm_cycle; h2purge_count++;} else {h2_blow = 0;}
  
  
- if ((cap <= cap_min) || (pump_on == true)) //Pumpe Einschaltbedingung
-  {  
-   pump_on = true;
+//*** Abschalten
+ if (  (cap >= (cap_max - (In1 * cap_delta)))         // Abschalten wenn CAP max erreicht
+     ||(bz  >  (bz_max))                              // Abschalten wenn BZ max überrschritten 
+    )                                                     
+  {
+   pump_on = false;
+   pwm_pro = 0;
+  }
 
-   if (bz > bz_on) {1 - ((pump_red / ((bz_max-(!In1 * cap_delta)) - bz_on) * (bz - bz_on)) / 1000);}               //Luftreduzierung wenn Spannung über BZ_on (lineare Reduzierung bis BZ_max um pump_red(%))
-                 
-   pwm_pro = ((float(pwm_on) * regler) / pwm_cycle );                                                               //PWM Prozent für Monitoring
-  
-   
-   if ((pump_block == 0) && (counter_takt > (pwm_cycle - pwm_on) * Regler_Hz / 1000.0)) {pump = 1 ;}     //Set PWM from low to high 
-   
+//*** Einschalten
+ if ( 
+      ((cap <= (cap_min - (In1 * cap_delta)))           // Einschalten wenn Cap min erreicht
+     ||(pump_on == true))                               // Einschalten wenn Pumpe in Betrieb - Abschaltbedingung nicht erreicht
+     && (In2 == false)                                  // Serviceschalter nicht betätigt
+    )                                                   
+  {
+   pump_on = true;
+   pwm_pro = ((float(pwm_on)) / pwm_cycle );                                                // PWM Prozent für Monitoring              
+    
+   if ((pump_block == 0) && (counter_takt > (pwm_cycle - pwm_on))) {pump = 1 ;}             //Set PWM from low to high 
                                      
-   if (counter_takt > (pwm_cycle * Regler_Hz / 1000))                             //End PWM cycle
-    {
-     if (counter_cycle == (50)) TEMP();                                           //Temperatur messen
+   if (counter_takt > pwm_cycle)                                                            //End PWM cycle    40ms * 7500/1000=7.5/ms
+    {                 
      counter_takt  = 0;
      counter_cycle++;
               
-     if ((boost > 0) && (pump_block == 0)) {boost--;}
+     if (h2purge > 0){h2purge--;};
      
-     if (counter_cycle >= ((purge_start - boost_time) * 1000 / pwm_cycle))         //Vorgezogener Anlauf Purgepumpe
-      {                                                                     
-       if (pump = 1) purge = 1;                                     
-      }
+     if ((boost > 0) && (pump_block == 0)) {boost--; pump = 1;} else {pump = 0;}            //PWM Betrieb
      
-     if (counter_cycle < ((purge_start - boost_time) * 1000 / pwm_cycle))  
-      {           
-       if (boost == 0) pump = 0;                                                  //PWM Betrieb
-       purge = 0;                                     
-      }
-     else
-      {
-       pump = 0;
+     if (
+          (   (counter_cycle == int((purge_start - boost_time) * (1000 / pwm_cycle)))       //Vorgezogener Anlauf Purgepumpe
+          || (power_up == 1))
+        )   
+      {                                                                     
+       if (pump_2 == true) purge = 1; else purge = 0;                                     
+      } 
+       
+     if (counter_cycle == int((purge_start) * (1000 / pwm_cycle)))                          //Purge Start
+      {                                                                                                                                  
       }
              
-     if (counter_cycle >= (purge_end * (1000 / pwm_cycle )))                           //Purge Ende   counter_cycle = 1 pro Mosfet (PWM 1000/Regler_Hz in [ms])
+     if (counter_cycle >= int(purge_end * (1000 / pwm_cycle )))          //Purge Ende   counter_cycle = 1 pro Mosfet (PWM 1000/Regler_Hz in [ms])
       {
        counter_cycle = 0; 
-       purge = 0; 
-       pump = 0;
+       if ((power_up == 0))purge = 0; 
        if (pump_block > 0) pump_block--;
       }    
     }      
   }
  else
   {
-   pump_on = 0; pump = 0; purge = 0;                                       //Pumpe aus
-   boost = (boost_time * 1000 / pwm_cycle);                                // Boost für nächsten Start setzen
+   pump_on = 0; pump = 0; purge = 0;                                                        //Pumpe aus
+   boost = (boost_time * 1000 / pwm_cycle);                                                 //Boost für nächsten Start setzen
   }
-
 }
 
-int main() 
-{ 
-    mosfet = mosfet_open;                                                //Mosfet schließen
-    pc.baud(115200);                                                     //config Serial Port
-    load_cfg();                                                          //init config File
-    //semihost_powerdown();                                                //Mbed Interface powerdown
-    PC_OUT_timer.attach(&SEND  , (1/sample));                            //Serial output Timer
-    LED_timer.attach   (&LED   , 0.200     );                            //LED Status    Timer                      
-    
-    t.start();                                                           //Timer für PWM starten
-    float bz_faktor;                                                     //Temp Variable
-    unsigned int pwm_cycle_count;
-    bool  power_up = 0;      
-    
-    pwm_cycle_count = ((1000000/Regler_Hz)); 
-    bz_faktor = ((bz_p_oben - bz_p_unten)/(bz_max - bz_min));            //Prozent Umrechnung BZ
 
 
+void control_block()
+{    
+ float cur_t;
+ float bz_t;
+ float cap_t;
+ float bz_cur_t;
+ 
+     counter_takt++;
+     counter_send++;        
     
-    while(1) 
-    {
-     while (t.read_us() <= pwm_cycle_count){};
      
-     get_input();                                                        //µs Timer für Mosfet PWM starten
-     PUMPE();                                                            //Pumpen PWM aufrufen
-                                                                                      
-     bz  = (((24.54 * bz_in * bz_in)+(31.812 * bz_in)+0.04)*2 + bz )/3;                                    //BZ  RAW in Spannung umrechnen (2*neu zu 1*alt Glättung) (immer)
-     if (counter_takt%2 == 0){cap = (((24.54 * cap_in * cap_in)+(31.812 * cap_in + 0.04))*2 + cap_in )/3;}          //CAP RAW in Spannung umrechnen (2*neu zu 1*alt Glättung) (jedes 2.mal)
-     else                    {current = (cur_in - 0.32) * 12.568;}           //Current RAW in Spannung umrechnen (ohne Glättung)       (jedes 2.mal)
-     
-                                                                        
+     if (counter_takt%(int(Regler_Hz/1000)) == 0) PUMPE();                                   //Pumpen PWM aufrufen
+     cur_t = (cur_in - 0.270);                         
+     current = ( (cur_t * cur_t * 24.975) + (cur_t * 4.701) + (current) ) / 2;               //Current RAW in Spannung umrechnen (immer) -0.28
+     if (current > 10.1) current = 10.1;
+     if (counter_takt%2 == 0){cap_t = cap_in; cap = (((24.54 * cap_t * cap_t)+(31.812 * cap_t) + 0.04) + cap * 2)/3;}   //CAP RAW in Spannung umrechnen (1*neu zu 2*alt Glättung) (jedes 2.mal)
+     else                    {bz_t  = bz_in ; bz  = (((24.54 * bz_t  * bz_t )+(31.812 * bz_t ) + 0.04) + bz  * 2)/3;}   //BZ  RAW in Spannung umrechnen (1*neu zu 2*alt Glättung)(jedes 2.mal)                                                                      
      
 //***Regulate Cell Level***     
-     Cel_Level = (bz_faktor * (bz - bz_min) + bz_p_unten);          //%Load aus Zellenspannung berechnen
+     Cel_Level = (bz_faktor * (bz - bz_min) + bz_p_unten);                                   //%Load aus Zellenspannung berechnen
 
 //***Regulate Cap´s Level*** 
-     Cap_Level   = (((cap / cap_max) * (cap_p_max - cap_p_min)) + cap_p_min);  //%Load aus Cap Level
+     Cap_Level   = (((cap / cap_max) * (cap_p_max - cap_p_min)) + cap_p_min);                //%Load aus Cap Level
 
 //***Regulate Current Level***     
-     if (cap < cap_min) {power_up = 1;};
-     if (cap > 0.9*cap_max){power_up = 0;}; 
-     if ((current-(bz_current + (power_up * bz_cur_add))) > 0)
-      {if (Cur_Level > (bz_c_i_min)) Cur_Level--;}                     //to much Load
-     else
-      {if (Cur_Level < (bz_c_i_max)) Cur_Level++;}                     //less Load  
+      if ((cap <  pow_up_on ) && (cap > 10)) {power_up = 1;};
+      if ((cap >  pow_up_off)              ) {power_up = 0;}; 
+      
+      if ( (cap > cap_up) || (cap < 14)    ) {bz_cur_t = bz_current;} else {bz_cur_t = bz_current + bz_cur_add;};
+      if ( (current - bz_cur_t) > 0        )
+       {if ((Cur_Level > bz_c_i_min))                  {Cur_Level = Cur_Level - 0.2;}}                  //to much Amp-Load
+      else
+       {if ((Cur_Level < bz_c_i_max) && (bz > bz_min)) {Cur_Level = Cur_Level + 0.2;}}                  //less Amp-Load  
+          
+//*** Sum and limit all Regulators     
+ 
+     Load_Level  = Cur_Level + Cap_Level + Cel_Level;   
+     
+     if (  (Load_Level > 100)
+         ||(mos_over == true)
+        )                       Load_Level = 100.1;
            
-//*** Sum all Regulators     
- 
-     Load_Level  = Cur_Level;   
-     Load_Level  = Load_Level + Cap_Level;
-     Load_Level  = Load_Level + Cel_Level;
-     if (Load_Level > 100) Load_Level = 100;
-     if (Load_Level <   0) Load_Level =   0;
-
- t.reset();  
+     if (  (cap > (bz_max - 3.5))
+         ||(cap <  cap_low)
+        )                       Load_Level = 100.1;
 
-     if (Load == true)                                                 // Laden aktiv
-      {
-                                                                       
-       if (bz > bz_min || bz > bz_max)                                 // Zelle über min. Spannung oder über max Spannung zum Entladen
-        { 
-         while (t.read_us() <= pwm_cycle_count)                        // während der PWM (1khz Periode)
-          {
-           if (t.read_us() < Load_Level * 10000 / Regler_Hz)            // %Load PWM zu Timer vergleich
-            {mosfet = mosfet_close;}                                   // %Load PWM nicht erreicht Mosfet an
-           else
-            {mosfet = mosfet_open;}                                    // %Load PWM erreicht Mosfet aus
-          }
-        }
-       else
-        {
-          mosfet = mosfet_open ;                                       // Mosfet wegen Unterspannung BZ auskoppeln 
-          Load = false;                                                // Laden beenden bis BZ > BZ on (Sicherungsschaltung)
-        }       
-      }
-     else
-      {
-       if (bz > cap + 0.5){mosfet = mosfet_open ;}                      // Mosfet im nicht Ladebetrieb auskoppeln 
-       else               {mosfet = mosfet_close;}                      // Mosfet im nicht Ladebetrieb einkoppeln (Treiber stromfrei = Stromsparen) 
-      }  
-    
-    
-     
-     if ((current < 0.2) && (Cur_Level > (0.9 * bz_c_i_max)) && (Load == true)){Load = false; load_fail = 100;}
-    
-     if (( cap < cap_min) && (bz > bz_on) && (Load == false)) 
+     if (Load_Level <   0)      Load_Level =   0;  
+}
+
+
+
+
+bool LOAD_STATE()
+{
+ if (
+        ( bz   >  bz_on )                //  BZ über Einschaltspannung 
+     && ( Load == false )                //  Laden noch nicht ausgelöst
+    )
        {
-        Load = true; 
-        if (bz_c_i_min < 0) {Cur_Level = 2 * bz_c_i_min;}else {Cur_Level = -100;}   // Cap unter Minimum oder BZ über Maximum = Laden beginnen / PWM vom Minimum Starten
+        Load = true;              
+        Cur_Level = bz_c_i_min;        // mit I min einkoppeln
        }
        
-     if (  bz  > bz_max && load_fail == 0) Load = true;   // Überladung abführen
-     
-     if (load_fail > 0){load_fail--;pump_on = false;}
-   
-    }
- 
+ if (
+        (  Load      == true     )       // Laden aktiv
+      &&( (bz - cap) >  0.0      )                     
+      &&(  bz        >  bz_min   ) 
+    ) 
+    {return true;}
+  else 
+    {Load = false; mosfet = mosfet_open; return false;};
 }
+
+
+
+
+