main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include <string>
00004 #include "FastPWM.h"
00005 #include "components.cpp"
00006 
00007 
00008 const char I2C_IN  = 4;
00009 const char I2C_OUT = 8;
00010 
00011 const int PWM_Frequenz = 10000; 
00012 
00013 DigitalOut led_1(LED1); 
00014 DigitalOut led_2(LED2);
00015 DigitalOut led_3(LED3);
00016 DigitalOut led_4(LED4);
00017 
00018 DigitalOut i2c_cs(p29);
00019 DigitalOut xb_rst(p11);
00020 DigitalOut mp_A(p15);
00021 DigitalOut mp_B(p16);
00022 
00023 AnalogIn   current(p19);
00024 AnalogIn   mp1yA(p20);
00025 AnalogIn   mp2yA(p18);
00026 AnalogIn   mp2xA(p17);
00027 DigitalIn  mp2xD(p12);
00028 
00029 FastPWM    pwm_6(p21,-1); //Mosfet
00030 DigitalOut h2_purge(p22);
00031 FastPWM    pwm_4(p23,-1); //H2O Fan
00032 FastPWM    pwm_3(p24,-1); //H20 Pump
00033 FastPWM    pwm_2(p25,-1); //Air Pump 2
00034 FastPWM    pwm_1(p26,-1); //Air Pump 2              
00035 
00036 
00037 Serial xbee(p9,p10);
00038 Serial gps(p13,p14);
00039 Serial pc(USBTX,USBRX);
00040 
00041 SPI ext_spi(p5, p6, p7);
00042 
00043 I2C i2c(p28, p27);
00044 
00045 LocalFileSystem local("local");                  //init Flashdrive for reading config file
00046 FILE *fp; 
00047 
00048 struct_cap      cap;
00049 struct_fuelcell fc;
00050 struct_h2o      h2o;
00051 struct_mosfet   mosfet;
00052 struct_o2       o2;
00053 struct_switch   switches;
00054 struct_system   sys;
00055 struct_error    error;
00056 
00057 
00058 //Standard Funktions
00059 //----------------------------------------------------------------------------------------------
00060 void read_val(const char* name,const char* value);
00061 
00062 
00063 double clamp(double value, double val_min, double val_max, bool* has_clamped)
00064 {
00065     *has_clamped = true;
00066     if(value < val_min) return val_min;
00067     if(value > val_max) return val_max;
00068     *has_clamped = false;
00069     return value;
00070 }
00071 
00072 
00073 
00074 
00075 void load_cfg()
00076 {
00077  char name_val[2][20];
00078  char i    = 0;
00079  char name = 1;
00080  int  c    = 50;
00081   
00082  fp = fopen("/local/power.cfg", "r");
00083     if ( fp != NULL )
00084      {
00085       while(c != EOF)
00086           { 
00087 
00088            c = fgetc(fp);
00089 
00090            if (c == 10){
00091                         name_val[name][i+1] = '\0'; 
00092                         name = 1; 
00093                         i    = 0; 
00094                         read_val(&name_val[1][0], &name_val[0][0]);
00095                        } 
00096             else       
00097                        {
00098                         if (c == '='){
00099                                       name_val[name][i+1] = '\0'; 
00100                                       name = 0; 
00101                                       i    = 0;
00102                                      } 
00103                          else        
00104                                      { 
00105                                       name_val[name][i] = c;
00106                                       i++;
00107                                      }
00108                        }
00109           }
00110       fclose(fp);
00111      }
00112 }      
00113 
00114 
00115 void read_val(const char* name,const char* value)
00116 {                
00117  float temp;
00118  
00119  sscanf(value, "%f", &temp );
00120  pc.printf("Input : %-18s  Value : %6d  ", name, int(temp));
00121  
00122  if (string(&name[0]) == "O2_air_current"   ){ o2.air_current    = temp; pc.printf("Value read: %6d \n\r", o2.air_current  );}
00123  if (string(&name[0]) == "O2_water_created" ){ o2.water_created  = temp; pc.printf("Value read: %6d \n\r", o2.water_created);}
00124  if (string(&name[0]) == "O2_lambda_max"    ) o2.lambda_max      = temp;
00125  if (string(&name[0]) == "O2_lambda_min"    ) o2.lambda_min      = temp;
00126  if (string(&name[0]) == "O2_pump_over_load") o2.pump_over_load  = temp;
00127  if (string(&name[0]) == "O2_pump_up"       ) o2.pump_up         = temp;
00128  if (string(&name[0]) == "O2_pump_dual_on"  ) o2.pump_dual_on    = temp;
00129  if (string(&name[0]) == "O2_pump_dual_off" ) o2.pump_dual_off   = temp; 
00130 
00131  if (string(&name[0]) == "CAP_voltage_low"  ) cap.voltage_low   = temp;
00132  if (string(&name[0]) == "CAP_voltage_mid"  ) cap.voltage_mid   = temp;               
00133  if (string(&name[0]) == "CAP_voltage_high" ) cap.voltage_high  = temp;
00134  if (string(&name[0]) == "CAP_voltage_max"  ) cap.voltage_max   = temp;
00135  if (string(&name[0]) == "CAP_voltage_down" ) cap.voltage_down  = temp;
00136  if (string(&name[0]) == "CAP_current_low"  ) cap.current_low   = temp;
00137  if (string(&name[0]) == "CAP_current_mid"  ) cap.current_mid   = temp;
00138  if (string(&name[0]) == "CAP_current_high" ) cap.current_high  = temp;
00139 
00140  if (string(&name[0]) == "FC_temp_max"      ) fc.temp_max       = temp;
00141  if (string(&name[0]) == "FC_voltage_max"   ) fc.voltage_max    = temp;
00142  
00143  if (string(&name[0]) == "H2O_fan_over_pwm" ) h2o.fan_over_pwm  = temp;
00144  if (string(&name[0]) == "H2O_fan_p"        ) h2o.fan_p         = temp;
00145  if (string(&name[0]) == "H2O_fan_i"        ) h2o.fan_i         = temp;
00146  if (string(&name[0]) == "H2O_fan_thr"      ) h2o.fan_thr       = temp;
00147  if (string(&name[0]) == "H2O_fan_up"       ) h2o.fan_up        = temp;
00148  if (string(&name[0]) == "H2O_pump_over_pwm") h2o.pump_over_pwm = temp;
00149  if (string(&name[0]) == "H2O_pump_p"       ) h2o.pump_p        = temp;
00150  if (string(&name[0]) == "H2O_pump_i"       ) h2o.pump_i        = temp;
00151  if (string(&name[0]) == "H2O_pump_min"     ) h2o.pump_min      = temp;
00152  if (string(&name[0]) == "H2O_pump_up"      ) h2o.pump_up       = temp;
00153 
00154  if (string(&name[0]) == "MOSFET_temp_max"  ) mosfet.temp_max   = temp;
00155 }       
00156     
00157 
00158 // T - Funktions
00159 //----------------------------------------------------------------------------------------------
00160 
00161 void T_1()
00162  {
00163   o2.water_in = 13235 * o2.rh_in / (o2.temp_in + 2731) * pow(10.0,(7.5 * o2.temp_in)/((2373 + o2.temp_in))-1); 
00164  
00165   o2.water_in = clamp(o2.water_in,0,4000,&error.t_1);
00166  }
00167 
00168 void T_2()
00169  {
00170   if (o2.pump_load_act > 0) {
00171                                 o2.delta_t   = o2.temp_out - h2o.temp_out; 
00172                                 o2.temp_calc = o2.temp_out;
00173                                }
00174    else 
00175                                {
00176                                 o2.temp_calc = o2.delta_t  + h2o.temp_out;
00177                                }
00178   
00179   o2.temp_calc = clamp(o2.temp_calc,0,800,&error.t_2);  
00180  }
00181  
00182 void T_3()
00183  { 
00184   o2.water_out = 13235000 / (o2.temp_calc + 2731) * pow(10.0,(7.5 * o2.temp_calc)/((2373 + o2.temp_calc))-1); 
00185   
00186   o2.water_out = clamp(o2.water_out,0,30000,&error.t_3);         
00187  }
00188  
00189 void T_4()
00190  {
00191   o2.water_extracted = o2.water_out - o2.water_in;
00192   o2.air_needed      = o2.water_created * 10000 / o2.water_extracted;   
00193   o2.lambda          = o2.air_needed * 100 / o2.air_current;
00194   
00195   o2.lambda          = clamp(o2.lambda,100,500,&error.t_4);    
00196  }
00197 
00198 void T_5()
00199  {
00200     o2.lambda_delta     = ((o2.lambda_max        + o2.lambda_min) / 2) - o2.lambda;
00201     
00202     o2.lambda_delta_sum =  o2.lambda_delta_sum + (o2.lambda_delta * 0.1);
00203     o2.lambda_delta_sum = clamp(o2.lambda_delta_sum,-2000,7000,&error.t_5);  
00204    
00205     h2o.fan_pwm         = ((h2o.fan_p * o2.lambda_delta) ) + ((h2o.fan_i * o2.lambda_delta_sum / 10) );
00206     h2o.fan_pwm         = clamp( h2o.fan_pwm,0,1000,&error.t_5);
00207  }  
00208  
00209 void T_6()
00210  {  
00211     h2o.pump_pwm   = ((h2o.pump_p * o2.lambda_delta) ) + ((h2o.pump_i * o2.lambda_delta_sum / 10) + h2o.pump_min );
00212     h2o.pump_pwm   = clamp( h2o.pump_pwm,h2o.pump_min,1000,&error.t_6);
00213  }     
00214       
00215 void T_7() 
00216  { 
00217     if(h2o.fan_override) { 
00218                           h2o.fan_pwm_act = h2o.fan_over_pwm;
00219                          }
00220      else                 
00221                          {
00222                           if ((h2o.fan_pwm_act == 0) && (h2o.fan_pwm >= h2o.fan_thr))
00223                             {
00224                              h2o.fan_up_count = h2o.fan_up;  
00225                             }
00226                            else 
00227                             {
00228                              h2o.fan_pwm_act = h2o.fan_pwm;    
00229                             }
00230                          }
00231  
00232   if (h2o.fan_up_count > 0) {h2o.fan_up_count--; h2o.fan_pwm_act = 1000;}
00233   
00234   if (!((h2o.fan_pwm_act >= h2o.fan_thr) & sys.run)){h2o.fan_pwm_act = 0;}                                                        
00235   
00236   h2o.fan_pwm_act = clamp( h2o.fan_pwm_act,0,1000,&error.t_7);
00237  
00238   pwm_4.write(h2o.fan_pwm_act * 0.001);
00239  }
00240 
00241 
00242 void T_8()
00243 {
00244     if(h2o.pump_override){ 
00245                           h2o.pump_pwm_act = h2o.pump_over_pwm;
00246                          }
00247      else                 
00248                          {
00249                           if ((h2o.pump_pwm_act == 0) && (h2o.pump_pwm > 0))
00250                             {
00251                              h2o.pump_up_count = h2o.pump_up;  
00252                             }
00253                            else 
00254                             { 
00255                              h2o.pump_pwm_act = h2o.pump_pwm;                            
00256                             }
00257                          }                         
00258     
00259     if (h2o.pump_up_count > 0) {h2o.pump_up_count--; h2o.pump_pwm_act = 1000;}
00260 
00261     if (!(sys.run)){ h2o.pump_pwm_act = 0;}
00262 
00263     h2o.pump_pwm_act = clamp( h2o.pump_pwm_act,0,1000,&error.t_8); 
00264     
00265     pwm_3.write(h2o.fan_pwm_act * 0.001);                                                        
00266 }
00267 
00268 void T_ALL(){
00269              T_1();
00270              T_2();
00271              T_3();
00272              T_4();
00273              T_5();
00274              T_6();
00275              T_7();
00276              T_8();
00277             }
00278 
00279 // O - Funktions
00280 //----------------------------------------------------------------------------------------------
00281 
00282 
00283 
00284 
00285 void O_1()
00286 {
00287     o2.rh_out_delta = ((o2.rh_out_soll) - o2.rh_out;
00288     o2.rh_out_delta = clamp( o2.rh_out_delta,-100,100,&error.o_1);  
00289     o2.rh_pump_load = 02.rh_p * o2.rh_out_delta;
00290     o2.rh_pump_load = clamp( o2.rh_pump_load,o2.rh_pump_min,o2.rh_pump_max,&error.o_2);
00291 }
00292 
00293 void O_2()
00294 {
00295     if(cap.voltage_act < cap.voltage_mid)
00296     {
00297         if(cap.voltage_act < cap.voltage_low) fc.current_load = cap.current_low;
00298         else fc.current_load = cap.current_mid;
00299     }
00300     else fc.current_load = cap.current_high;
00301     return 0;
00302 }
00303 
00304 bool O_3()
00305 {
00306     o2.lambda_min = 150;
00307     fc.air_needed = fc.current_load * o2.lambda_min *  0.007;
00308     return 0;
00309 }
00310 
00311 bool O_4()
00312 {
00313     if(o2.pump_on == false) o2.pump_load = 0;
00314     return 0;
00315 }
00316 
00317 bool O_5()
00318 {
00319     if(o2.pump_override) o2.pump_load_act = o2.pump_over_load;
00320     else o2.pump_load_act = o2.pump_load;
00321     return 0;
00322 }
00323 
00324 bool O_6()
00325 {
00326     if(o2.pump_load_act < o2.pump_dual_off){o2.pump_dual = false;}
00327     if(o2.pump_load_act > o2.pump_dual_on ){o2.pump_dual = true;}
00328     
00329     if (o2.pump_dual)
00330      {
00331       o2.pump_pwm_1 = o2.pump_load_act * 0.5;
00332       o2.pump_pwm_2 = o2.pump_load_act * 0.5;   
00333      }
00334     else
00335      {
00336       o2.pump_pwm_1 = o2.pump_load_act;
00337       o2.pump_pwm_2 = 0;
00338      }
00339     return 0;
00340 }
00341 
00342 void O_ALL()
00343 {
00344  O_1();
00345  O_2();
00346  O_3();
00347  O_4();
00348  O_5();
00349  O_6();
00350 }
00351 
00352 
00353 // M - Funktions
00354 //----------------------------------------------------------------------------------------------
00355 
00356 bool M_1()
00357 {
00358     if(o2.pump_on) mosfet.pwm = 0;
00359     mosfet.pwm = mosfet.pwm + ( ((fc.current_load - fc.current_act) * mosfet.i) / 100000);
00360     if(sys.load_act == false) mosfet.pwm = 0;
00361     return 0;
00362 }
00363 
00364 bool M_2()
00365 {
00366     sys.mos_overtemp = (mosfet.temp_act < mosfet.temp_max);
00367     return 0;
00368 }
00369 
00370 bool M_3()
00371 {
00372     if(sys.mosfet && sys.run) mosfet.pwm_act = mosfet.pwm;
00373     else mosfet.pwm_act = 0;
00374     return 0;
00375 }
00376 
00377 // S - Funktions
00378 //----------------------------------------------------------------------------------------------
00379 
00380 bool S_1()
00381 {
00382     sys.run = (switches.master && switches.safety);
00383     return 0;
00384 }
00385 
00386 bool S_2()
00387 {
00388     sys.fc_overtemp = (fc.temp_act > fc.temp_max);
00389     return 0;
00390 }
00391 
00392 bool S_3()                                                                              
00393 {
00394     sys.fc_overvoltage = (fc.voltage_act > fc.voltage_max);
00395     return 0;
00396 }
00397 
00398 bool S_4()
00399 {
00400     sys.fuelcell = (sys.temp && sys.voltage);
00401     return 0;
00402 }
00403 
00404 // C - Funktions
00405 //----------------------------------------------------------------------------------------------
00406 
00407 bool C_1()
00408 {
00409     sys.cap_load = (cap.voltage_act < cap.voltage_max);
00410     return 0;
00411 }
00412 
00413 bool C_2()
00414 {
00415     sys.cap_down_load = (cap.voltage_act < cap.voltage_down);
00416     return 0;
00417 }
00418 
00419 bool C_3()
00420 {
00421     sys.cap_voltage_reset = (cap.voltage_act > cap.voltage_max);
00422     return 0;
00423 }
00424 
00425 bool C_4()
00426 {
00427     sys.cap_down_reset = (cap.voltage_act > cap.voltage_down);
00428     return 0;
00429 }
00430 
00431 bool C_5()
00432 {
00433     sys.load = ((switches.drive) || (sys.cap_load && (!switches.capdown)) || (switches.capdown && sys.cap_down_load)); 
00434     return 0;
00435 }
00436 
00437 bool C_6()
00438 {
00439     sys.load_reset = (sys.cap_voltage_reset || (sys.cap_down_reset && switches.capdown));
00440     return 0;
00441 }
00442 
00443 bool C_7()
00444 {
00445     if(sys.load) sys.load_act = true;
00446     if(sys.load_reset) sys.load_act = false;
00447     return 0;
00448 }
00449 
00450 bool C_8()
00451 {
00452     o2.pump_on = (sys.mosfet && sys.run && sys.fuelcell && sys.load_act);
00453     return 0;
00454 }
00455 
00456 
00457 void read_inputs()
00458 {
00459  char channel = sys.count%4; 
00460   
00461   switch (channel)
00462      {
00463       case 0 :  switches.drive       = mp2xD;     
00464                 sys.h2_analog        = mp2yA.read_u16()  /2;
00465                 fc.voltage_act       = mp1yA.read_u16()  /1.638;
00466                 fc.current_act       = current.read_u16()/4;
00467                 break;
00468       
00469       case 1 :  sys.safety_V         = mp2xA.read_u16() /4;  
00470                 switches.safety      = (sys.safety_V > 8000);
00471                 sys.current_out      = mp2yA.read_u16()  /5; 
00472                 cap.voltage_act      = mp1yA.read_u16()  /1.638;
00473                 fc.current_act       = current.read_u16()/4;
00474                 break;
00475      
00476       case 2 :  switches.capdown     = !mp2xD;     
00477                 mosfet.temp_act      = mosfet.temp_act   + ((((59000-mp2yA.read_u16())/90) - mosfet.temp_act  ) >> 3);
00478                 fc.voltage_act       = mp1yA.read_u16()  /1.638;
00479                 fc.current_act       = current.read_u16()/4;
00480                 break;
00481                 
00482       case 3 :  switches.master      = mp2xD;     
00483                 fc.temp_act          = fc.temp_act + ((((61800-mp2yA.read_u16())/90) - fc.temp_act) >> 3);
00484                 h2o.temp_out         = fc.temp_act;
00485                 cap.voltage_act      = mp1yA.read_u16()  /1.638;
00486                 fc.current_act       = current.read_u16()/4;
00487                 break;           
00488      } 
00489   
00490   mp_A = ( (channel + 1)       & 0x01);       // A setzen
00491   mp_B = (((channel + 1) >> 1) & 0x01);       // B setzen
00492  
00493  }
00494 
00495 void read_i2c_RHTMP(char ad)
00496 {
00497   char input[4];
00498   short RH  = 0;
00499   short TMP = 0;
00500   
00501   i2c.write(ad<<1, 0x00  , 1);               //wake up
00502   i2c.read (ad<<1, input , 4);               //read out
00503             
00504   RH    = (( 1000 * (((input[0]<<8) | (input[1]   )) & 0x3FFF ))>> 14)     ; //transform into    0 - 1000 as Integer represents   0.0 - 100.0 %
00505     
00506   TMP   = (( 1650 * (((input[2]<<6) | (input[3]>>2)) & 0x3FFF ))>> 14) -400; //transform into -400 - 1250 as Integer represents -40.0 - 125.0 °C
00507  
00508   if (TMP > -50 && TMP < 800) 
00509    {
00510       if (ad == I2C_IN ){o2.temp_in  = TMP; o2.rh_in  = RH;error.o2_in  = false;};
00511       if (ad == I2C_OUT){o2.temp_out = TMP; o2.rh_out = RH;error.o2_out = false;}; 
00512    } 
00513     else 
00514    {
00515       if (ad == I2C_IN ){error.o2_in  = true;};
00516       if (ad == I2C_OUT){error.o2_out = true;};     
00517    } 
00518 }
00519 
00520 
00521 void timetable()
00522 {
00523   switch (sys.count)
00524     {       
00525       case   0: read_i2c_RHTMP(I2C_IN)  ; break;
00526       case 125: T_ALL()                 ; break; 
00527       case 250: read_i2c_RHTMP(I2C_OUT) ; break;
00528       case 375: O_ALL()                 ; break;
00529       case 500: read_i2c_RHTMP(I2C_IN)  ; break;
00530       case 625: T_ALL()                 ; break;
00531       case 750: read_i2c_RHTMP(I2C_OUT) ; break;
00532       case 875: O_ALL()                 ; break;
00533     }        
00534     
00535   read_inputs();
00536   
00537   sys.count ++; 
00538      
00539   if (sys.count >= 1000){sys.count=0; led_1 = !led_1;};
00540     
00541 }
00542 
00543 
00544 
00545 
00546 int main()
00547 {
00548   pc.baud(115200);
00549   i2c.frequency(400000);   
00550 
00551   pwm_6.pulsewidth(1.0/PWM_Frequenz); //Mosfet
00552 //pwm_5.pulsewidth(1.0/PWM_Frequenz); //H2 Purge
00553   pwm_4.pulsewidth(1.0/PWM_Frequenz); //H2O Fan
00554   pwm_3.pulsewidth(1.0/PWM_Frequenz); //H20 Pump
00555   pwm_2.pulsewidth(1.0/PWM_Frequenz); //Air Pump 2
00556   pwm_1.pulsewidth(1.0/PWM_Frequenz); //Air Pump 2
00557   
00558 
00559   load_cfg();
00560  
00561   while(1) 
00562     {
00563         timetable();
00564     }
00565 }