Vorlage für Projekt
Dependencies: CD4052 COUNTER RC_PWM Telemetrie_eth_h2m1 mbed
main.cpp
- Committer:
- HMFK03LST1
- Date:
- 2017-11-09
- Revision:
- 0:d621f13db8c0
File content as of revision 0:d621f13db8c0:
#include "mbed.h" #include "components.h" #include "CD4052.h" #include "Telemetry.h" #include "counter.h" #include "rc_pwm.h" #define M_PI 3.1415926 DigitalOut myled(LED1); CD4052 mp(p22, p21, p19, p20); Serial debug(USBTX, USBRX); Telemetry Network(p29,p30); Ticker data_out; Ticker slow_timer; Ticker fast_timer; Counter rpm_c(p23); RC_PWM emc(p21); char CTL_SRV[] = "192.168.0.15"; int CTL_PORT = 42042; char DB_SERVER[] = "141.39.249.90"; int DB_PORT = 8887; Endpoint CTL_Server; Endpoint DB_Server; int count; void set_sys(void) { sys.version = 10; sys.blade = 2; sys.Amax = 25; sys.RPMmax = 2000; sys.PWMmax = 1900; sys.PWMmin = 900; sys.t_eng_max = 68; sys.air_ro = 1.185 * 10000; sys.pwm = 0; emc.min = sys.PWMmin; emc.max = sys.PWMmax; } void set_cal(void) { cal.thrust_o = -4 ; cal.thrust_s = 0.080 ; cal.torque_o = -4 ; cal.torque_s = 0.080 ; cal.voltage_o = -52 ; cal.voltage_s = 0.4290 ; cal.current_o = -10650 ; cal.current_s = 1.352 ; cal.p_prop_o = -8700 ; cal.p_prop_s = 0.263 ; cal.p_tunnel_o = -8690 ; cal.p_tunnel_s = 0.263 ; cal.temp_eng_o = -3435 ; cal.temp_eng_s = 0.081 ; cal.temp_air_o = -3490 ; cal.temp_air_s = 0.081 ; } //Helpscreen output void help() { debug.printf("\n*** V %.1f ***\r\n",(float)sys.version/10); debug.printf("*** Motor Test command***\r\n\n"); debug.printf("M = Messure ON\r\n"); debug.printf("A = Messure OFF\r\n"); debug.printf("R = Run\r\n"); debug.printf("S = Stop\r\n"); } void get_input() { char wert; while (debug.readable() == true) { wert = debug.getc(); switch(wert) { case 'M': sys.do_measure = true; break; case 'A': sys.do_measure = false; break; case 'R': sys.run = true; break; case 'S': sys.run = false; sys.pwm = 0; break; case '0': cal.thrust_o = cal.thrust_o - raw.thrust; break; case '9': cal.thrust_s = cal.thrust_s * (850 / raw.thrust); break; case 'c': //save sys break; default : help(); } } } void read_values(void) { mp.channel(0); raw.thrust = (mp.x_i() * cal.thrust_s ) + cal.thrust_o ; raw.p_prop = (mp.y_i() * cal.p_prop_s ) + cal.p_prop_o ; wait_us(50); mp.channel(1); raw.torque = (mp.x_i() * cal.torque_s ) + cal.torque_o ; raw.p_tunnel = (mp.y_i() * cal.p_tunnel_s) + cal.p_tunnel_o; wait_us(50); mp.channel(2); raw.current = (mp.x_i() * cal.current_s ) + cal.current_o ; raw.temp_air = (mp.y_i() * cal.temp_air_s) + cal.temp_air_o ; wait_us(50); mp.channel(3); raw.voltage = (mp.x_i() * cal.voltage_s ) + cal.voltage_o ; raw.temp_eng = (mp.y_i() * cal.temp_eng_s) + cal.temp_eng_o ; } void calc_values(void) { raw.power_in = (float)raw.voltage / 100 * (float)raw.current / 100 ; if (raw.power_in == 0) raw.power_in = 1; raw.power_out = (float)raw.torque / 500 * (float)raw.rpm / 60 * M_PI ; // P = 2*Pi* RPS * M raw.eta = (float)raw.power_out / (float)raw.power_in * 1000 ; raw.thrust_eff= (float)raw.thrust / (float)raw.power_in * 100 ; raw.air_ro = sys.air_ro ; raw.v_prop = sqrt((float)raw.p_prop) * 20000 / raw.air_ro ; raw.v_tunnel = sqrt((float)raw.p_tunnel) * 20000 / raw.air_ro ; } void midd_values(void) { values.thrust = values.thrust + ((float)raw.thrust / 100 - values.thrust ) / 100; values.torque = values.torque + ((float)raw.torque / 1000 - values.torque ) / 100; values.voltage = values.voltage + ((float)raw.voltage / 1000 - values.voltage ) / 100; values.current = values.current + ((float)raw.current / 1000 - values.current ) / 100; values.power_in = values.power_in + ((float)raw.power_in / 100 - values.power_in ) / 100; values.power_out = values.power_out + ((float)raw.power_out / 100 - values.power_out ) / 100; values.eta = values.eta + ((float)raw.eta / 10 - values.eta ) / 100; values.thrust_eff = values.thrust_eff + ((float)raw.thrust_eff / 100 - values.thrust_eff) / 100; values.p_prop = values.p_prop + ((float)raw.p_prop - values.p_prop ) / 100; values.p_tunnel = values.p_tunnel + ((float)raw.p_tunnel - values.p_tunnel ) / 100; values.v_prop = values.v_prop + ((float)raw.v_prop / 10 - values.v_prop ) / 100; values.v_tunnel = values.v_tunnel + ((float)raw.v_tunnel / 10 - values.v_tunnel ) / 100; values.temp_eng = values.temp_eng + ((float)raw.temp_eng / 10 - values.temp_eng ) / 100; values.temp_air = values.temp_air + ((float)raw.temp_air / 10 - values.temp_air ) / 100; values.rpm = values.rpm + ((float)raw.rpm - values.rpm ) / 8; } void tick_fast(void){ sys.do_time = true; } void get_data(void){ if (sys.do_measure == 1) { sys.measure++; switch(sys.measure%4){ case 0: break; case 1: break; //stabilize case 2: break; //stabilize case 3: sys.measure = 0; sys.pwm = sys.pwm + 1; break; } } else {sys.measure = 0;} } void do_limit(void){ if (values.current > sys.Amax){ //Limit current if (sys.C_limit == 1){ sys.do_measure = 0; sys.pwm = 16; debug.printf("\n\r!!Overcurrent Shutdown!!\n\r"); } sys.C_limit = 1; } else {sys.C_limit = 0;} if (values.rpm > sys.RPMmax){ //Limit RPM if (sys.RPM_limit == 1){ sys.do_measure = 0; sys.pwm = 16; debug.printf("\n\r!!Overrun Shutdown!!\n\r"); } sys.RPM_limit = 1; } else {sys.RPM_limit = 0;} if (values.temp_eng > sys.t_eng_max){ //Limit Temp if (sys.T_limit == 1){ sys.do_measure = 0; sys.pwm = 16; debug.printf("\n\r!!Overtemp Shutdown!!\n\r"); } sys.T_limit = 1; } else {sys.T_limit = 0;} } void timetable(void){ sys.fast++; switch(sys.fast%4){ // every 1 ms case 1: read_values(); break; case 2: calc_values(); break; case 3: midd_values(); break; } if (0 == sys.fast%100){ // every 100 ms sys.slow++; do_limit(); //Limit RPM and Current on every slow loop switch(sys.slow%10){ case 0: emc.set(sys.pwm); break; case 1: raw.rpm = rpm_c.cps * 60 / sys.blade ; break; case 2: debug.printf("RPM: %05d mid: %05d\n\r",raw.rpm,values.rpm); break; case 3: if ( sys.run == true){if ((values.rpm < 200) && (sys.pwm < 16)) sys.pwm++;} // if ((sys.run == true) && (sys.do_measure == false)){if (values.rpm > 240) sys.pwm--;} break; case 9: get_data(); break; } } } int main() { debug.baud(115200); //Network.InitEthernetConnection(); //Network.ConnectSocket_UDP_Client(); CTL_Server.set_address(CTL_SRV, CTL_PORT); DB_Server.set_address(DB_SERVER, DB_PORT); fast_timer.attach(&tick_fast,0.001); set_sys(); set_cal(); while(1) { if(sys.do_time){ timetable(); sys.do_time = false; } } }