Hochschule München / Mbed 2 deprecated Motorteststand

Vorlage für Projekt

Dependencies:   CD4052 COUNTER RC_PWM Telemetrie_eth_h2m1 mbed

main.cpp@2:c2193c6cdf53, 2017-11-09 (annotated)

Committer:
HMFK03LST1
Date:
Thu Nov 09 14:03:05 2017 +0000
Revision:
2:c2193c6cdf53
Parent:
0:d621f13db8c0 
Vorlage f?r Projekt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HMFK03LST1 0:d621f13db8c0 1 #include "mbed.h"
HMFK03LST1 0:d621f13db8c0 2 #include "components.h"
HMFK03LST1 0:d621f13db8c0 3 #include "CD4052.h"
HMFK03LST1 0:d621f13db8c0 4 #include "Telemetry.h"
HMFK03LST1 0:d621f13db8c0 5 #include "counter.h"
HMFK03LST1 0:d621f13db8c0 6 #include "rc_pwm.h"
HMFK03LST1 0:d621f13db8c0 7
HMFK03LST1 0:d621f13db8c0 8 #define M_PI 3.1415926
HMFK03LST1 0:d621f13db8c0 9
HMFK03LST1 0:d621f13db8c0 10 DigitalOut myled(LED1);
HMFK03LST1 0:d621f13db8c0 11 CD4052 mp(p22, p21, p19, p20);
HMFK03LST1 0:d621f13db8c0 12 Serial debug(USBTX, USBRX);
HMFK03LST1 0:d621f13db8c0 13 Telemetry Network(p29,p30);
HMFK03LST1 0:d621f13db8c0 14 Ticker data_out;
HMFK03LST1 0:d621f13db8c0 15 Ticker slow_timer;
HMFK03LST1 0:d621f13db8c0 16 Ticker fast_timer;
HMFK03LST1 0:d621f13db8c0 17 Counter rpm_c(p23);
HMFK03LST1 0:d621f13db8c0 18 RC_PWM emc(p21);
HMFK03LST1 0:d621f13db8c0 19
HMFK03LST1 0:d621f13db8c0 20
HMFK03LST1 0:d621f13db8c0 21
HMFK03LST1 0:d621f13db8c0 22 char CTL_SRV[] = "192.168.0.15";
HMFK03LST1 0:d621f13db8c0 23 int CTL_PORT = 42042;
HMFK03LST1 0:d621f13db8c0 24 char DB_SERVER[] = "141.39.249.90";
HMFK03LST1 0:d621f13db8c0 25 int DB_PORT = 8887;
HMFK03LST1 0:d621f13db8c0 26
HMFK03LST1 0:d621f13db8c0 27 Endpoint CTL_Server;
HMFK03LST1 0:d621f13db8c0 28 Endpoint DB_Server;
HMFK03LST1 0:d621f13db8c0 29
HMFK03LST1 0:d621f13db8c0 30
HMFK03LST1 0:d621f13db8c0 31 int count;
HMFK03LST1 0:d621f13db8c0 32
HMFK03LST1 0:d621f13db8c0 33
HMFK03LST1 0:d621f13db8c0 34
HMFK03LST1 0:d621f13db8c0 35 void set_sys(void)
HMFK03LST1 0:d621f13db8c0 36 {
HMFK03LST1 0:d621f13db8c0 37 sys.version = 10;
HMFK03LST1 0:d621f13db8c0 38 sys.blade = 2;
HMFK03LST1 0:d621f13db8c0 39 sys.Amax = 25;
HMFK03LST1 0:d621f13db8c0 40 sys.RPMmax = 2000;
HMFK03LST1 0:d621f13db8c0 41 sys.PWMmax = 1900;
HMFK03LST1 0:d621f13db8c0 42 sys.PWMmin = 900;
HMFK03LST1 0:d621f13db8c0 43 sys.t_eng_max = 68;
HMFK03LST1 0:d621f13db8c0 44 sys.air_ro = 1.185 * 10000;
HMFK03LST1 0:d621f13db8c0 45 sys.pwm = 0;
HMFK03LST1 0:d621f13db8c0 46
HMFK03LST1 0:d621f13db8c0 47 emc.min = sys.PWMmin;
HMFK03LST1 0:d621f13db8c0 48 emc.max = sys.PWMmax;
HMFK03LST1 0:d621f13db8c0 49 }
HMFK03LST1 0:d621f13db8c0 50
HMFK03LST1 0:d621f13db8c0 51 void set_cal(void)
HMFK03LST1 0:d621f13db8c0 52 {
HMFK03LST1 0:d621f13db8c0 53 cal.thrust_o = -4 ;
HMFK03LST1 0:d621f13db8c0 54 cal.thrust_s = 0.080 ;
HMFK03LST1 0:d621f13db8c0 55 cal.torque_o = -4 ;
HMFK03LST1 0:d621f13db8c0 56 cal.torque_s = 0.080 ;
HMFK03LST1 0:d621f13db8c0 57 cal.voltage_o = -52 ;
HMFK03LST1 0:d621f13db8c0 58 cal.voltage_s = 0.4290 ;
HMFK03LST1 0:d621f13db8c0 59 cal.current_o = -10650 ;
HMFK03LST1 0:d621f13db8c0 60 cal.current_s = 1.352 ;
HMFK03LST1 0:d621f13db8c0 61 cal.p_prop_o = -8700 ;
HMFK03LST1 0:d621f13db8c0 62 cal.p_prop_s = 0.263 ;
HMFK03LST1 0:d621f13db8c0 63 cal.p_tunnel_o = -8690 ;
HMFK03LST1 0:d621f13db8c0 64 cal.p_tunnel_s = 0.263 ;
HMFK03LST1 0:d621f13db8c0 65 cal.temp_eng_o = -3435 ;
HMFK03LST1 0:d621f13db8c0 66 cal.temp_eng_s = 0.081 ;
HMFK03LST1 0:d621f13db8c0 67 cal.temp_air_o = -3490 ;
HMFK03LST1 0:d621f13db8c0 68 cal.temp_air_s = 0.081 ;
HMFK03LST1 0:d621f13db8c0 69 }
HMFK03LST1 0:d621f13db8c0 70
HMFK03LST1 0:d621f13db8c0 71
HMFK03LST1 0:d621f13db8c0 72
HMFK03LST1 0:d621f13db8c0 73
HMFK03LST1 0:d621f13db8c0 74
HMFK03LST1 0:d621f13db8c0 75 //Helpscreen output
HMFK03LST1 0:d621f13db8c0 76 void help()
HMFK03LST1 0:d621f13db8c0 77 {
HMFK03LST1 0:d621f13db8c0 78 debug.printf("\n*** V %.1f ***\r\n",(float)sys.version/10);
HMFK03LST1 0:d621f13db8c0 79 debug.printf("*** Motor Test command***\r\n\n");
HMFK03LST1 0:d621f13db8c0 80 debug.printf("M = Messure ON\r\n");
HMFK03LST1 0:d621f13db8c0 81 debug.printf("A = Messure OFF\r\n");
HMFK03LST1 0:d621f13db8c0 82 debug.printf("R = Run\r\n");
HMFK03LST1 0:d621f13db8c0 83 debug.printf("S = Stop\r\n");
HMFK03LST1 0:d621f13db8c0 84 }
HMFK03LST1 0:d621f13db8c0 85
HMFK03LST1 0:d621f13db8c0 86
HMFK03LST1 0:d621f13db8c0 87 void get_input()
HMFK03LST1 0:d621f13db8c0 88 {
HMFK03LST1 0:d621f13db8c0 89 char wert;
HMFK03LST1 0:d621f13db8c0 90
HMFK03LST1 0:d621f13db8c0 91 while (debug.readable() == true)
HMFK03LST1 0:d621f13db8c0 92 {
HMFK03LST1 0:d621f13db8c0 93 wert = debug.getc();
HMFK03LST1 0:d621f13db8c0 94 switch(wert)
HMFK03LST1 0:d621f13db8c0 95 {
HMFK03LST1 0:d621f13db8c0 96 case 'M': sys.do_measure = true;
HMFK03LST1 0:d621f13db8c0 97 break;
HMFK03LST1 0:d621f13db8c0 98 case 'A': sys.do_measure = false;
HMFK03LST1 0:d621f13db8c0 99 break;
HMFK03LST1 0:d621f13db8c0 100 case 'R': sys.run = true;
HMFK03LST1 0:d621f13db8c0 101 break;
HMFK03LST1 0:d621f13db8c0 102 case 'S': sys.run = false;
HMFK03LST1 0:d621f13db8c0 103 sys.pwm = 0;
HMFK03LST1 0:d621f13db8c0 104 break;
HMFK03LST1 0:d621f13db8c0 105 case '0': cal.thrust_o = cal.thrust_o - raw.thrust;
HMFK03LST1 0:d621f13db8c0 106 break;
HMFK03LST1 0:d621f13db8c0 107 case '9': cal.thrust_s = cal.thrust_s * (850 / raw.thrust);
HMFK03LST1 0:d621f13db8c0 108 break;
HMFK03LST1 0:d621f13db8c0 109 case 'c': //save sys
HMFK03LST1 0:d621f13db8c0 110 break;
HMFK03LST1 0:d621f13db8c0 111 default : help();
HMFK03LST1 0:d621f13db8c0 112
HMFK03LST1 0:d621f13db8c0 113 }
HMFK03LST1 0:d621f13db8c0 114 }
HMFK03LST1 0:d621f13db8c0 115 }
HMFK03LST1 0:d621f13db8c0 116
HMFK03LST1 0:d621f13db8c0 117
HMFK03LST1 0:d621f13db8c0 118 void read_values(void)
HMFK03LST1 0:d621f13db8c0 119 {
HMFK03LST1 0:d621f13db8c0 120 mp.channel(0);
HMFK03LST1 0:d621f13db8c0 121 raw.thrust = (mp.x_i() * cal.thrust_s ) + cal.thrust_o ;
HMFK03LST1 0:d621f13db8c0 122 raw.p_prop = (mp.y_i() * cal.p_prop_s ) + cal.p_prop_o ;
HMFK03LST1 0:d621f13db8c0 123 wait_us(50);
HMFK03LST1 0:d621f13db8c0 124 mp.channel(1);
HMFK03LST1 0:d621f13db8c0 125 raw.torque = (mp.x_i() * cal.torque_s ) + cal.torque_o ;
HMFK03LST1 0:d621f13db8c0 126 raw.p_tunnel = (mp.y_i() * cal.p_tunnel_s) + cal.p_tunnel_o;
HMFK03LST1 0:d621f13db8c0 127 wait_us(50);
HMFK03LST1 0:d621f13db8c0 128 mp.channel(2);
HMFK03LST1 0:d621f13db8c0 129 raw.current = (mp.x_i() * cal.current_s ) + cal.current_o ;
HMFK03LST1 0:d621f13db8c0 130 raw.temp_air = (mp.y_i() * cal.temp_air_s) + cal.temp_air_o ;
HMFK03LST1 0:d621f13db8c0 131 wait_us(50);
HMFK03LST1 0:d621f13db8c0 132 mp.channel(3);
HMFK03LST1 0:d621f13db8c0 133 raw.voltage = (mp.x_i() * cal.voltage_s ) + cal.voltage_o ;
HMFK03LST1 0:d621f13db8c0 134 raw.temp_eng = (mp.y_i() * cal.temp_eng_s) + cal.temp_eng_o ;
HMFK03LST1 0:d621f13db8c0 135
HMFK03LST1 0:d621f13db8c0 136 }
HMFK03LST1 0:d621f13db8c0 137
HMFK03LST1 0:d621f13db8c0 138 void calc_values(void)
HMFK03LST1 0:d621f13db8c0 139 {
HMFK03LST1 0:d621f13db8c0 140 raw.power_in = (float)raw.voltage / 100 * (float)raw.current / 100 ;
HMFK03LST1 0:d621f13db8c0 141 if (raw.power_in == 0) raw.power_in = 1;
HMFK03LST1 0:d621f13db8c0 142 raw.power_out = (float)raw.torque / 500 * (float)raw.rpm / 60 * M_PI ; // P = 2*Pi* RPS * M
HMFK03LST1 0:d621f13db8c0 143 raw.eta = (float)raw.power_out / (float)raw.power_in * 1000 ;
HMFK03LST1 0:d621f13db8c0 144 raw.thrust_eff= (float)raw.thrust / (float)raw.power_in * 100 ;
HMFK03LST1 0:d621f13db8c0 145 raw.air_ro = sys.air_ro ;
HMFK03LST1 0:d621f13db8c0 146 raw.v_prop = sqrt((float)raw.p_prop) * 20000 / raw.air_ro ;
HMFK03LST1 0:d621f13db8c0 147 raw.v_tunnel = sqrt((float)raw.p_tunnel) * 20000 / raw.air_ro ;
HMFK03LST1 0:d621f13db8c0 148 }
HMFK03LST1 0:d621f13db8c0 149
HMFK03LST1 0:d621f13db8c0 150 void midd_values(void)
HMFK03LST1 0:d621f13db8c0 151 {
HMFK03LST1 0:d621f13db8c0 152 values.thrust = values.thrust + ((float)raw.thrust / 100 - values.thrust ) / 100;
HMFK03LST1 0:d621f13db8c0 153 values.torque = values.torque + ((float)raw.torque / 1000 - values.torque ) / 100;
HMFK03LST1 0:d621f13db8c0 154 values.voltage = values.voltage + ((float)raw.voltage / 1000 - values.voltage ) / 100;
HMFK03LST1 0:d621f13db8c0 155 values.current = values.current + ((float)raw.current / 1000 - values.current ) / 100;
HMFK03LST1 0:d621f13db8c0 156 values.power_in = values.power_in + ((float)raw.power_in / 100 - values.power_in ) / 100;
HMFK03LST1 0:d621f13db8c0 157 values.power_out = values.power_out + ((float)raw.power_out / 100 - values.power_out ) / 100;
HMFK03LST1 0:d621f13db8c0 158 values.eta = values.eta + ((float)raw.eta / 10 - values.eta ) / 100;
HMFK03LST1 0:d621f13db8c0 159 values.thrust_eff = values.thrust_eff + ((float)raw.thrust_eff / 100 - values.thrust_eff) / 100;
HMFK03LST1 0:d621f13db8c0 160 values.p_prop = values.p_prop + ((float)raw.p_prop - values.p_prop ) / 100;
HMFK03LST1 0:d621f13db8c0 161 values.p_tunnel = values.p_tunnel + ((float)raw.p_tunnel - values.p_tunnel ) / 100;
HMFK03LST1 0:d621f13db8c0 162 values.v_prop = values.v_prop + ((float)raw.v_prop / 10 - values.v_prop ) / 100;
HMFK03LST1 0:d621f13db8c0 163 values.v_tunnel = values.v_tunnel + ((float)raw.v_tunnel / 10 - values.v_tunnel ) / 100;
HMFK03LST1 0:d621f13db8c0 164 values.temp_eng = values.temp_eng + ((float)raw.temp_eng / 10 - values.temp_eng ) / 100;
HMFK03LST1 0:d621f13db8c0 165 values.temp_air = values.temp_air + ((float)raw.temp_air / 10 - values.temp_air ) / 100;
HMFK03LST1 0:d621f13db8c0 166 values.rpm = values.rpm + ((float)raw.rpm - values.rpm ) / 8;
HMFK03LST1 0:d621f13db8c0 167 }
HMFK03LST1 0:d621f13db8c0 168
HMFK03LST1 0:d621f13db8c0 169
HMFK03LST1 0:d621f13db8c0 170 void tick_fast(void){
HMFK03LST1 0:d621f13db8c0 171 sys.do_time = true;
HMFK03LST1 0:d621f13db8c0 172 }
HMFK03LST1 0:d621f13db8c0 173
HMFK03LST1 0:d621f13db8c0 174
HMFK03LST1 0:d621f13db8c0 175 void get_data(void){
HMFK03LST1 0:d621f13db8c0 176 if (sys.do_measure == 1)
HMFK03LST1 0:d621f13db8c0 177 {
HMFK03LST1 0:d621f13db8c0 178 sys.measure++;
HMFK03LST1 0:d621f13db8c0 179 switch(sys.measure%4){
HMFK03LST1 0:d621f13db8c0 180 case 0: break;
HMFK03LST1 0:d621f13db8c0 181 case 1: break; //stabilize
HMFK03LST1 0:d621f13db8c0 182 case 2: break; //stabilize
HMFK03LST1 0:d621f13db8c0 183 case 3: sys.measure = 0;
HMFK03LST1 0:d621f13db8c0 184 sys.pwm = sys.pwm + 1;
HMFK03LST1 0:d621f13db8c0 185 break;
HMFK03LST1 0:d621f13db8c0 186 }
HMFK03LST1 0:d621f13db8c0 187 }
HMFK03LST1 0:d621f13db8c0 188 else {sys.measure = 0;}
HMFK03LST1 0:d621f13db8c0 189 }
HMFK03LST1 0:d621f13db8c0 190
HMFK03LST1 0:d621f13db8c0 191
HMFK03LST1 0:d621f13db8c0 192 void do_limit(void){
HMFK03LST1 0:d621f13db8c0 193
HMFK03LST1 0:d621f13db8c0 194 if (values.current > sys.Amax){ //Limit current
HMFK03LST1 0:d621f13db8c0 195 if (sys.C_limit == 1){
HMFK03LST1 0:d621f13db8c0 196 sys.do_measure = 0;
HMFK03LST1 0:d621f13db8c0 197 sys.pwm = 16;
HMFK03LST1 0:d621f13db8c0 198 debug.printf("\n\r!!Overcurrent Shutdown!!\n\r");
HMFK03LST1 0:d621f13db8c0 199 }
HMFK03LST1 0:d621f13db8c0 200 sys.C_limit = 1;
HMFK03LST1 0:d621f13db8c0 201 }
HMFK03LST1 0:d621f13db8c0 202 else
HMFK03LST1 0:d621f13db8c0 203 {sys.C_limit = 0;}
HMFK03LST1 0:d621f13db8c0 204
HMFK03LST1 0:d621f13db8c0 205
HMFK03LST1 0:d621f13db8c0 206 if (values.rpm > sys.RPMmax){ //Limit RPM
HMFK03LST1 0:d621f13db8c0 207 if (sys.RPM_limit == 1){
HMFK03LST1 0:d621f13db8c0 208 sys.do_measure = 0;
HMFK03LST1 0:d621f13db8c0 209 sys.pwm = 16;
HMFK03LST1 0:d621f13db8c0 210 debug.printf("\n\r!!Overrun Shutdown!!\n\r");
HMFK03LST1 0:d621f13db8c0 211 }
HMFK03LST1 0:d621f13db8c0 212 sys.RPM_limit = 1;
HMFK03LST1 0:d621f13db8c0 213 }
HMFK03LST1 0:d621f13db8c0 214 else
HMFK03LST1 0:d621f13db8c0 215 {sys.RPM_limit = 0;}
HMFK03LST1 0:d621f13db8c0 216
HMFK03LST1 0:d621f13db8c0 217
HMFK03LST1 0:d621f13db8c0 218 if (values.temp_eng > sys.t_eng_max){ //Limit Temp
HMFK03LST1 0:d621f13db8c0 219 if (sys.T_limit == 1){
HMFK03LST1 0:d621f13db8c0 220 sys.do_measure = 0;
HMFK03LST1 0:d621f13db8c0 221 sys.pwm = 16;
HMFK03LST1 0:d621f13db8c0 222 debug.printf("\n\r!!Overtemp Shutdown!!\n\r");
HMFK03LST1 0:d621f13db8c0 223 }
HMFK03LST1 0:d621f13db8c0 224 sys.T_limit = 1;
HMFK03LST1 0:d621f13db8c0 225 }
HMFK03LST1 0:d621f13db8c0 226 else
HMFK03LST1 0:d621f13db8c0 227 {sys.T_limit = 0;}
HMFK03LST1 0:d621f13db8c0 228
HMFK03LST1 0:d621f13db8c0 229 }
HMFK03LST1 0:d621f13db8c0 230
HMFK03LST1 0:d621f13db8c0 231
HMFK03LST1 0:d621f13db8c0 232
HMFK03LST1 0:d621f13db8c0 233 void timetable(void){
HMFK03LST1 0:d621f13db8c0 234 sys.fast++;
HMFK03LST1 0:d621f13db8c0 235 switch(sys.fast%4){ // every 1 ms
HMFK03LST1 0:d621f13db8c0 236 case 1: read_values();
HMFK03LST1 0:d621f13db8c0 237 break;
HMFK03LST1 0:d621f13db8c0 238 case 2: calc_values();
HMFK03LST1 0:d621f13db8c0 239 break;
HMFK03LST1 0:d621f13db8c0 240 case 3: midd_values();
HMFK03LST1 0:d621f13db8c0 241 break;
HMFK03LST1 0:d621f13db8c0 242 }
HMFK03LST1 0:d621f13db8c0 243
HMFK03LST1 0:d621f13db8c0 244 if (0 == sys.fast%100){ // every 100 ms
HMFK03LST1 0:d621f13db8c0 245 sys.slow++;
HMFK03LST1 0:d621f13db8c0 246
HMFK03LST1 0:d621f13db8c0 247 do_limit(); //Limit RPM and Current on every slow loop
HMFK03LST1 0:d621f13db8c0 248
HMFK03LST1 0:d621f13db8c0 249 switch(sys.slow%10){
HMFK03LST1 0:d621f13db8c0 250 case 0: emc.set(sys.pwm);
HMFK03LST1 0:d621f13db8c0 251 break;
HMFK03LST1 0:d621f13db8c0 252 case 1: raw.rpm = rpm_c.cps * 60 / sys.blade ;
HMFK03LST1 0:d621f13db8c0 253 break;
HMFK03LST1 0:d621f13db8c0 254 case 2: debug.printf("RPM: %05d mid: %05d\n\r",raw.rpm,values.rpm);
HMFK03LST1 0:d621f13db8c0 255 break;
HMFK03LST1 0:d621f13db8c0 256 case 3: if ( sys.run == true){if ((values.rpm < 200) && (sys.pwm < 16)) sys.pwm++;} //
HMFK03LST1 0:d621f13db8c0 257 if ((sys.run == true) && (sys.do_measure == false)){if (values.rpm > 240) sys.pwm--;}
HMFK03LST1 0:d621f13db8c0 258 break;
HMFK03LST1 0:d621f13db8c0 259 case 9: get_data();
HMFK03LST1 0:d621f13db8c0 260 break;
HMFK03LST1 0:d621f13db8c0 261
HMFK03LST1 0:d621f13db8c0 262 }
HMFK03LST1 0:d621f13db8c0 263 }
HMFK03LST1 0:d621f13db8c0 264 }
HMFK03LST1 0:d621f13db8c0 265
HMFK03LST1 0:d621f13db8c0 266
HMFK03LST1 0:d621f13db8c0 267 int main() {
HMFK03LST1 0:d621f13db8c0 268 debug.baud(115200);
HMFK03LST1 0:d621f13db8c0 269 //Network.InitEthernetConnection();
HMFK03LST1 0:d621f13db8c0 270 //Network.ConnectSocket_UDP_Client();
HMFK03LST1 0:d621f13db8c0 271 CTL_Server.set_address(CTL_SRV, CTL_PORT);
HMFK03LST1 0:d621f13db8c0 272 DB_Server.set_address(DB_SERVER, DB_PORT);
HMFK03LST1 0:d621f13db8c0 273 fast_timer.attach(&tick_fast,0.001);
HMFK03LST1 0:d621f13db8c0 274 set_sys();
HMFK03LST1 0:d621f13db8c0 275 set_cal();
HMFK03LST1 0:d621f13db8c0 276
HMFK03LST1 0:d621f13db8c0 277 while(1) {
HMFK03LST1 0:d621f13db8c0 278
HMFK03LST1 0:d621f13db8c0 279 if(sys.do_time){
HMFK03LST1 0:d621f13db8c0 280 timetable();
HMFK03LST1 0:d621f13db8c0 281 sys.do_time = false;
HMFK03LST1 0:d621f13db8c0 282 }
HMFK03LST1 0:d621f13db8c0 283 }
HMFK03LST1 0:d621f13db8c0 284 }