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
main.cpp
- Committer:
- JonFreeman
- Date:
- 2017-11-13
- Revision:
- 1:8ef34deb5177
- Parent:
- 0:23cc72b18e74
File content as of revision 1:8ef34deb5177:
// Electric Locomotive Controller // Jon Freeman B. Eng Hons // Last Updated 13 November 2017 // Touch Screen Loco 2017 - WITH SD card data logger functions // This code runs on STM 32F746NG DISCO module, high performance ARM Cortex with touch screen // ffi on ST module -> https://developer.mbed.org/platforms/ST-Discovery-F746NG/ // Board plugs onto simple mother-board containing low voltage power supplies, interfacing buffers, connectors etc. // See www.jons-workshop.com ffi on hardware. // Design provides PWM outputs to drive up to four brushless motor drive modules, each able to return speed information. // Output signals are dual PWM, one to set max motor voltage, other to set max motor current. // This code as supplied uses current control to drive locomotive. This means that drive fader acts as a Torque, not Speed, Demand control. // Regenerative braking is included in the design. // NOTE that when braking, the motor supply rail voltage will be lifted. Failure to design-in some type of 'surplus power dump' // may result in over-voltage damage to batteries or power electronics. #include "mbed.h" #include "FastPWM.h" #include "TS_DISCO_F746NG.h" #include "LCD_DISCO_F746NG.h" //#include "SD_DISCO_F746NG.h" // SD card stuff now in separate file sd_card.cpp #include "Electric_Loco.h" // Design Topology // This F746NG is the single loco control computer. // Assumed 4 motor controllers driven from same signal set via multiple opto / buffers // Outputs are : - // FastPWM maxv on D12 - in drive, sets motor volts to pwm proportion of available volts. Also used in regen braking // FastPWM maxi on D11 - used to set upper bound on motor current, used as analogue out to set current limit on motor driver // DigitalOut reverse (D7) - D6,7 select fwd, rev, brake, parking brake // DigitalOut forward (D6) // Inputs are : - // AnalogIn ht_amps_ain (A0); // Jan 2017 // AnalogIn ht_volts_ain (A1); // Jan 2017 // InterruptIn mot4hall (D2); // InterruptIn mot3hall (D3); // InterruptIn mot2hall (D4); // InterruptIn mot1hall (D5); /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : - FWD(A5) REV(A4) PWM Action 0 0 0 'Handbrake' - energises motor to not move 0 0 1 'Handbrake' - energises motor to not move 0 1 0 Reverse0 0 1 1 Reverse1 1 0 0 Forward0 1 0 1 Forward1 1 1 0 Regen Braking 1 1 1 Regen Braking */ LCD_DISCO_F746NG lcd; TS_DISCO_F746NG touch_screen; //SD_DISCO_F746NG sd; // SD card stuff now in sd_card.cpp FastPWM maxv (D12, 1), maxi (D11, 1); // pin, prescaler value Serial pc (USBTX, USBRX); // Comms to 'PuTTY' or similar comms programme on pc DigitalOut reverse_pin (D7); // DigitalOut forward_pin (D6); //these two decode to fwd, rev, regen_braking and park DigitalOut GfetT2 (D14); // a horn DigitalOut GfetT1 (D15); // another horn DigitalOut led_grn (LED1); // the only on board user led DigitalIn f_r_switch (D0); // Reads position of centre-off ignition switch //DigitalIn spareio_d8 (D8); //DigitalOut throttle_servo_pulse_out (D8); // now defined in throttle.cpp DigitalIn spareio_d9 (D9); DigitalIn spareio_d10 (D10); // D8, D9, D10 wired to jumper on pcb - not used to Apr 2017 AnalogIn ht_volts_ain (A0); // Jan 2017 AnalogIn ht_amps_ain (A1); // Jan 2017 AnalogIn spare_ain2 (A2); AnalogIn spare_ain3 (A3); AnalogIn spare_ain4 (A4); // hardware on pcb for these 3 spare analogue inputs - not used to Apr 2017 //AnalogIn spare_ain5 (A5); // causes display flicker ! InterruptIn mot4hall (D2); // One Hall sensor signal from each motor fed back to measure speed InterruptIn mot3hall (D3); InterruptIn mot2hall (D4); InterruptIn mot1hall (D5); extern int get_button_press (struct point & pt) ; extern void displaytext (int x, int y, const int font, uint32_t BCol, uint32_t TCol, char * txt) ; extern void displaytext (int x, int y, const int font, char * txt) ; extern void displaytext (int x, int y, char * txt) ; extern void setup_buttons () ; extern void draw_numeric_keypad (int colour) ; extern void draw_button_hilight (int bu, int colour) ; extern void read_presses (int * a) ; extern void read_keypresses (struct ky_bd & a) ; extern void SliderGraphic (struct slide & q) ; extern void vm_set () ; extern void update_meters (double, double, double) ; extern void command_line_interpreter () ; extern int throttle (double, double) ; // called from main every 31ms extern void update_SD_card () ; // Hall pulse total updated once per sec and saved in blocks of 128 to SD card extern bool read_SD_state () ; extern bool mainSDtest(); static const int DAMPER_DECAY = 42, // Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel MAF_PTS = 140, // Moving Average Filter points. Filters reduce noise on volatage and current readings PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear // PWM_HZ = 2000, // Used this to experiment on much bigger motor MAX_PWM_TICKS = 108000000 / PWM_HZ, // 108000000 for F746N, due to cpu clock = 216 MHz FWD = 0, REV = ~FWD; //from .h struct slide { int position; int oldpos; int state; int direction; bool recalc_run; bool handbrake_slipping; double handbrake_effort; double loco_speed } ; struct slide slider ; int V_maf[MAF_PTS + 2], I_maf[MAF_PTS + 2], maf_ptr = 0; //uint32_t Hall_pulse[8] = {0,0,0,0,0,0,0,0}; // more than max number of motors uint32_t Hall_pulse[8] = {1,1,1,1,1,1,1,1}; // more than max number of motors uint32_t historic_distance = 0; bool qtrsec_trig = false; bool trigger_current_read = false; volatile bool trigger_32ms = false; double last_pwm = 0.0; class speed_measurement // Interrupts at qtr sec cause read of Hall_pulse counters which are incremented by transitions of Hall inputs { static const int SPEED_AVE_PTS = 9; // AVE_PTS - points in moving average filters int speed_maf_mem [(SPEED_AVE_PTS + 1) * 2][NUMBER_OF_MOTORS], latest_counter_read[NUMBER_OF_MOTORS], prev_counter_read[NUMBER_OF_MOTORS], mafptr; int raw_filtered () ; // sum of count for all motors public: speed_measurement () { memset(speed_maf_mem, 0, sizeof(speed_maf_mem)); mafptr = 0; memset (latest_counter_read, 0, sizeof(latest_counter_read)); memset (prev_counter_read, 0, sizeof(prev_counter_read)); } // constructor int raw_filtered (int) ; // count for one motor int RPM () ; double MPH () ; void qtr_sec_update () ; uint32_t metres_travelled (); uint32_t pulse_total (); } speed ; int speed_measurement::raw_filtered () // sum of count for all motors { int result = 0, a, b; for (b = 0; b < NUMBER_OF_MOTORS; b++) { for (a = 0; a < SPEED_AVE_PTS; a++) { result += speed_maf_mem[a][b]; } } return result; } int speed_measurement::raw_filtered (int motor) // count for one motor { int result = 0, a; for (a = 0; a < SPEED_AVE_PTS; a++) { result += speed_maf_mem[a][motor]; } return result; } double speed_measurement::MPH () { return rpm2mph * (double)RPM(); } int speed_measurement::RPM () { int rpm = raw_filtered (); rpm *= 60 * 4; // 60 sec per min, 4 quarters per sec, result pulses per min rpm /= (SPEED_AVE_PTS * NUMBER_OF_MOTORS * 8); // 8 transitions counted per rev return rpm; } void speed_measurement::qtr_sec_update () // this to be called every quarter sec to read counters and update maf { mafptr++; if (mafptr >= SPEED_AVE_PTS) mafptr = 0; for (int a = 0; a < NUMBER_OF_MOTORS; a++) { prev_counter_read[a] = latest_counter_read[a]; latest_counter_read[a] = Hall_pulse[a]; speed_maf_mem[mafptr][a] = latest_counter_read[a] - prev_counter_read[a]; } } uint32_t speed_measurement::metres_travelled () { return pulse_total() / (int)PULSES_PER_METRE; } uint32_t speed_measurement::pulse_total () { return historic_distance + Hall_pulse[0] + Hall_pulse[1] + Hall_pulse[2] + Hall_pulse[3]; } uint32_t get_pulse_total () { // called by SD card code return speed.pulse_total(); } void set_V_limit (double p) // Sets max motor voltage { if (p < 0.0) p = 0.0; if (p > 1.0) p = 1.0; last_pwm = p; p *= 0.95; // need limit, ffi see MCP1630 data p = 1.0 - p; // because pwm is wrong way up maxv.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output on pin D12 inverted motor pwm } void set_I_limit (double p) // Sets max motor current { int a; if (p < 0.0) p = 0.0; if (p > 1.0) p = 1.0; a = (int)(p * MAX_PWM_TICKS); if (a > MAX_PWM_TICKS) a = MAX_PWM_TICKS; if (a < 0) a = 0; maxi.pulsewidth_ticks (a); // PWM output on pin D12 inverted motor pwm } double read_ammeter () { int a = 0; for (int b = 0; b < MAF_PTS; b++) a += I_maf[b]; a /= MAF_PTS; double i = (double) a; return (i * 95.0 / 32768.0) - 95.0 + 0.46; // fiddled to suit current module } double read_voltmeter () { int a = 0; for (int b = 0; b < MAF_PTS; b++) a += V_maf[b]; a /= MAF_PTS; double i = (double) a; return (i / 617.75) + 0.3; // fiddled to suit current module } // Interrupt Service Routines void ISR_mot1_hall_handler () // read motor position pulse signals from up to six motors { Hall_pulse[0]++; } void ISR_mot2_hall_handler () { Hall_pulse[1]++; } void ISR_mot3_hall_handler () { Hall_pulse[2]++; } void ISR_mot4_hall_handler () { Hall_pulse[3]++; } void ISR_mot5_hall_handler () // If only 4 motors this never gets used, there is no fifth motor { Hall_pulse[4]++; } void ISR_mot6_hall_handler () // As one above { Hall_pulse[5]++; } void ISR_current_reader (void) // FIXED at 250us { static int ms32 = 0, ms250 = 0; trigger_current_read = true; // every 250us, i.e. 4kHz NOTE only sets trigger here, readings taken in main loop ms32++; if (ms32 > 124) { ms32 = 0; trigger_32ms = true; ms250++; if (ms250 > 7) { ms250 = 0; qtrsec_trig = true; } } } /*void ISR_tick_32ms (void) // { trigger_32ms = true; } void ISR_tick_250ms (void) { qtrsec_trig = true; } */ // End of Interrupt Service Routines bool inlist (struct ky_bd & a, int key) { int i = 0; while (i < a.count) { if (key == a.ky[i].keynum) return true; i++; } return false; } void stuff_to_do_every_250us () // Take readings of system voltage and current { if (!trigger_current_read) return; trigger_current_read = false; I_maf[maf_ptr] = ht_amps_ain.read_u16(); V_maf[maf_ptr] = ht_volts_ain.read_u16(); maf_ptr++; if (maf_ptr > MAF_PTS - 1) maf_ptr = 0; } /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : - FWD(A5) REV(A4) PWM Action 0 0 0 'Handbrake' - energises motor to not move 0 0 1 'Handbrake' - energises motor to not move 0 1 0 Reverse0 0 1 1 Reverse1 1 0 0 Forward0 1 0 1 Forward1 1 1 0 Regen Braking 1 1 1 Regen Braking */ void set_run_mode (int mode) { // NOTE Nov 2017 - Handbrake not implemented if (mode == HANDBRAKE_SLIPPING) slider.handbrake_slipping = true; else slider.handbrake_slipping = false; switch (mode) { // STATES, INACTIVE, RUN, NEUTRAL_DRIFT, REGEN_BRAKE, PARK}; // case HANDBRAKE_SLIPPING: // break; case PARK: // PARKED new rom code IS now finished. forward_pin = 0; reverse_pin = 0; slider.state = mode; set_V_limit (0.075); // was 0.1 set_I_limit (slider.handbrake_effort); break; case REGEN_BRAKE: // BRAKING, pwm affects degree forward_pin = 1; reverse_pin = 1; slider.state = mode; break; case NEUTRAL_DRIFT: slider.state = mode; set_I_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch set_V_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch break; case RUN: if (slider.direction) { forward_pin = 0; reverse_pin = 1; } else { forward_pin = 1; reverse_pin = 0; } slider.state = mode; break; default: break; } } int main() { int c_5 = 0, seconds = 0, minutes = 0; double electrical_power_Watt = 0.0; ky_bd kybd_a, kybd_b; memset (&kybd_a, 0, sizeof(kybd_a)); memset (&kybd_b, 0, sizeof(kybd_b)); // spareio_d8.mode (PullUp); now output driving throttle servo spareio_d9.mode (PullUp); spareio_d10.mode(PullUp); Ticker tick250us; // Ticker tick32ms; // Ticker tick250ms; // Setup User Interrupt Vectors mot1hall.fall (&ISR_mot1_hall_handler); mot1hall.rise (&ISR_mot1_hall_handler); mot2hall.fall (&ISR_mot2_hall_handler); mot2hall.rise (&ISR_mot2_hall_handler); mot3hall.fall (&ISR_mot3_hall_handler); mot3hall.rise (&ISR_mot3_hall_handler); mot4hall.fall (&ISR_mot4_hall_handler); mot4hall.rise (&ISR_mot4_hall_handler); tick250us.attach_us (&ISR_current_reader, 250); // count 125 of these to trig 31.25ms // tick32ms.attach_us (&ISR_tick_32ms, 32001); // tick32ms.attach_us (&ISR_tick_32ms, 31250); // then count 8 pulses per 250ms // tick250ms.attach_us (&ISR_tick_250ms, 250002); pc.baud (9600); GfetT1 = 0; GfetT2 = 0; // two output bits for future use driving horns if (f_r_switch) slider.direction = FWD; // make decision from key switch position here else slider.direction = REV; // make decision from key switch position here // max_pwm_ticks = SystemCoreClock / (2 * PWM_HZ); // prescaler min value is 2, or so it would seem. SystemCoreClock returns 216000000 on F746NG board maxv.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz maxi.period_ticks (MAX_PWM_TICKS + 1); set_I_limit (0.0); set_V_limit (0.0); pc.printf ("Jon's Touch Screen Loco 2017 sytem starting up %s\r\n", slider.direction ? "Forward":"Reverse"); uint8_t lcd_status = touch_screen.Init(lcd.GetXSize(), lcd.GetYSize()); if (lcd_status != TS_OK) { lcd.Clear(LCD_COLOR_RED); lcd.SetBackColor(LCD_COLOR_RED); lcd.SetTextColor(LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE); wait (20); } else { lcd.Clear(LCD_COLOR_DARKBLUE); lcd.SetBackColor(LCD_COLOR_GREEN); lcd.SetTextColor(LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE); } lcd.SetFont(&Font16); lcd.Clear(LCD_COLOR_LIGHTGRAY); setup_buttons(); // draws buttons slider.oldpos = 0; slider.loco_speed = 0.0; slider.handbrake_effort = 0.1; slider.position = MAX_POS - 2; // Low down in REGEN_BRAKE position - NOT to power-up in PARK SliderGraphic (slider); // sets slider.state to value determined by slider.position set_run_mode (REGEN_BRAKE); // sets slider.mode lcd.SetBackColor(LCD_COLOR_DARKBLUE); vm_set(); // Draw 3 analogue meter movements, speedo, voltmeter, ammeter mainSDtest(); double torque_req = 0.0; bool toggle32ms = false; // Main loop while(1) { // struct ky_bd * present_kybd, * previous_kybd; bool sliderpress = false; command_line_interpreter () ; // Do any actions from command line via usb link stuff_to_do_every_250us () ; if (trigger_32ms == true) { // Stuff to do every 32 milli secs trigger_32ms = false; // CALL THROTTLE HERE - why here ? Ah yes, this initiates servo pulse. Need steady stream of servo pulses even when nothing changes. throttle (torque_req, 2.3); toggle32ms = !toggle32ms; if (toggle32ms) { present_kybd = &kybd_a; previous_kybd = &kybd_b; } else { present_kybd = &kybd_b; previous_kybd = &kybd_a; } read_keypresses (*present_kybd); sliderpress = false; slider.recalc_run = false; int j = 0; // if (present2->count > previous_kybd->count) pc.printf ("More presses\r\n"); // if (present2->count < previous_kybd->count) pc.printf ("Fewer presses\r\n"); if (present_kybd->count || previous_kybd->count) { // at least one key pressed this time or last time int k; double dbl; // pc.printf ("Keys action may be required"); // if key in present and ! in previous, found new key press to handle // if key ! in present and in previous, found new key release to handle if (inlist(*present_kybd, SLIDER)) { // Finger is on slider, so Update slider graphic here sliderpress = true; k = present_kybd->slider_y; // get position of finger on slider if (slider.state == RUN && k != slider.position) // Finger has moved within RUN range slider.recalc_run = true; if (slider.state == RUN && k >= NEUTRAL_VAL) // Finger has moved from RUN to BRAKE range slider.position = k = NEUTRAL_VAL; // kill drive for rapid reaction to braking else { // nice slow non-jerky glidey movement required dbl = (double)(k - slider.position); dbl /= 13.179; // Where did 13.179 come from ? if (dbl < 0.0) dbl -= 1.0; if (dbl > 0.0) dbl += 1.0; slider.position += (int)dbl; } SliderGraphic (slider); // sets slider.state to value determined by slider.position set_run_mode (slider.state); draw_button_hilight (SLIDER, LCD_COLOR_YELLOW) ; if (slider.state == REGEN_BRAKE) { double brake_effort = ((double)(slider.position - NEUTRAL_VAL) / (double)(MAX_POS - NEUTRAL_VAL)); // brake_effort normalised to range 0.0 to 1.0 brake_effort *= 0.97; // upper limit to braking effort, observed effect before was quite fierce pc.printf ("Brake effort %.2f\r\n", brake_effort); /* set_pwm (brake_effort); */ set_V_limit (sqrt(brake_effort)); // sqrt gives more linear feel to control set_I_limit (1.0); } } else { // pc.printf ("Slider not touched\r\n"); } j = 0; while (j < present_kybd->count) { // handle new key presses k = present_kybd->ky[j++].keynum; if (inlist(*present_kybd, k)) { switch (k) { // Here for auto-repeat type key behaviour case 21: // key is 'voltmeter' // set_V_limit (last_pwm * 1.002 + 0.001); break; case 22: // key is 'ammeter' // set_V_limit (last_pwm * 0.99); break; } // endof switch (k) } // endof if (inlist(*present2, k)) { if (inlist(*present_kybd, k) && !inlist(*previous_kybd, k)) { pc.printf ("Handle Press %d\r\n", k); draw_button_hilight (k, LCD_COLOR_YELLOW) ; switch (k) { // Handle new touch screen button presses here - single action per press, not autorepeat case SPEEDO_BUT: // pc.printf ("Speedometer key pressed %d\r\n", k); break; case VMETER_BUT: // pc.printf ("Voltmeter key pressed %d\r\n", k); break; case AMETER_BUT: // pc.printf ("Ammeter key pressed %d\r\n", k); break; default: pc.printf ("Unhandled keypress %d\r\n", k); break; } // endof switch (button) } } // endof while - handle new key presses j = 0; while (j < previous_kybd->count) { // handle new key releases k = previous_kybd->ky[j++].keynum; if (inlist(*previous_kybd, k) && !inlist(*present_kybd, k)) { pc.printf ("Handle Release %d\r\n", k); draw_button_hilight (k, LCD_COLOR_DARKBLUE) ; } } // endof while - handle new key releases } // endof at least one key pressed this time or last time if (sliderpress == false) { // need to glide dead-mans function towards neutral here if (slider.position < NEUTRAL_VAL) { slider.position += 1 + (NEUTRAL_VAL - slider.position) / DAMPER_DECAY; SliderGraphic (slider); slider.recalc_run = true; } } if (slider.recalc_run) { // range of slider.position in RUN mode is min_pos_() to NEUTRAL_VAL - 1 slider.recalc_run = false; // All RUN power and pwm calcs done here int b = slider.position; // double torque_req; // now declared above to be used as parameter for throttle if (b > NEUTRAL_VAL) b = NEUTRAL_VAL; if (b < MIN_POS) // if finger position is above top of slider limit b = MIN_POS; b = NEUTRAL_VAL - b; // now got integer going positive for increasing power demand torque_req = (double) b; torque_req /= (NEUTRAL_VAL - MIN_POS); // in range 0.0 to 1.0 pc.printf ("torque_rec = %.3f, last_pwm = %.3f\r\n", torque_req, last_pwm); set_I_limit (torque_req); if (torque_req < 0.05) set_V_limit (last_pwm / 2.0); else { if (last_pwm < 0.99) set_V_limit (last_pwm + 0.05); // ramp voltage up rather than slam to max } } } // endof doing 32ms stuff if (qtrsec_trig == true) { // do every quarter second stuff here qtrsec_trig = false; speed.qtr_sec_update (); double speedmph = speed.MPH(), amps = 0.0 - read_ammeter(), volts = read_voltmeter(); //static const double mph_2_mm_per_sec = 447.04; // exact // double mm_travelled_in_qtrsec = speedmph * mph_2_mm_per_sec / 4.0; slider.loco_speed = speedmph; electrical_power_Watt = volts * amps; // visible throughout main update_meters (speedmph, electrical_power_Watt, volts) ; // displays speed, volts and power (volts times amps) // update_meters (7.5, amps, volts) ; led_grn = !led_grn; if (slider.state == PARK) { if (speedmph > LOCO_HANDBRAKE_ESCAPE_SPEED / 4.0) { slider.handbrake_effort *= 1.1; if (slider.handbrake_effort > 0.55) slider.handbrake_effort = 0.55; set_run_mode (PARK); pc.printf ("Handbrake slipping, effort %.2f\r\n", slider.handbrake_effort); } if (speedmph < 0.02) { slider.handbrake_effort *= 0.9; if (slider.handbrake_effort < 0.05) slider.handbrake_effort = 0.05; set_run_mode (PARK); pc.printf ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort); } } c_5++; // Can do stuff once per second here if(c_5 > 3) { c_5 = 0; seconds++; if (seconds > 59) { seconds = 0; minutes++; // do once per minute stuff here } // fall back into once per second // if(SD_state == SD_OK) { if(read_SD_state() == true) { uint32_t distance = speed.metres_travelled(); char dist[20]; sprintf (dist, "%05d m", distance); displaytext (236, 226, 2, dist); update_SD_card (); // Buffers data for SD card, writes when buffer filled } // calc_motor_amps( mva); } // endof if(c_5 > 3 } // endof if (qtrsec_trig == true) { } // endof while(1) main programme loop } // endof int main() {