Jon Freeman
Electric Locomotive control system. Touch screen driver control, includes regenerative braking, drives 4 brushless motors, displays speed MPH, system volts and power
Dependencies: BSP_DISCO_F746NG FastPWM LCD_DISCO_F746NG SD_DISCO_F746NG TS_DISCO_F746NG mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // Electric Locomotive Controller 00002 // Jon Freeman B. Eng Hons 00003 00004 // Last Updated 13 November 2017 00005 00006 // Touch Screen Loco 2017 - WITH SD card data logger functions 00007 00008 // This code runs on STM 32F746NG DISCO module, high performance ARM Cortex with touch screen 00009 // ffi on ST module -> https://developer.mbed.org/platforms/ST-Discovery-F746NG/ 00010 // Board plugs onto simple mother-board containing low voltage power supplies, interfacing buffers, connectors etc. 00011 // See www.jons-workshop.com ffi on hardware. 00012 00013 // Design provides PWM outputs to drive up to four brushless motor drive modules, each able to return speed information. 00014 // Output signals are dual PWM, one to set max motor voltage, other to set max motor current. 00015 // This code as supplied uses current control to drive locomotive. This means that drive fader acts as a Torque, not Speed, Demand control. 00016 // Regenerative braking is included in the design. 00017 // NOTE that when braking, the motor supply rail voltage will be lifted. Failure to design-in some type of 'surplus power dump' 00018 // may result in over-voltage damage to batteries or power electronics. 00019 00020 #include "mbed.h" 00021 #include "FastPWM.h" 00022 #include "TS_DISCO_F746NG.h" 00023 #include "LCD_DISCO_F746NG.h" 00024 //#include "SD_DISCO_F746NG.h" // SD card stuff now in separate file sd_card.cpp 00025 #include "Electric_Loco.h" 00026 00027 // Design Topology 00028 // This F746NG is the single loco control computer. 00029 // Assumed 4 motor controllers driven from same signal set via multiple opto / buffers 00030 // Outputs are : - 00031 // FastPWM maxv on D12 - in drive, sets motor volts to pwm proportion of available volts. Also used in regen braking 00032 // FastPWM maxi on D11 - used to set upper bound on motor current, used as analogue out to set current limit on motor driver 00033 // DigitalOut reverse (D7) - D6,7 select fwd, rev, brake, parking brake 00034 // DigitalOut forward (D6) 00035 // Inputs are : - 00036 // AnalogIn ht_amps_ain (A0); // Jan 2017 00037 // AnalogIn ht_volts_ain (A1); // Jan 2017 00038 // InterruptIn mot4hall (D2); 00039 // InterruptIn mot3hall (D3); 00040 // InterruptIn mot2hall (D4); 00041 // InterruptIn mot1hall (D5); 00042 00043 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : - 00044 FWD(A5) REV(A4) PWM Action 00045 0 0 0 'Handbrake' - energises motor to not move 00046 0 0 1 'Handbrake' - energises motor to not move 00047 0 1 0 Reverse0 00048 0 1 1 Reverse1 00049 00050 1 0 0 Forward0 00051 1 0 1 Forward1 00052 1 1 0 Regen Braking 00053 1 1 1 Regen Braking 00054 */ 00055 00056 LCD_DISCO_F746NG lcd; 00057 TS_DISCO_F746NG touch_screen; 00058 //SD_DISCO_F746NG sd; // SD card stuff now in sd_card.cpp 00059 00060 FastPWM maxv (D12, 1), 00061 maxi (D11, 1); // pin, prescaler value 00062 Serial pc (USBTX, USBRX); // Comms to 'PuTTY' or similar comms programme on pc 00063 00064 DigitalOut reverse_pin (D7); // 00065 DigitalOut forward_pin (D6); //these two decode to fwd, rev, regen_braking and park 00066 DigitalOut GfetT2 (D14); // a horn 00067 DigitalOut GfetT1 (D15); // another horn 00068 DigitalOut led_grn (LED1); // the only on board user led 00069 00070 DigitalIn f_r_switch (D0); // Reads position of centre-off ignition switch 00071 //DigitalIn spareio_d8 (D8); 00072 //DigitalOut throttle_servo_pulse_out (D8); // now defined in throttle.cpp 00073 DigitalIn spareio_d9 (D9); 00074 DigitalIn spareio_d10 (D10); // D8, D9, D10 wired to jumper on pcb - not used to Apr 2017 00075 00076 AnalogIn ht_volts_ain (A0); // Jan 2017 00077 AnalogIn ht_amps_ain (A1); // Jan 2017 00078 AnalogIn spare_ain2 (A2); 00079 AnalogIn spare_ain3 (A3); 00080 AnalogIn spare_ain4 (A4); // hardware on pcb for these 3 spare analogue inputs - not used to Apr 2017 00081 //AnalogIn spare_ain5 (A5); // causes display flicker ! 00082 00083 InterruptIn mot4hall (D2); // One Hall sensor signal from each motor fed back to measure speed 00084 InterruptIn mot3hall (D3); 00085 InterruptIn mot2hall (D4); 00086 InterruptIn mot1hall (D5); 00087 00088 extern int get_button_press (struct point & pt) ; 00089 extern void displaytext (int x, int y, const int font, uint32_t BCol, uint32_t TCol, char * txt) ; 00090 extern void displaytext (int x, int y, const int font, char * txt) ; 00091 extern void displaytext (int x, int y, char * txt) ; 00092 extern void setup_buttons () ; 00093 extern void draw_numeric_keypad (int colour) ; 00094 extern void draw_button_hilight (int bu, int colour) ; 00095 extern void read_presses (int * a) ; 00096 extern void read_keypresses (struct ky_bd & a) ; 00097 extern void SliderGraphic (struct slide & q) ; 00098 extern void vm_set () ; 00099 extern void update_meters (double, double, double) ; 00100 extern void command_line_interpreter () ; 00101 00102 extern int throttle (double, double) ; // called from main every 31ms 00103 00104 extern void update_SD_card () ; // Hall pulse total updated once per sec and saved in blocks of 128 to SD card 00105 extern bool read_SD_state () ; 00106 extern bool mainSDtest(); 00107 00108 static const int 00109 DAMPER_DECAY = 42, // Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel 00110 MAF_PTS = 140, // Moving Average Filter points. Filters reduce noise on volatage and current readings 00111 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear 00112 // PWM_HZ = 2000, // Used this to experiment on much bigger motor 00113 MAX_PWM_TICKS = 108000000 / PWM_HZ, // 108000000 for F746N, due to cpu clock = 216 MHz 00114 FWD = 0, 00115 REV = ~FWD; 00116 00117 //from .h struct slide { int position; int oldpos; int state; int direction; bool recalc_run; bool handbrake_slipping; double handbrake_effort; double loco_speed } ; 00118 struct slide slider ; 00119 00120 00121 int V_maf[MAF_PTS + 2], I_maf[MAF_PTS + 2], maf_ptr = 0; 00122 //uint32_t Hall_pulse[8] = {0,0,0,0,0,0,0,0}; // more than max number of motors 00123 uint32_t Hall_pulse[8] = {1,1,1,1,1,1,1,1}; // more than max number of motors 00124 uint32_t historic_distance = 0; 00125 00126 bool qtrsec_trig = false; 00127 bool trigger_current_read = false; 00128 volatile bool trigger_32ms = false; 00129 00130 double last_pwm = 0.0; 00131 00132 class speed_measurement // Interrupts at qtr sec cause read of Hall_pulse counters which are incremented by transitions of Hall inputs 00133 { 00134 static const int SPEED_AVE_PTS = 9; // AVE_PTS - points in moving average filters 00135 int speed_maf_mem [(SPEED_AVE_PTS + 1) * 2][NUMBER_OF_MOTORS], 00136 latest_counter_read[NUMBER_OF_MOTORS], 00137 prev_counter_read[NUMBER_OF_MOTORS], 00138 mafptr; 00139 int raw_filtered () ; // sum of count for all motors 00140 00141 public: 00142 00143 speed_measurement () { 00144 memset(speed_maf_mem, 0, sizeof(speed_maf_mem)); 00145 mafptr = 0; 00146 memset (latest_counter_read, 0, sizeof(latest_counter_read)); 00147 memset (prev_counter_read, 0, sizeof(prev_counter_read)); 00148 } // constructor 00149 int raw_filtered (int) ; // count for one motor 00150 int RPM () ; 00151 double MPH () ; 00152 void qtr_sec_update () ; 00153 uint32_t metres_travelled (); 00154 uint32_t pulse_total (); 00155 } 00156 speed ; 00157 00158 int speed_measurement::raw_filtered () // sum of count for all motors 00159 { 00160 int result = 0, a, b; 00161 for (b = 0; b < NUMBER_OF_MOTORS; b++) { 00162 for (a = 0; a < SPEED_AVE_PTS; a++) { 00163 result += speed_maf_mem[a][b]; 00164 } 00165 } 00166 return result; 00167 } 00168 00169 int speed_measurement::raw_filtered (int motor) // count for one motor 00170 { 00171 int result = 0, a; 00172 for (a = 0; a < SPEED_AVE_PTS; a++) { 00173 result += speed_maf_mem[a][motor]; 00174 } 00175 return result; 00176 } 00177 00178 double speed_measurement::MPH () 00179 { 00180 return rpm2mph * (double)RPM(); 00181 } 00182 00183 int speed_measurement::RPM () 00184 { 00185 int rpm = raw_filtered (); 00186 rpm *= 60 * 4; // 60 sec per min, 4 quarters per sec, result pulses per min 00187 rpm /= (SPEED_AVE_PTS * NUMBER_OF_MOTORS * 8); // 8 transitions counted per rev 00188 return rpm; 00189 } 00190 00191 void speed_measurement::qtr_sec_update () // this to be called every quarter sec to read counters and update maf 00192 { 00193 mafptr++; 00194 if (mafptr >= SPEED_AVE_PTS) 00195 mafptr = 0; 00196 for (int a = 0; a < NUMBER_OF_MOTORS; a++) { 00197 prev_counter_read[a] = latest_counter_read[a]; 00198 latest_counter_read[a] = Hall_pulse[a]; 00199 speed_maf_mem[mafptr][a] = latest_counter_read[a] - prev_counter_read[a]; 00200 } 00201 } 00202 00203 uint32_t speed_measurement::metres_travelled () 00204 { 00205 return pulse_total() / (int)PULSES_PER_METRE; 00206 } 00207 00208 uint32_t speed_measurement::pulse_total () 00209 { 00210 return historic_distance + Hall_pulse[0] + Hall_pulse[1] + Hall_pulse[2] + Hall_pulse[3]; 00211 } 00212 00213 uint32_t get_pulse_total () { // called by SD card code 00214 return speed.pulse_total(); 00215 } 00216 00217 void set_V_limit (double p) // Sets max motor voltage 00218 { 00219 if (p < 0.0) 00220 p = 0.0; 00221 if (p > 1.0) 00222 p = 1.0; 00223 last_pwm = p; 00224 p *= 0.95; // need limit, ffi see MCP1630 data 00225 p = 1.0 - p; // because pwm is wrong way up 00226 maxv.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output on pin D12 inverted motor pwm 00227 } 00228 00229 void set_I_limit (double p) // Sets max motor current 00230 { 00231 int a; 00232 if (p < 0.0) 00233 p = 0.0; 00234 if (p > 1.0) 00235 p = 1.0; 00236 a = (int)(p * MAX_PWM_TICKS); 00237 if (a > MAX_PWM_TICKS) 00238 a = MAX_PWM_TICKS; 00239 if (a < 0) 00240 a = 0; 00241 maxi.pulsewidth_ticks (a); // PWM output on pin D12 inverted motor pwm 00242 } 00243 00244 double read_ammeter () 00245 { 00246 int a = 0; 00247 for (int b = 0; b < MAF_PTS; b++) 00248 a += I_maf[b]; 00249 a /= MAF_PTS; 00250 double i = (double) a; 00251 return (i * 95.0 / 32768.0) - 95.0 + 0.46; // fiddled to suit current module 00252 } 00253 00254 double read_voltmeter () 00255 { 00256 int a = 0; 00257 for (int b = 0; b < MAF_PTS; b++) 00258 a += V_maf[b]; 00259 a /= MAF_PTS; 00260 double i = (double) a; 00261 return (i / 617.75) + 0.3; // fiddled to suit current module 00262 } 00263 00264 // Interrupt Service Routines 00265 00266 void ISR_mot1_hall_handler () // read motor position pulse signals from up to six motors 00267 { 00268 Hall_pulse[0]++; 00269 } 00270 void ISR_mot2_hall_handler () 00271 { 00272 Hall_pulse[1]++; 00273 } 00274 void ISR_mot3_hall_handler () 00275 { 00276 Hall_pulse[2]++; 00277 } 00278 void ISR_mot4_hall_handler () 00279 { 00280 Hall_pulse[3]++; 00281 } 00282 void ISR_mot5_hall_handler () // If only 4 motors this never gets used, there is no fifth motor 00283 { 00284 Hall_pulse[4]++; 00285 } 00286 void ISR_mot6_hall_handler () // As one above 00287 { 00288 Hall_pulse[5]++; 00289 } 00290 00291 00292 void ISR_current_reader (void) // FIXED at 250us 00293 { 00294 static int ms32 = 0, ms250 = 0; 00295 trigger_current_read = true; // every 250us, i.e. 4kHz NOTE only sets trigger here, readings taken in main loop 00296 ms32++; 00297 if (ms32 > 124) { 00298 ms32 = 0; 00299 trigger_32ms = true; 00300 ms250++; 00301 if (ms250 > 7) { 00302 ms250 = 0; 00303 qtrsec_trig = true; 00304 } 00305 } 00306 } 00307 00308 /*void ISR_tick_32ms (void) // 00309 { 00310 trigger_32ms = true; 00311 } 00312 void ISR_tick_250ms (void) 00313 { 00314 qtrsec_trig = true; 00315 } 00316 */ 00317 // End of Interrupt Service Routines 00318 00319 00320 bool inlist (struct ky_bd & a, int key) 00321 { 00322 int i = 0; 00323 while (i < a.count) { 00324 if (key == a.ky[i].keynum) 00325 return true; 00326 i++; 00327 } 00328 return false; 00329 } 00330 00331 00332 void stuff_to_do_every_250us () // Take readings of system voltage and current 00333 { 00334 if (!trigger_current_read) 00335 return; 00336 trigger_current_read = false; 00337 I_maf[maf_ptr] = ht_amps_ain.read_u16(); 00338 V_maf[maf_ptr] = ht_volts_ain.read_u16(); 00339 maf_ptr++; 00340 if (maf_ptr > MAF_PTS - 1) 00341 maf_ptr = 0; 00342 } 00343 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : - 00344 FWD(A5) REV(A4) PWM Action 00345 0 0 0 'Handbrake' - energises motor to not move 00346 0 0 1 'Handbrake' - energises motor to not move 00347 0 1 0 Reverse0 00348 0 1 1 Reverse1 00349 00350 1 0 0 Forward0 00351 1 0 1 Forward1 00352 1 1 0 Regen Braking 00353 1 1 1 Regen Braking 00354 */ 00355 void set_run_mode (int mode) 00356 { // NOTE Nov 2017 - Handbrake not implemented 00357 if (mode == HANDBRAKE_SLIPPING) slider.handbrake_slipping = true; 00358 else slider.handbrake_slipping = false; 00359 switch (mode) { 00360 // STATES, INACTIVE, RUN, NEUTRAL_DRIFT, REGEN_BRAKE, PARK}; 00361 // case HANDBRAKE_SLIPPING: 00362 // break; 00363 case PARK: // PARKED new rom code IS now finished. 00364 forward_pin = 0; 00365 reverse_pin = 0; 00366 slider.state = mode; 00367 set_V_limit (0.075); // was 0.1 00368 set_I_limit (slider.handbrake_effort); 00369 break; 00370 case REGEN_BRAKE: // BRAKING, pwm affects degree 00371 forward_pin = 1; 00372 reverse_pin = 1; 00373 slider.state = mode; 00374 break; 00375 case NEUTRAL_DRIFT: 00376 slider.state = mode; 00377 set_I_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch 00378 set_V_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch 00379 break; 00380 case RUN: 00381 if (slider.direction) { 00382 forward_pin = 0; 00383 reverse_pin = 1; 00384 } else { 00385 forward_pin = 1; 00386 reverse_pin = 0; 00387 } 00388 slider.state = mode; 00389 break; 00390 default: 00391 break; 00392 } 00393 } 00394 00395 int main() 00396 { 00397 int c_5 = 0, seconds = 0, minutes = 0; 00398 double electrical_power_Watt = 0.0; 00399 ky_bd kybd_a, kybd_b; 00400 memset (&kybd_a, 0, sizeof(kybd_a)); 00401 memset (&kybd_b, 0, sizeof(kybd_b)); 00402 00403 // spareio_d8.mode (PullUp); now output driving throttle servo 00404 spareio_d9.mode (PullUp); 00405 spareio_d10.mode(PullUp); 00406 00407 Ticker tick250us; 00408 // Ticker tick32ms; 00409 // Ticker tick250ms; 00410 00411 // Setup User Interrupt Vectors 00412 mot1hall.fall (&ISR_mot1_hall_handler); 00413 mot1hall.rise (&ISR_mot1_hall_handler); 00414 mot2hall.fall (&ISR_mot2_hall_handler); 00415 mot2hall.rise (&ISR_mot2_hall_handler); 00416 mot3hall.fall (&ISR_mot3_hall_handler); 00417 mot3hall.rise (&ISR_mot3_hall_handler); 00418 mot4hall.fall (&ISR_mot4_hall_handler); 00419 mot4hall.rise (&ISR_mot4_hall_handler); 00420 00421 tick250us.attach_us (&ISR_current_reader, 250); // count 125 of these to trig 31.25ms 00422 // tick32ms.attach_us (&ISR_tick_32ms, 32001); 00423 // tick32ms.attach_us (&ISR_tick_32ms, 31250); // then count 8 pulses per 250ms 00424 // tick250ms.attach_us (&ISR_tick_250ms, 250002); 00425 pc.baud (9600); 00426 GfetT1 = 0; 00427 GfetT2 = 0; // two output bits for future use driving horns 00428 if (f_r_switch) 00429 slider.direction = FWD; // make decision from key switch position here 00430 else 00431 slider.direction = REV; // make decision from key switch position here 00432 00433 // max_pwm_ticks = SystemCoreClock / (2 * PWM_HZ); // prescaler min value is 2, or so it would seem. SystemCoreClock returns 216000000 on F746NG board 00434 maxv.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz 00435 maxi.period_ticks (MAX_PWM_TICKS + 1); 00436 set_I_limit (0.0); 00437 set_V_limit (0.0); 00438 00439 pc.printf ("Jon's Touch Screen Loco 2017 sytem starting up %s\r\n", slider.direction ? "Forward":"Reverse"); 00440 uint8_t lcd_status = touch_screen.Init(lcd.GetXSize(), lcd.GetYSize()); 00441 if (lcd_status != TS_OK) { 00442 lcd.Clear(LCD_COLOR_RED); 00443 lcd.SetBackColor(LCD_COLOR_RED); 00444 lcd.SetTextColor(LCD_COLOR_WHITE); 00445 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE); 00446 wait (20); 00447 } else { 00448 lcd.Clear(LCD_COLOR_DARKBLUE); 00449 lcd.SetBackColor(LCD_COLOR_GREEN); 00450 lcd.SetTextColor(LCD_COLOR_WHITE); 00451 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE); 00452 } 00453 00454 lcd.SetFont(&Font16); 00455 lcd.Clear(LCD_COLOR_LIGHTGRAY); 00456 setup_buttons(); // draws buttons 00457 00458 slider.oldpos = 0; 00459 slider.loco_speed = 0.0; 00460 slider.handbrake_effort = 0.1; 00461 slider.position = MAX_POS - 2; // Low down in REGEN_BRAKE position - NOT to power-up in PARK 00462 SliderGraphic (slider); // sets slider.state to value determined by slider.position 00463 set_run_mode (REGEN_BRAKE); // sets slider.mode 00464 00465 lcd.SetBackColor(LCD_COLOR_DARKBLUE); 00466 00467 vm_set(); // Draw 3 analogue meter movements, speedo, voltmeter, ammeter 00468 00469 mainSDtest(); 00470 00471 double torque_req = 0.0; 00472 bool toggle32ms = false; 00473 // Main loop 00474 while(1) { // 00475 struct ky_bd * present_kybd, * previous_kybd; 00476 bool sliderpress = false; 00477 command_line_interpreter () ; // Do any actions from command line via usb link 00478 stuff_to_do_every_250us () ; 00479 00480 if (trigger_32ms == true) { // Stuff to do every 32 milli secs 00481 trigger_32ms = false; 00482 00483 // CALL THROTTLE HERE - why here ? Ah yes, this initiates servo pulse. Need steady stream of servo pulses even when nothing changes. 00484 throttle (torque_req, 2.3); 00485 00486 toggle32ms = !toggle32ms; 00487 if (toggle32ms) { 00488 present_kybd = &kybd_a; 00489 previous_kybd = &kybd_b; 00490 } else { 00491 present_kybd = &kybd_b; 00492 previous_kybd = &kybd_a; 00493 } 00494 read_keypresses (*present_kybd); 00495 sliderpress = false; 00496 slider.recalc_run = false; 00497 int j = 0; 00498 // if (present2->count > previous_kybd->count) pc.printf ("More presses\r\n"); 00499 // if (present2->count < previous_kybd->count) pc.printf ("Fewer presses\r\n"); 00500 if (present_kybd->count || previous_kybd->count) { // at least one key pressed this time or last time 00501 int k; 00502 double dbl; 00503 // pc.printf ("Keys action may be required"); 00504 // if key in present and ! in previous, found new key press to handle 00505 // if key ! in present and in previous, found new key release to handle 00506 if (inlist(*present_kybd, SLIDER)) { // Finger is on slider, so Update slider graphic here 00507 sliderpress = true; 00508 k = present_kybd->slider_y; // get position of finger on slider 00509 if (slider.state == RUN && k != slider.position) // Finger has moved within RUN range 00510 slider.recalc_run = true; 00511 if (slider.state == RUN && k >= NEUTRAL_VAL) // Finger has moved from RUN to BRAKE range 00512 slider.position = k = NEUTRAL_VAL; // kill drive for rapid reaction to braking 00513 00514 else { // nice slow non-jerky glidey movement required 00515 dbl = (double)(k - slider.position); 00516 dbl /= 13.179; // Where did 13.179 come from ? 00517 if (dbl < 0.0) 00518 dbl -= 1.0; 00519 if (dbl > 0.0) 00520 dbl += 1.0; 00521 slider.position += (int)dbl; 00522 } 00523 SliderGraphic (slider); // sets slider.state to value determined by slider.position 00524 set_run_mode (slider.state); 00525 draw_button_hilight (SLIDER, LCD_COLOR_YELLOW) ; 00526 00527 if (slider.state == REGEN_BRAKE) { 00528 double brake_effort = ((double)(slider.position - NEUTRAL_VAL) 00529 / (double)(MAX_POS - NEUTRAL_VAL)); 00530 // brake_effort normalised to range 0.0 to 1.0 00531 brake_effort *= 0.97; // upper limit to braking effort, observed effect before was quite fierce 00532 pc.printf ("Brake effort %.2f\r\n", brake_effort); 00533 /* set_pwm (brake_effort); */ 00534 set_V_limit (sqrt(brake_effort)); // sqrt gives more linear feel to control 00535 set_I_limit (1.0); 00536 } 00537 } else { // pc.printf ("Slider not touched\r\n"); 00538 } 00539 00540 j = 0; 00541 while (j < present_kybd->count) { // handle new key presses 00542 k = present_kybd->ky[j++].keynum; 00543 if (inlist(*present_kybd, k)) { 00544 switch (k) { // Here for auto-repeat type key behaviour 00545 case 21: // key is 'voltmeter' 00546 // set_V_limit (last_pwm * 1.002 + 0.001); 00547 break; 00548 case 22: // key is 'ammeter' 00549 // set_V_limit (last_pwm * 0.99); 00550 break; 00551 } // endof switch (k) 00552 } // endof if (inlist(*present2, k)) { 00553 if (inlist(*present_kybd, k) && !inlist(*previous_kybd, k)) { 00554 pc.printf ("Handle Press %d\r\n", k); 00555 draw_button_hilight (k, LCD_COLOR_YELLOW) ; 00556 switch (k) { // Handle new touch screen button presses here - single action per press, not autorepeat 00557 case SPEEDO_BUT: // 00558 pc.printf ("Speedometer key pressed %d\r\n", k); 00559 break; 00560 case VMETER_BUT: // 00561 pc.printf ("Voltmeter key pressed %d\r\n", k); 00562 break; 00563 case AMETER_BUT: // 00564 pc.printf ("Ammeter key pressed %d\r\n", k); 00565 break; 00566 default: 00567 pc.printf ("Unhandled keypress %d\r\n", k); 00568 break; 00569 } // endof switch (button) 00570 } 00571 } // endof while - handle new key presses 00572 j = 0; 00573 while (j < previous_kybd->count) { // handle new key releases 00574 k = previous_kybd->ky[j++].keynum; 00575 if (inlist(*previous_kybd, k) && !inlist(*present_kybd, k)) { 00576 pc.printf ("Handle Release %d\r\n", k); 00577 draw_button_hilight (k, LCD_COLOR_DARKBLUE) ; 00578 } 00579 } // endof while - handle new key releases 00580 } // endof at least one key pressed this time or last time 00581 00582 if (sliderpress == false) { // need to glide dead-mans function towards neutral here 00583 if (slider.position < NEUTRAL_VAL) { 00584 slider.position += 1 + (NEUTRAL_VAL - slider.position) / DAMPER_DECAY; 00585 SliderGraphic (slider); 00586 slider.recalc_run = true; 00587 } 00588 } 00589 00590 if (slider.recalc_run) { // range of slider.position in RUN mode is min_pos_() to NEUTRAL_VAL - 1 00591 slider.recalc_run = false; // All RUN power and pwm calcs done here 00592 int b = slider.position; 00593 // double torque_req; // now declared above to be used as parameter for throttle 00594 if (b > NEUTRAL_VAL) 00595 b = NEUTRAL_VAL; 00596 if (b < MIN_POS) // if finger position is above top of slider limit 00597 b = MIN_POS; 00598 b = NEUTRAL_VAL - b; // now got integer going positive for increasing power demand 00599 torque_req = (double) b; 00600 torque_req /= (NEUTRAL_VAL - MIN_POS); // in range 0.0 to 1.0 00601 pc.printf ("torque_rec = %.3f, last_pwm = %.3f\r\n", torque_req, last_pwm); 00602 set_I_limit (torque_req); 00603 if (torque_req < 0.05) 00604 set_V_limit (last_pwm / 2.0); 00605 else { 00606 if (last_pwm < 0.99) 00607 set_V_limit (last_pwm + 0.05); // ramp voltage up rather than slam to max 00608 } 00609 } 00610 } // endof doing 32ms stuff 00611 00612 if (qtrsec_trig == true) { // do every quarter second stuff here 00613 qtrsec_trig = false; 00614 speed.qtr_sec_update (); 00615 double speedmph = speed.MPH(), amps = 0.0 - read_ammeter(), volts = read_voltmeter(); 00616 //static const double mph_2_mm_per_sec = 447.04; // exact 00617 // double mm_travelled_in_qtrsec = speedmph * mph_2_mm_per_sec / 4.0; 00618 slider.loco_speed = speedmph; 00619 electrical_power_Watt = volts * amps; // visible throughout main 00620 update_meters (speedmph, electrical_power_Watt, volts) ; // displays speed, volts and power (volts times amps) 00621 // update_meters (7.5, amps, volts) ; 00622 led_grn = !led_grn; 00623 if (slider.state == PARK) { 00624 if (speedmph > LOCO_HANDBRAKE_ESCAPE_SPEED / 4.0) { 00625 slider.handbrake_effort *= 1.1; 00626 if (slider.handbrake_effort > 0.55) slider.handbrake_effort = 0.55; 00627 set_run_mode (PARK); 00628 pc.printf ("Handbrake slipping, effort %.2f\r\n", slider.handbrake_effort); 00629 } 00630 if (speedmph < 0.02) { 00631 slider.handbrake_effort *= 0.9; 00632 if (slider.handbrake_effort < 0.05) slider.handbrake_effort = 0.05; 00633 set_run_mode (PARK); 00634 pc.printf ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort); 00635 } 00636 } 00637 c_5++; 00638 // Can do stuff once per second here 00639 if(c_5 > 3) { 00640 c_5 = 0; 00641 seconds++; 00642 if (seconds > 59) { 00643 seconds = 0; 00644 minutes++; 00645 // do once per minute stuff here 00646 } // fall back into once per second 00647 // if(SD_state == SD_OK) { 00648 if(read_SD_state() == true) { 00649 uint32_t distance = speed.metres_travelled(); 00650 char dist[20]; 00651 sprintf (dist, "%05d m", distance); 00652 displaytext (236, 226, 2, dist); 00653 update_SD_card (); // Buffers data for SD card, writes when buffer filled 00654 } 00655 // calc_motor_amps( mva); 00656 } // endof if(c_5 > 3 00657 } // endof if (qtrsec_trig == true) { 00658 } // endof while(1) main programme loop 00659 } // endof int main() { 00660 00661
Generated on Sun Jul 17 2022 14:01:42 by
1.7.2