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
Diff: main.cpp
- Revision:
- 0:23cc72b18e74
- Child:
- 1:8ef34deb5177
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Nov 12 06:26:29 2017 +0000
@@ -0,0 +1,836 @@
+// Electric Locomotive Controller
+// Jon Freeman B. Eng Hons
+
+// Last Updated 12 April 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"
+#include "dro.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;
+
+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);
+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 () ;
+
+static const int NUMBER_OF_MOTORS = 4,
+ SD_BLOCKSIZE = 512, /* SD card data Block Size in Bytes */
+ DAMPER_DECAY = 42, // Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel
+ MAF_PTS = 140, // Moving Average Filter points
+ PWM_HZ = 13000,
+// 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;
+
+static const double
+ MOTOR_PINION_T = 17.0, // motor pinion teeth, wheel gear teeth and wheel dia required to calculate speed and distance.
+ WHEEL_GEAR_T = 76.0,
+ WHEEL_DIA_MM = 147.0,
+ WHEEL_CIRCUMFERENCE_METRE = PI * WHEEL_DIA_MM / 1000.0,
+ PULSES_PER_WHEEL_REV = 32.0 * WHEEL_GEAR_T / MOTOR_PINION_T,
+ PULSES_PER_METRE = PULSES_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_METRE,
+ rpm2mph = 60.0 // = Motor Revs per hour;
+ * (MOTOR_PINION_T / WHEEL_GEAR_T) // = Wheel rev per hour
+ * WHEEL_CIRCUMFERENCE_METRE // = metres per hour
+ * 39.37 // = inches per hour
+ / (1760 * 36) // = miles per hour
+ ;
+
+// Assume SD card size is 4Gbyte, might be 8 Gbyte
+// Then can use 8388608 blocks (8 * 1024 * 1024)
+
+uint64_t SD_blockptr = 0;
+uint32_t SDBuffer[(SD_BLOCKSIZE >> 2)]; // = space for (512 / 4) uint32_t
+uint8_t SD_state = SD_OK, sd_jf = 0;
+
+static const uint64_t GIGAB = 1024 * 1024 * 1024;
+//static const uint64_t SDBLOCKS = (GIGAB / SD_BLOCKSIZE) * 4; // software drives SD up to 4Gbyte only - 8 M block
+static const uint64_t SDBLOCKS = (GIGAB / SD_BLOCKSIZE) * 2; // software drives SD up to 4Gbyte only - 8 M block
+// If data logger takes 2 minutes to fill 1 block, a 4G card takes 32 years run-time to fill
+// If system generates approx 320 pulses per metre travelled, max distance recordable in uint32_t is 65536 * 65536 / 320 = 13421.772 km
+
+//from dro.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 ;
+
+
+
+//static const double mph_2_mm_per_sec = 447.04; // exact
+
+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;
+
+bool sd_error () { // Test and Clear error code sd_jf, return true if any error bits set, false on 0
+ bool retval = false;
+ if (sd_jf != 0) {
+ retval = true;
+ sd_jf = 0;
+ }
+ return retval;
+}
+
+bool check_SD_block_clear (uint32_t block) {
+ uint32_t b[(SD_BLOCKSIZE >> 2)];
+ SD_state = sd.ReadBlocks(b, (uint64_t)(SD_BLOCKSIZE * block), SD_BLOCKSIZE, 1);
+ if(SD_state != SD_OK) {
+ sd_jf = 1;
+ pc.printf ("Failed, not SD_OK, erasing block %d\r\n", block);
+ return false;
+ }
+ for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++)
+ if (b[i] != 0)
+ return false;
+ return true;
+}
+
+/*bool erase_block (uint32_t block2erase) {
+ uint64_t addr = SD_BLOCKSIZE * (uint64_t)block2erase;
+ SD_state = sd.Erase(addr, addr + SD_BLOCKSIZE);
+ if (SD_state != SD_OK) {
+ sd_jf = 1; // Assert error flag
+ pc.printf ("Failed, not SD_OK, erasing block %d\r\n", block2erase);
+ return false;
+ }
+ return check_SD_block_clear (block2erase);
+}*/
+
+bool SD_find_next_clear_block (uint64_t * blok) { // Successive approximation algorithm to quickly find next vacant SD card 512 byte block
+ uint64_t toaddsub = SDBLOCKS / 2, stab = SDBLOCKS - 1;
+ pc.printf ("At SD_find_next_clear_block \r\n");
+ while (toaddsub) {
+ pc.printf ("stab = %lld, toadsub = %lld\r\n", stab, toaddsub); // lld for long long int
+ bool clear_block = true;
+ SD_state = sd.ReadBlocks(SDBuffer, SD_BLOCKSIZE * stab, SD_BLOCKSIZE, 1);
+ if(SD_state != SD_OK) {
+ sd_jf = 1;
+ pc.printf ("SD error in SD_find_next_clear_block, returning -1\r\n");
+ return false;
+ }
+ for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++) {
+ if (SDBuffer[i] != 0) {
+ clear_block = false;
+ pc.printf ("Buff at %d contains %x\r\n", i, SDBuffer[i]);
+ i = SD_BLOCKSIZE; // to exit loop
+ }
+ }
+ if (clear_block)
+ stab -= toaddsub;
+ else
+ stab += toaddsub;
+ toaddsub >>= 1;
+ }
+ if (!check_SD_block_clear(stab))
+ stab++;
+ if (sd_error()) { // sd_error() tests and clears error bits
+ pc.printf ("check_SD_block_clear(%ld)returned ERROR in SD_find_next_clear_block\r\n", stab);
+ sd_jf = 1; // reassert error flag
+ return false;
+ }
+ pc.printf ("Completed find_next, stab = %d\r\n", stab);
+ *blok = stab; // block number of next free block
+ return true;
+}
+
+bool SD_card_erase_all (void) { // assumes sd card is 4 Gbyte, erases 4 Gbyte. Called from CLI
+ uint64_t EndAddr = GIGAB * 4,
+ StartAddr = 0LL;
+ sd_jf = 0;
+ pc.printf ("Erasing SD card ... ");
+ // uint8_t Erase(uint64_t StartAddr, uint64_t EndAddr);
+ SD_state = sd.Erase(StartAddr, EndAddr);
+ if (SD_state != SD_OK) {
+ pc.printf ("SD_card_erase_all FAILED\r\n");
+ sd_jf = 1;
+ return false;
+ }
+ pc.printf ("no error detected\r\n");
+ return true;
+}
+
+
+bool mainSDtest()
+{
+ SD_state = sd.Init();
+ if(SD_state != SD_OK) {
+ pc.printf ("sd.Init set SD_state to %0x\r\n", SD_state);
+ if(SD_state == MSD_ERROR_SD_NOT_PRESENT) {
+ pc.printf("SD shall be inserted before running test\r\n");
+ } else {
+ pc.printf("SD Initialization : FAIL.\r\n");
+ }
+ pc.printf("SD Test Aborted.\r\n");
+ return false;
+ }
+// else { // SD_state is SD_OK
+ pc.printf("SD Initialization : OK.\r\n");
+
+
+
+// SD_card_erase_all();
+// if (sd_error())
+// pc.printf ("SD_card_erase_all() reports ERROR");
+
+
+
+ SD_find_next_clear_block(& SD_blockptr);
+ pc.printf ("SD_find_next_clear_block returned %lld\r\n\n\n", SD_blockptr);
+ if (sd_error()) {
+ pc.printf ("***** ERROR returned from SD_find_next_clear_block ***** SD ops aborted\r\n");
+ return false;
+ }
+ pc.printf("SD_find_next_clear_block() returned %ld\r\n", SD_blockptr);
+ if (SD_blockptr < 1) {
+ pc.printf ("Looks like card newly erased, SD_blockptr value of %d\r\n", SD_blockptr);
+ SD_blockptr = 0;
+ historic_distance = 0;
+ }
+ else {
+ SD_state = sd.ReadBlocks(SDBuffer, SD_BLOCKSIZE * (SD_blockptr - 1), SD_BLOCKSIZE, 1);
+ if (SD_state != SD_OK) {
+ pc.printf ("Error reading last block from SD block %d\r\n", SD_blockptr - 1);
+ return false;
+ }
+ for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++)
+ pc.printf ("%lx\t", SDBuffer[i]);
+ historic_distance = SDBuffer[(SD_BLOCKSIZE >> 2) - 1];
+ pc.printf ("\r\nAbove, data read from last filled SD block %lld, using historic_distance = %lx\r\n", SD_blockptr - 1, historic_distance);
+ }
+ if (SD_blockptr > 2) {
+ for (int i = SD_blockptr - 2; i < SD_blockptr + 2; i++) {
+ pc.printf ("check_SD_block_clear (%d) ", i);
+ if (check_SD_block_clear(i))
+ pc.printf ("block %ld is CLEAR\r\n", i);
+ else
+ pc.printf ("block %ld is NOT clear\r\n", i);
+ if (sd_error()) {
+ pc.printf ("ERROR from check_SD_block_clear ()\r\n");
+ }
+ }
+ }
+ return true;
+}
+
+
+
+
+
+
+
+
+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];
+}
+
+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 ()
+{
+ Hall_pulse[4]++;
+}
+void ISR_mot6_hall_handler ()
+{
+ Hall_pulse[5]++;
+}
+*/
+
+void ISR_current_reader (void) // FIXED at 250us
+{
+ trigger_current_read = true; // every 250us, i.e. 4kHz NOTE only sets trigger here, readings taken in main loop
+}
+
+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)
+{
+ 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;
+ }
+}
+
+void update_SD_card () { // Hall pulse total updated once per sec and saved in blocks of 128 to SD card
+ static int index = 0;
+ static uint32_t buff[(SD_BLOCKSIZE >> 2) + 2];
+ buff[index++] = speed.pulse_total(); // pulse_total for all time, add this to buffer to write to SD
+ if (index >= (SD_BLOCKSIZE >> 2)) {
+ pc.printf ("Writing new SD block %d ... ", SD_blockptr);
+ SD_state = sd.WriteBlocks(buff, SD_BLOCKSIZE * SD_blockptr, SD_BLOCKSIZE, 1);
+ SD_blockptr++;
+ if (SD_state == SD_OK)
+ pc.printf ("OK, distance %d\r\n", buff[index - 1] / (int)PULSES_PER_METRE);
+ else
+ pc.printf ("ERROR\r\n");
+ index = 0;
+ }
+}
+
+int main()
+{
+ int c_5 = 0, seconds = 0, minutes = 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);
+ 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); // set to longer time to test
+ tick32ms.attach_us (&ISR_tick_32ms, 32001);
+ 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();
+
+ 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;
+ 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;
+ 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;
+ 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;
+ update_meters (speedmph, amps, volts) ;
+// 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) {
+ 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() {
+
+