File content as of revision 0:23cc72b18e74:

//  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()    {