Jon Freeman / Mbed 2 deprecated Loco_10_17_SD

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@1:8ef34deb5177, 2017-11-13 (annotated)

Committer:
JonFreeman
Date:
Mon Nov 13 09:53:00 2017 +0000
Revision:
1:8ef34deb5177
Parent:
0:23cc72b18e74 
Brushless Motor electric locomotive congtrol system; Drives 4 motors using touch-screen control.; Displays speed MPH, system volts and power

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonFreeman 0:23cc72b18e74 1 // Electric Locomotive Controller
JonFreeman 0:23cc72b18e74 2 // Jon Freeman B. Eng Hons
JonFreeman 0:23cc72b18e74 3
JonFreeman 1:8ef34deb5177 4 // Last Updated 13 November 2017
JonFreeman 1:8ef34deb5177 5
JonFreeman 0:23cc72b18e74 6 // Touch Screen Loco 2017 - WITH SD card data logger functions
JonFreeman 0:23cc72b18e74 7
JonFreeman 0:23cc72b18e74 8 // This code runs on STM 32F746NG DISCO module, high performance ARM Cortex with touch screen
JonFreeman 0:23cc72b18e74 9 // ffi on ST module -> https://developer.mbed.org/platforms/ST-Discovery-F746NG/
JonFreeman 0:23cc72b18e74 10 // Board plugs onto simple mother-board containing low voltage power supplies, interfacing buffers, connectors etc.
JonFreeman 0:23cc72b18e74 11 // See www.jons-workshop.com ffi on hardware.
JonFreeman 0:23cc72b18e74 12
JonFreeman 0:23cc72b18e74 13 // Design provides PWM outputs to drive up to four brushless motor drive modules, each able to return speed information.
JonFreeman 0:23cc72b18e74 14 // Output signals are dual PWM, one to set max motor voltage, other to set max motor current.
JonFreeman 0:23cc72b18e74 15 // This code as supplied uses current control to drive locomotive. This means that drive fader acts as a Torque, not Speed, Demand control.
JonFreeman 0:23cc72b18e74 16 // Regenerative braking is included in the design.
JonFreeman 0:23cc72b18e74 17 // NOTE that when braking, the motor supply rail voltage will be lifted. Failure to design-in some type of 'surplus power dump'
JonFreeman 0:23cc72b18e74 18 // may result in over-voltage damage to batteries or power electronics.
JonFreeman 0:23cc72b18e74 19
JonFreeman 0:23cc72b18e74 20 #include "mbed.h"
JonFreeman 0:23cc72b18e74 21 #include "FastPWM.h"
JonFreeman 0:23cc72b18e74 22 #include "TS_DISCO_F746NG.h"
JonFreeman 0:23cc72b18e74 23 #include "LCD_DISCO_F746NG.h"
JonFreeman 1:8ef34deb5177 24 //#include "SD_DISCO_F746NG.h" // SD card stuff now in separate file sd_card.cpp
JonFreeman 1:8ef34deb5177 25 #include "Electric_Loco.h"
JonFreeman 0:23cc72b18e74 26
JonFreeman 0:23cc72b18e74 27 // Design Topology
JonFreeman 0:23cc72b18e74 28 // This F746NG is the single loco control computer.
JonFreeman 0:23cc72b18e74 29 // Assumed 4 motor controllers driven from same signal set via multiple opto / buffers
JonFreeman 0:23cc72b18e74 30 // Outputs are : -
JonFreeman 0:23cc72b18e74 31 // FastPWM maxv on D12 - in drive, sets motor volts to pwm proportion of available volts. Also used in regen braking
JonFreeman 0:23cc72b18e74 32 // FastPWM maxi on D11 - used to set upper bound on motor current, used as analogue out to set current limit on motor driver
JonFreeman 0:23cc72b18e74 33 // DigitalOut reverse (D7) - D6,7 select fwd, rev, brake, parking brake
JonFreeman 0:23cc72b18e74 34 // DigitalOut forward (D6)
JonFreeman 0:23cc72b18e74 35 // Inputs are : -
JonFreeman 0:23cc72b18e74 36 // AnalogIn ht_amps_ain (A0); // Jan 2017
JonFreeman 0:23cc72b18e74 37 // AnalogIn ht_volts_ain (A1); // Jan 2017
JonFreeman 0:23cc72b18e74 38 // InterruptIn mot4hall (D2);
JonFreeman 0:23cc72b18e74 39 // InterruptIn mot3hall (D3);
JonFreeman 0:23cc72b18e74 40 // InterruptIn mot2hall (D4);
JonFreeman 0:23cc72b18e74 41 // InterruptIn mot1hall (D5);
JonFreeman 0:23cc72b18e74 42
JonFreeman 0:23cc72b18e74 43 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : -
JonFreeman 0:23cc72b18e74 44 FWD(A5) REV(A4) PWM Action
JonFreeman 0:23cc72b18e74 45 0 0 0 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 46 0 0 1 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 47 0 1 0 Reverse0
JonFreeman 0:23cc72b18e74 48 0 1 1 Reverse1
JonFreeman 0:23cc72b18e74 49
JonFreeman 0:23cc72b18e74 50 1 0 0 Forward0
JonFreeman 0:23cc72b18e74 51 1 0 1 Forward1
JonFreeman 0:23cc72b18e74 52 1 1 0 Regen Braking
JonFreeman 0:23cc72b18e74 53 1 1 1 Regen Braking
JonFreeman 0:23cc72b18e74 54 */
JonFreeman 0:23cc72b18e74 55
JonFreeman 0:23cc72b18e74 56 LCD_DISCO_F746NG lcd;
JonFreeman 0:23cc72b18e74 57 TS_DISCO_F746NG touch_screen;
JonFreeman 1:8ef34deb5177 58 //SD_DISCO_F746NG sd; // SD card stuff now in sd_card.cpp
JonFreeman 0:23cc72b18e74 59
JonFreeman 0:23cc72b18e74 60 FastPWM maxv (D12, 1),
JonFreeman 0:23cc72b18e74 61 maxi (D11, 1); // pin, prescaler value
JonFreeman 0:23cc72b18e74 62 Serial pc (USBTX, USBRX); // Comms to 'PuTTY' or similar comms programme on pc
JonFreeman 0:23cc72b18e74 63
JonFreeman 0:23cc72b18e74 64 DigitalOut reverse_pin (D7); //
JonFreeman 0:23cc72b18e74 65 DigitalOut forward_pin (D6); //these two decode to fwd, rev, regen_braking and park
JonFreeman 0:23cc72b18e74 66 DigitalOut GfetT2 (D14); // a horn
JonFreeman 0:23cc72b18e74 67 DigitalOut GfetT1 (D15); // another horn
JonFreeman 0:23cc72b18e74 68 DigitalOut led_grn (LED1); // the only on board user led
JonFreeman 0:23cc72b18e74 69
JonFreeman 0:23cc72b18e74 70 DigitalIn f_r_switch (D0); // Reads position of centre-off ignition switch
JonFreeman 1:8ef34deb5177 71 //DigitalIn spareio_d8 (D8);
JonFreeman 1:8ef34deb5177 72 //DigitalOut throttle_servo_pulse_out (D8); // now defined in throttle.cpp
JonFreeman 0:23cc72b18e74 73 DigitalIn spareio_d9 (D9);
JonFreeman 0:23cc72b18e74 74 DigitalIn spareio_d10 (D10); // D8, D9, D10 wired to jumper on pcb - not used to Apr 2017
JonFreeman 0:23cc72b18e74 75
JonFreeman 0:23cc72b18e74 76 AnalogIn ht_volts_ain (A0); // Jan 2017
JonFreeman 0:23cc72b18e74 77 AnalogIn ht_amps_ain (A1); // Jan 2017
JonFreeman 0:23cc72b18e74 78 AnalogIn spare_ain2 (A2);
JonFreeman 0:23cc72b18e74 79 AnalogIn spare_ain3 (A3);
JonFreeman 0:23cc72b18e74 80 AnalogIn spare_ain4 (A4); // hardware on pcb for these 3 spare analogue inputs - not used to Apr 2017
JonFreeman 0:23cc72b18e74 81 //AnalogIn spare_ain5 (A5); // causes display flicker !
JonFreeman 0:23cc72b18e74 82
JonFreeman 0:23cc72b18e74 83 InterruptIn mot4hall (D2); // One Hall sensor signal from each motor fed back to measure speed
JonFreeman 0:23cc72b18e74 84 InterruptIn mot3hall (D3);
JonFreeman 0:23cc72b18e74 85 InterruptIn mot2hall (D4);
JonFreeman 0:23cc72b18e74 86 InterruptIn mot1hall (D5);
JonFreeman 0:23cc72b18e74 87
JonFreeman 0:23cc72b18e74 88 extern int get_button_press (struct point & pt) ;
JonFreeman 0:23cc72b18e74 89 extern void displaytext (int x, int y, const int font, uint32_t BCol, uint32_t TCol, char * txt) ;
JonFreeman 0:23cc72b18e74 90 extern void displaytext (int x, int y, const int font, char * txt) ;
JonFreeman 0:23cc72b18e74 91 extern void displaytext (int x, int y, char * txt) ;
JonFreeman 0:23cc72b18e74 92 extern void setup_buttons () ;
JonFreeman 0:23cc72b18e74 93 extern void draw_numeric_keypad (int colour) ;
JonFreeman 0:23cc72b18e74 94 extern void draw_button_hilight (int bu, int colour) ;
JonFreeman 0:23cc72b18e74 95 extern void read_presses (int * a) ;
JonFreeman 0:23cc72b18e74 96 extern void read_keypresses (struct ky_bd & a) ;
JonFreeman 0:23cc72b18e74 97 extern void SliderGraphic (struct slide & q) ;
JonFreeman 0:23cc72b18e74 98 extern void vm_set () ;
JonFreeman 0:23cc72b18e74 99 extern void update_meters (double, double, double) ;
JonFreeman 0:23cc72b18e74 100 extern void command_line_interpreter () ;
JonFreeman 0:23cc72b18e74 101
JonFreeman 1:8ef34deb5177 102 extern int throttle (double, double) ; // called from main every 31ms
JonFreeman 1:8ef34deb5177 103
JonFreeman 1:8ef34deb5177 104 extern void update_SD_card () ; // Hall pulse total updated once per sec and saved in blocks of 128 to SD card
JonFreeman 1:8ef34deb5177 105 extern bool read_SD_state () ;
JonFreeman 1:8ef34deb5177 106 extern bool mainSDtest();
JonFreeman 1:8ef34deb5177 107
JonFreeman 1:8ef34deb5177 108 static const int
JonFreeman 0:23cc72b18e74 109 DAMPER_DECAY = 42, // Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel
JonFreeman 1:8ef34deb5177 110 MAF_PTS = 140, // Moving Average Filter points. Filters reduce noise on volatage and current readings
JonFreeman 1:8ef34deb5177 111 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear
JonFreeman 0:23cc72b18e74 112 // PWM_HZ = 2000, // Used this to experiment on much bigger motor
JonFreeman 0:23cc72b18e74 113 MAX_PWM_TICKS = 108000000 / PWM_HZ, // 108000000 for F746N, due to cpu clock = 216 MHz
JonFreeman 0:23cc72b18e74 114 FWD = 0,
JonFreeman 0:23cc72b18e74 115 REV = ~FWD;
JonFreeman 0:23cc72b18e74 116
JonFreeman 1:8ef34deb5177 117 //from .h struct slide { int position; int oldpos; int state; int direction; bool recalc_run; bool handbrake_slipping; double handbrake_effort; double loco_speed } ;
JonFreeman 0:23cc72b18e74 118 struct slide slider ;
JonFreeman 0:23cc72b18e74 119
JonFreeman 0:23cc72b18e74 120
JonFreeman 0:23cc72b18e74 121 int V_maf[MAF_PTS + 2], I_maf[MAF_PTS + 2], maf_ptr = 0;
JonFreeman 0:23cc72b18e74 122 //uint32_t Hall_pulse[8] = {0,0,0,0,0,0,0,0}; // more than max number of motors
JonFreeman 0:23cc72b18e74 123 uint32_t Hall_pulse[8] = {1,1,1,1,1,1,1,1}; // more than max number of motors
JonFreeman 0:23cc72b18e74 124 uint32_t historic_distance = 0;
JonFreeman 0:23cc72b18e74 125
JonFreeman 0:23cc72b18e74 126 bool qtrsec_trig = false;
JonFreeman 0:23cc72b18e74 127 bool trigger_current_read = false;
JonFreeman 0:23cc72b18e74 128 volatile bool trigger_32ms = false;
JonFreeman 0:23cc72b18e74 129
JonFreeman 0:23cc72b18e74 130 double last_pwm = 0.0;
JonFreeman 0:23cc72b18e74 131
JonFreeman 0:23cc72b18e74 132 class speed_measurement // Interrupts at qtr sec cause read of Hall_pulse counters which are incremented by transitions of Hall inputs
JonFreeman 0:23cc72b18e74 133 {
JonFreeman 0:23cc72b18e74 134 static const int SPEED_AVE_PTS = 9; // AVE_PTS - points in moving average filters
JonFreeman 0:23cc72b18e74 135 int speed_maf_mem [(SPEED_AVE_PTS + 1) * 2][NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 136 latest_counter_read[NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 137 prev_counter_read[NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 138 mafptr;
JonFreeman 0:23cc72b18e74 139 int raw_filtered () ; // sum of count for all motors
JonFreeman 0:23cc72b18e74 140
JonFreeman 0:23cc72b18e74 141 public:
JonFreeman 0:23cc72b18e74 142
JonFreeman 0:23cc72b18e74 143 speed_measurement () {
JonFreeman 0:23cc72b18e74 144 memset(speed_maf_mem, 0, sizeof(speed_maf_mem));
JonFreeman 0:23cc72b18e74 145 mafptr = 0;
JonFreeman 0:23cc72b18e74 146 memset (latest_counter_read, 0, sizeof(latest_counter_read));
JonFreeman 0:23cc72b18e74 147 memset (prev_counter_read, 0, sizeof(prev_counter_read));
JonFreeman 0:23cc72b18e74 148 } // constructor
JonFreeman 0:23cc72b18e74 149 int raw_filtered (int) ; // count for one motor
JonFreeman 0:23cc72b18e74 150 int RPM () ;
JonFreeman 0:23cc72b18e74 151 double MPH () ;
JonFreeman 0:23cc72b18e74 152 void qtr_sec_update () ;
JonFreeman 0:23cc72b18e74 153 uint32_t metres_travelled ();
JonFreeman 0:23cc72b18e74 154 uint32_t pulse_total ();
JonFreeman 0:23cc72b18e74 155 }
JonFreeman 0:23cc72b18e74 156 speed ;
JonFreeman 0:23cc72b18e74 157
JonFreeman 0:23cc72b18e74 158 int speed_measurement::raw_filtered () // sum of count for all motors
JonFreeman 0:23cc72b18e74 159 {
JonFreeman 0:23cc72b18e74 160 int result = 0, a, b;
JonFreeman 0:23cc72b18e74 161 for (b = 0; b < NUMBER_OF_MOTORS; b++) {
JonFreeman 0:23cc72b18e74 162 for (a = 0; a < SPEED_AVE_PTS; a++) {
JonFreeman 0:23cc72b18e74 163 result += speed_maf_mem[a][b];
JonFreeman 0:23cc72b18e74 164 }
JonFreeman 0:23cc72b18e74 165 }
JonFreeman 0:23cc72b18e74 166 return result;
JonFreeman 0:23cc72b18e74 167 }
JonFreeman 0:23cc72b18e74 168
JonFreeman 0:23cc72b18e74 169 int speed_measurement::raw_filtered (int motor) // count for one motor
JonFreeman 0:23cc72b18e74 170 {
JonFreeman 0:23cc72b18e74 171 int result = 0, a;
JonFreeman 0:23cc72b18e74 172 for (a = 0; a < SPEED_AVE_PTS; a++) {
JonFreeman 0:23cc72b18e74 173 result += speed_maf_mem[a][motor];
JonFreeman 0:23cc72b18e74 174 }
JonFreeman 0:23cc72b18e74 175 return result;
JonFreeman 0:23cc72b18e74 176 }
JonFreeman 0:23cc72b18e74 177
JonFreeman 0:23cc72b18e74 178 double speed_measurement::MPH ()
JonFreeman 0:23cc72b18e74 179 {
JonFreeman 0:23cc72b18e74 180 return rpm2mph * (double)RPM();
JonFreeman 0:23cc72b18e74 181 }
JonFreeman 0:23cc72b18e74 182
JonFreeman 0:23cc72b18e74 183 int speed_measurement::RPM ()
JonFreeman 0:23cc72b18e74 184 {
JonFreeman 0:23cc72b18e74 185 int rpm = raw_filtered ();
JonFreeman 0:23cc72b18e74 186 rpm *= 60 * 4; // 60 sec per min, 4 quarters per sec, result pulses per min
JonFreeman 0:23cc72b18e74 187 rpm /= (SPEED_AVE_PTS * NUMBER_OF_MOTORS * 8); // 8 transitions counted per rev
JonFreeman 0:23cc72b18e74 188 return rpm;
JonFreeman 0:23cc72b18e74 189 }
JonFreeman 0:23cc72b18e74 190
JonFreeman 0:23cc72b18e74 191 void speed_measurement::qtr_sec_update () // this to be called every quarter sec to read counters and update maf
JonFreeman 0:23cc72b18e74 192 {
JonFreeman 0:23cc72b18e74 193 mafptr++;
JonFreeman 0:23cc72b18e74 194 if (mafptr >= SPEED_AVE_PTS)
JonFreeman 0:23cc72b18e74 195 mafptr = 0;
JonFreeman 0:23cc72b18e74 196 for (int a = 0; a < NUMBER_OF_MOTORS; a++) {
JonFreeman 0:23cc72b18e74 197 prev_counter_read[a] = latest_counter_read[a];
JonFreeman 0:23cc72b18e74 198 latest_counter_read[a] = Hall_pulse[a];
JonFreeman 0:23cc72b18e74 199 speed_maf_mem[mafptr][a] = latest_counter_read[a] - prev_counter_read[a];
JonFreeman 0:23cc72b18e74 200 }
JonFreeman 0:23cc72b18e74 201 }
JonFreeman 0:23cc72b18e74 202
JonFreeman 0:23cc72b18e74 203 uint32_t speed_measurement::metres_travelled ()
JonFreeman 0:23cc72b18e74 204 {
JonFreeman 0:23cc72b18e74 205 return pulse_total() / (int)PULSES_PER_METRE;
JonFreeman 0:23cc72b18e74 206 }
JonFreeman 0:23cc72b18e74 207
JonFreeman 0:23cc72b18e74 208 uint32_t speed_measurement::pulse_total ()
JonFreeman 0:23cc72b18e74 209 {
JonFreeman 0:23cc72b18e74 210 return historic_distance + Hall_pulse[0] + Hall_pulse[1] + Hall_pulse[2] + Hall_pulse[3];
JonFreeman 0:23cc72b18e74 211 }
JonFreeman 0:23cc72b18e74 212
JonFreeman 1:8ef34deb5177 213 uint32_t get_pulse_total () { // called by SD card code
JonFreeman 1:8ef34deb5177 214 return speed.pulse_total();
JonFreeman 1:8ef34deb5177 215 }
JonFreeman 1:8ef34deb5177 216
JonFreeman 0:23cc72b18e74 217 void set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:23cc72b18e74 218 {
JonFreeman 0:23cc72b18e74 219 if (p < 0.0)
JonFreeman 0:23cc72b18e74 220 p = 0.0;
JonFreeman 0:23cc72b18e74 221 if (p > 1.0)
JonFreeman 0:23cc72b18e74 222 p = 1.0;
JonFreeman 0:23cc72b18e74 223 last_pwm = p;
JonFreeman 0:23cc72b18e74 224 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:23cc72b18e74 225 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:23cc72b18e74 226 maxv.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output on pin D12 inverted motor pwm
JonFreeman 0:23cc72b18e74 227 }
JonFreeman 0:23cc72b18e74 228
JonFreeman 0:23cc72b18e74 229 void set_I_limit (double p) // Sets max motor current
JonFreeman 0:23cc72b18e74 230 {
JonFreeman 0:23cc72b18e74 231 int a;
JonFreeman 0:23cc72b18e74 232 if (p < 0.0)
JonFreeman 0:23cc72b18e74 233 p = 0.0;
JonFreeman 0:23cc72b18e74 234 if (p > 1.0)
JonFreeman 0:23cc72b18e74 235 p = 1.0;
JonFreeman 0:23cc72b18e74 236 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:23cc72b18e74 237 if (a > MAX_PWM_TICKS)
JonFreeman 0:23cc72b18e74 238 a = MAX_PWM_TICKS;
JonFreeman 0:23cc72b18e74 239 if (a < 0)
JonFreeman 0:23cc72b18e74 240 a = 0;
JonFreeman 0:23cc72b18e74 241 maxi.pulsewidth_ticks (a); // PWM output on pin D12 inverted motor pwm
JonFreeman 0:23cc72b18e74 242 }
JonFreeman 0:23cc72b18e74 243
JonFreeman 0:23cc72b18e74 244 double read_ammeter ()
JonFreeman 0:23cc72b18e74 245 {
JonFreeman 0:23cc72b18e74 246 int a = 0;
JonFreeman 0:23cc72b18e74 247 for (int b = 0; b < MAF_PTS; b++)
JonFreeman 0:23cc72b18e74 248 a += I_maf[b];
JonFreeman 0:23cc72b18e74 249 a /= MAF_PTS;
JonFreeman 0:23cc72b18e74 250 double i = (double) a;
JonFreeman 0:23cc72b18e74 251 return (i * 95.0 / 32768.0) - 95.0 + 0.46; // fiddled to suit current module
JonFreeman 0:23cc72b18e74 252 }
JonFreeman 0:23cc72b18e74 253
JonFreeman 0:23cc72b18e74 254 double read_voltmeter ()
JonFreeman 0:23cc72b18e74 255 {
JonFreeman 0:23cc72b18e74 256 int a = 0;
JonFreeman 0:23cc72b18e74 257 for (int b = 0; b < MAF_PTS; b++)
JonFreeman 0:23cc72b18e74 258 a += V_maf[b];
JonFreeman 0:23cc72b18e74 259 a /= MAF_PTS;
JonFreeman 0:23cc72b18e74 260 double i = (double) a;
JonFreeman 0:23cc72b18e74 261 return (i / 617.75) + 0.3; // fiddled to suit current module
JonFreeman 0:23cc72b18e74 262 }
JonFreeman 0:23cc72b18e74 263
JonFreeman 0:23cc72b18e74 264 // Interrupt Service Routines
JonFreeman 0:23cc72b18e74 265
JonFreeman 0:23cc72b18e74 266 void ISR_mot1_hall_handler () // read motor position pulse signals from up to six motors
JonFreeman 0:23cc72b18e74 267 {
JonFreeman 0:23cc72b18e74 268 Hall_pulse[0]++;
JonFreeman 0:23cc72b18e74 269 }
JonFreeman 0:23cc72b18e74 270 void ISR_mot2_hall_handler ()
JonFreeman 0:23cc72b18e74 271 {
JonFreeman 0:23cc72b18e74 272 Hall_pulse[1]++;
JonFreeman 0:23cc72b18e74 273 }
JonFreeman 0:23cc72b18e74 274 void ISR_mot3_hall_handler ()
JonFreeman 0:23cc72b18e74 275 {
JonFreeman 0:23cc72b18e74 276 Hall_pulse[2]++;
JonFreeman 0:23cc72b18e74 277 }
JonFreeman 0:23cc72b18e74 278 void ISR_mot4_hall_handler ()
JonFreeman 0:23cc72b18e74 279 {
JonFreeman 0:23cc72b18e74 280 Hall_pulse[3]++;
JonFreeman 0:23cc72b18e74 281 }
JonFreeman 1:8ef34deb5177 282 void ISR_mot5_hall_handler () // If only 4 motors this never gets used, there is no fifth motor
JonFreeman 0:23cc72b18e74 283 {
JonFreeman 0:23cc72b18e74 284 Hall_pulse[4]++;
JonFreeman 0:23cc72b18e74 285 }
JonFreeman 1:8ef34deb5177 286 void ISR_mot6_hall_handler () // As one above
JonFreeman 0:23cc72b18e74 287 {
JonFreeman 0:23cc72b18e74 288 Hall_pulse[5]++;
JonFreeman 0:23cc72b18e74 289 }
JonFreeman 1:8ef34deb5177 290
JonFreeman 0:23cc72b18e74 291
JonFreeman 0:23cc72b18e74 292 void ISR_current_reader (void) // FIXED at 250us
JonFreeman 0:23cc72b18e74 293 {
JonFreeman 1:8ef34deb5177 294 static int ms32 = 0, ms250 = 0;
JonFreeman 0:23cc72b18e74 295 trigger_current_read = true; // every 250us, i.e. 4kHz NOTE only sets trigger here, readings taken in main loop
JonFreeman 1:8ef34deb5177 296 ms32++;
JonFreeman 1:8ef34deb5177 297 if (ms32 > 124) {
JonFreeman 1:8ef34deb5177 298 ms32 = 0;
JonFreeman 1:8ef34deb5177 299 trigger_32ms = true;
JonFreeman 1:8ef34deb5177 300 ms250++;
JonFreeman 1:8ef34deb5177 301 if (ms250 > 7) {
JonFreeman 1:8ef34deb5177 302 ms250 = 0;
JonFreeman 1:8ef34deb5177 303 qtrsec_trig = true;
JonFreeman 1:8ef34deb5177 304 }
JonFreeman 1:8ef34deb5177 305 }
JonFreeman 0:23cc72b18e74 306 }
JonFreeman 0:23cc72b18e74 307
JonFreeman 1:8ef34deb5177 308 /*void ISR_tick_32ms (void) //
JonFreeman 0:23cc72b18e74 309 {
JonFreeman 0:23cc72b18e74 310 trigger_32ms = true;
JonFreeman 0:23cc72b18e74 311 }
JonFreeman 0:23cc72b18e74 312 void ISR_tick_250ms (void)
JonFreeman 0:23cc72b18e74 313 {
JonFreeman 0:23cc72b18e74 314 qtrsec_trig = true;
JonFreeman 0:23cc72b18e74 315 }
JonFreeman 1:8ef34deb5177 316 */
JonFreeman 0:23cc72b18e74 317 // End of Interrupt Service Routines
JonFreeman 0:23cc72b18e74 318
JonFreeman 0:23cc72b18e74 319
JonFreeman 0:23cc72b18e74 320 bool inlist (struct ky_bd & a, int key)
JonFreeman 0:23cc72b18e74 321 {
JonFreeman 0:23cc72b18e74 322 int i = 0;
JonFreeman 0:23cc72b18e74 323 while (i < a.count) {
JonFreeman 0:23cc72b18e74 324 if (key == a.ky[i].keynum)
JonFreeman 0:23cc72b18e74 325 return true;
JonFreeman 0:23cc72b18e74 326 i++;
JonFreeman 0:23cc72b18e74 327 }
JonFreeman 0:23cc72b18e74 328 return false;
JonFreeman 0:23cc72b18e74 329 }
JonFreeman 0:23cc72b18e74 330
JonFreeman 0:23cc72b18e74 331
JonFreeman 0:23cc72b18e74 332 void stuff_to_do_every_250us () // Take readings of system voltage and current
JonFreeman 0:23cc72b18e74 333 {
JonFreeman 0:23cc72b18e74 334 if (!trigger_current_read)
JonFreeman 0:23cc72b18e74 335 return;
JonFreeman 0:23cc72b18e74 336 trigger_current_read = false;
JonFreeman 0:23cc72b18e74 337 I_maf[maf_ptr] = ht_amps_ain.read_u16();
JonFreeman 0:23cc72b18e74 338 V_maf[maf_ptr] = ht_volts_ain.read_u16();
JonFreeman 0:23cc72b18e74 339 maf_ptr++;
JonFreeman 0:23cc72b18e74 340 if (maf_ptr > MAF_PTS - 1)
JonFreeman 0:23cc72b18e74 341 maf_ptr = 0;
JonFreeman 0:23cc72b18e74 342 }
JonFreeman 0:23cc72b18e74 343 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : -
JonFreeman 0:23cc72b18e74 344 FWD(A5) REV(A4) PWM Action
JonFreeman 0:23cc72b18e74 345 0 0 0 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 346 0 0 1 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 347 0 1 0 Reverse0
JonFreeman 0:23cc72b18e74 348 0 1 1 Reverse1
JonFreeman 0:23cc72b18e74 349
JonFreeman 0:23cc72b18e74 350 1 0 0 Forward0
JonFreeman 0:23cc72b18e74 351 1 0 1 Forward1
JonFreeman 0:23cc72b18e74 352 1 1 0 Regen Braking
JonFreeman 0:23cc72b18e74 353 1 1 1 Regen Braking
JonFreeman 0:23cc72b18e74 354 */
JonFreeman 0:23cc72b18e74 355 void set_run_mode (int mode)
JonFreeman 1:8ef34deb5177 356 { // NOTE Nov 2017 - Handbrake not implemented
JonFreeman 0:23cc72b18e74 357 if (mode == HANDBRAKE_SLIPPING) slider.handbrake_slipping = true;
JonFreeman 0:23cc72b18e74 358 else slider.handbrake_slipping = false;
JonFreeman 0:23cc72b18e74 359 switch (mode) {
JonFreeman 0:23cc72b18e74 360 // STATES, INACTIVE, RUN, NEUTRAL_DRIFT, REGEN_BRAKE, PARK};
JonFreeman 0:23cc72b18e74 361 // case HANDBRAKE_SLIPPING:
JonFreeman 0:23cc72b18e74 362 // break;
JonFreeman 0:23cc72b18e74 363 case PARK: // PARKED new rom code IS now finished.
JonFreeman 0:23cc72b18e74 364 forward_pin = 0;
JonFreeman 0:23cc72b18e74 365 reverse_pin = 0;
JonFreeman 0:23cc72b18e74 366 slider.state = mode;
JonFreeman 0:23cc72b18e74 367 set_V_limit (0.075); // was 0.1
JonFreeman 0:23cc72b18e74 368 set_I_limit (slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 369 break;
JonFreeman 0:23cc72b18e74 370 case REGEN_BRAKE: // BRAKING, pwm affects degree
JonFreeman 0:23cc72b18e74 371 forward_pin = 1;
JonFreeman 0:23cc72b18e74 372 reverse_pin = 1;
JonFreeman 0:23cc72b18e74 373 slider.state = mode;
JonFreeman 0:23cc72b18e74 374 break;
JonFreeman 0:23cc72b18e74 375 case NEUTRAL_DRIFT:
JonFreeman 0:23cc72b18e74 376 slider.state = mode;
JonFreeman 0:23cc72b18e74 377 set_I_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch
JonFreeman 0:23cc72b18e74 378 set_V_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch
JonFreeman 0:23cc72b18e74 379 break;
JonFreeman 0:23cc72b18e74 380 case RUN:
JonFreeman 0:23cc72b18e74 381 if (slider.direction) {
JonFreeman 0:23cc72b18e74 382 forward_pin = 0;
JonFreeman 0:23cc72b18e74 383 reverse_pin = 1;
JonFreeman 0:23cc72b18e74 384 } else {
JonFreeman 0:23cc72b18e74 385 forward_pin = 1;
JonFreeman 0:23cc72b18e74 386 reverse_pin = 0;
JonFreeman 0:23cc72b18e74 387 }
JonFreeman 0:23cc72b18e74 388 slider.state = mode;
JonFreeman 0:23cc72b18e74 389 break;
JonFreeman 0:23cc72b18e74 390 default:
JonFreeman 0:23cc72b18e74 391 break;
JonFreeman 0:23cc72b18e74 392 }
JonFreeman 0:23cc72b18e74 393 }
JonFreeman 0:23cc72b18e74 394
JonFreeman 0:23cc72b18e74 395 int main()
JonFreeman 0:23cc72b18e74 396 {
JonFreeman 0:23cc72b18e74 397 int c_5 = 0, seconds = 0, minutes = 0;
JonFreeman 1:8ef34deb5177 398 double electrical_power_Watt = 0.0;
JonFreeman 0:23cc72b18e74 399 ky_bd kybd_a, kybd_b;
JonFreeman 0:23cc72b18e74 400 memset (&kybd_a, 0, sizeof(kybd_a));
JonFreeman 0:23cc72b18e74 401 memset (&kybd_b, 0, sizeof(kybd_b));
JonFreeman 0:23cc72b18e74 402
JonFreeman 1:8ef34deb5177 403 // spareio_d8.mode (PullUp); now output driving throttle servo
JonFreeman 0:23cc72b18e74 404 spareio_d9.mode (PullUp);
JonFreeman 0:23cc72b18e74 405 spareio_d10.mode(PullUp);
JonFreeman 0:23cc72b18e74 406
JonFreeman 0:23cc72b18e74 407 Ticker tick250us;
JonFreeman 1:8ef34deb5177 408 // Ticker tick32ms;
JonFreeman 1:8ef34deb5177 409 // Ticker tick250ms;
JonFreeman 0:23cc72b18e74 410
JonFreeman 0:23cc72b18e74 411 // Setup User Interrupt Vectors
JonFreeman 0:23cc72b18e74 412 mot1hall.fall (&ISR_mot1_hall_handler);
JonFreeman 0:23cc72b18e74 413 mot1hall.rise (&ISR_mot1_hall_handler);
JonFreeman 0:23cc72b18e74 414 mot2hall.fall (&ISR_mot2_hall_handler);
JonFreeman 0:23cc72b18e74 415 mot2hall.rise (&ISR_mot2_hall_handler);
JonFreeman 0:23cc72b18e74 416 mot3hall.fall (&ISR_mot3_hall_handler);
JonFreeman 0:23cc72b18e74 417 mot3hall.rise (&ISR_mot3_hall_handler);
JonFreeman 0:23cc72b18e74 418 mot4hall.fall (&ISR_mot4_hall_handler);
JonFreeman 0:23cc72b18e74 419 mot4hall.rise (&ISR_mot4_hall_handler);
JonFreeman 0:23cc72b18e74 420
JonFreeman 1:8ef34deb5177 421 tick250us.attach_us (&ISR_current_reader, 250); // count 125 of these to trig 31.25ms
JonFreeman 1:8ef34deb5177 422 // tick32ms.attach_us (&ISR_tick_32ms, 32001);
JonFreeman 1:8ef34deb5177 423 // tick32ms.attach_us (&ISR_tick_32ms, 31250); // then count 8 pulses per 250ms
JonFreeman 1:8ef34deb5177 424 // tick250ms.attach_us (&ISR_tick_250ms, 250002);
JonFreeman 0:23cc72b18e74 425 pc.baud (9600);
JonFreeman 0:23cc72b18e74 426 GfetT1 = 0;
JonFreeman 0:23cc72b18e74 427 GfetT2 = 0; // two output bits for future use driving horns
JonFreeman 0:23cc72b18e74 428 if (f_r_switch)
JonFreeman 0:23cc72b18e74 429 slider.direction = FWD; // make decision from key switch position here
JonFreeman 0:23cc72b18e74 430 else
JonFreeman 0:23cc72b18e74 431 slider.direction = REV; // make decision from key switch position here
JonFreeman 0:23cc72b18e74 432
JonFreeman 0:23cc72b18e74 433 // max_pwm_ticks = SystemCoreClock / (2 * PWM_HZ); // prescaler min value is 2, or so it would seem. SystemCoreClock returns 216000000 on F746NG board
JonFreeman 0:23cc72b18e74 434 maxv.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:23cc72b18e74 435 maxi.period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:23cc72b18e74 436 set_I_limit (0.0);
JonFreeman 0:23cc72b18e74 437 set_V_limit (0.0);
JonFreeman 0:23cc72b18e74 438
JonFreeman 0:23cc72b18e74 439 pc.printf ("Jon's Touch Screen Loco 2017 sytem starting up %s\r\n", slider.direction ? "Forward":"Reverse");
JonFreeman 0:23cc72b18e74 440 uint8_t lcd_status = touch_screen.Init(lcd.GetXSize(), lcd.GetYSize());
JonFreeman 0:23cc72b18e74 441 if (lcd_status != TS_OK) {
JonFreeman 0:23cc72b18e74 442 lcd.Clear(LCD_COLOR_RED);
JonFreeman 0:23cc72b18e74 443 lcd.SetBackColor(LCD_COLOR_RED);
JonFreeman 0:23cc72b18e74 444 lcd.SetTextColor(LCD_COLOR_WHITE);
JonFreeman 0:23cc72b18e74 445 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE);
JonFreeman 0:23cc72b18e74 446 wait (20);
JonFreeman 0:23cc72b18e74 447 } else {
JonFreeman 0:23cc72b18e74 448 lcd.Clear(LCD_COLOR_DARKBLUE);
JonFreeman 0:23cc72b18e74 449 lcd.SetBackColor(LCD_COLOR_GREEN);
JonFreeman 0:23cc72b18e74 450 lcd.SetTextColor(LCD_COLOR_WHITE);
JonFreeman 0:23cc72b18e74 451 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE);
JonFreeman 0:23cc72b18e74 452 }
JonFreeman 0:23cc72b18e74 453
JonFreeman 0:23cc72b18e74 454 lcd.SetFont(&Font16);
JonFreeman 0:23cc72b18e74 455 lcd.Clear(LCD_COLOR_LIGHTGRAY);
JonFreeman 0:23cc72b18e74 456 setup_buttons(); // draws buttons
JonFreeman 0:23cc72b18e74 457
JonFreeman 0:23cc72b18e74 458 slider.oldpos = 0;
JonFreeman 0:23cc72b18e74 459 slider.loco_speed = 0.0;
JonFreeman 0:23cc72b18e74 460 slider.handbrake_effort = 0.1;
JonFreeman 0:23cc72b18e74 461 slider.position = MAX_POS - 2; // Low down in REGEN_BRAKE position - NOT to power-up in PARK
JonFreeman 0:23cc72b18e74 462 SliderGraphic (slider); // sets slider.state to value determined by slider.position
JonFreeman 0:23cc72b18e74 463 set_run_mode (REGEN_BRAKE); // sets slider.mode
JonFreeman 0:23cc72b18e74 464
JonFreeman 0:23cc72b18e74 465 lcd.SetBackColor(LCD_COLOR_DARKBLUE);
JonFreeman 0:23cc72b18e74 466
JonFreeman 0:23cc72b18e74 467 vm_set(); // Draw 3 analogue meter movements, speedo, voltmeter, ammeter
JonFreeman 0:23cc72b18e74 468
JonFreeman 0:23cc72b18e74 469 mainSDtest();
JonFreeman 0:23cc72b18e74 470
JonFreeman 1:8ef34deb5177 471 double torque_req = 0.0;
JonFreeman 0:23cc72b18e74 472 bool toggle32ms = false;
JonFreeman 0:23cc72b18e74 473 // Main loop
JonFreeman 0:23cc72b18e74 474 while(1) { //
JonFreeman 0:23cc72b18e74 475 struct ky_bd * present_kybd, * previous_kybd;
JonFreeman 0:23cc72b18e74 476 bool sliderpress = false;
JonFreeman 0:23cc72b18e74 477 command_line_interpreter () ; // Do any actions from command line via usb link
JonFreeman 0:23cc72b18e74 478 stuff_to_do_every_250us () ;
JonFreeman 0:23cc72b18e74 479
JonFreeman 0:23cc72b18e74 480 if (trigger_32ms == true) { // Stuff to do every 32 milli secs
JonFreeman 0:23cc72b18e74 481 trigger_32ms = false;
JonFreeman 1:8ef34deb5177 482
JonFreeman 1:8ef34deb5177 483 // CALL THROTTLE HERE - why here ? Ah yes, this initiates servo pulse. Need steady stream of servo pulses even when nothing changes.
JonFreeman 1:8ef34deb5177 484 throttle (torque_req, 2.3);
JonFreeman 1:8ef34deb5177 485
JonFreeman 0:23cc72b18e74 486 toggle32ms = !toggle32ms;
JonFreeman 0:23cc72b18e74 487 if (toggle32ms) {
JonFreeman 0:23cc72b18e74 488 present_kybd = &kybd_a;
JonFreeman 0:23cc72b18e74 489 previous_kybd = &kybd_b;
JonFreeman 0:23cc72b18e74 490 } else {
JonFreeman 0:23cc72b18e74 491 present_kybd = &kybd_b;
JonFreeman 0:23cc72b18e74 492 previous_kybd = &kybd_a;
JonFreeman 0:23cc72b18e74 493 }
JonFreeman 0:23cc72b18e74 494 read_keypresses (*present_kybd);
JonFreeman 0:23cc72b18e74 495 sliderpress = false;
JonFreeman 0:23cc72b18e74 496 slider.recalc_run = false;
JonFreeman 0:23cc72b18e74 497 int j = 0;
JonFreeman 0:23cc72b18e74 498 // if (present2->count > previous_kybd->count) pc.printf ("More presses\r\n");
JonFreeman 0:23cc72b18e74 499 // if (present2->count < previous_kybd->count) pc.printf ("Fewer presses\r\n");
JonFreeman 0:23cc72b18e74 500 if (present_kybd->count || previous_kybd->count) { // at least one key pressed this time or last time
JonFreeman 0:23cc72b18e74 501 int k;
JonFreeman 0:23cc72b18e74 502 double dbl;
JonFreeman 0:23cc72b18e74 503 // pc.printf ("Keys action may be required");
JonFreeman 0:23cc72b18e74 504 // if key in present and ! in previous, found new key press to handle
JonFreeman 0:23cc72b18e74 505 // if key ! in present and in previous, found new key release to handle
JonFreeman 0:23cc72b18e74 506 if (inlist(*present_kybd, SLIDER)) { // Finger is on slider, so Update slider graphic here
JonFreeman 0:23cc72b18e74 507 sliderpress = true;
JonFreeman 0:23cc72b18e74 508 k = present_kybd->slider_y; // get position of finger on slider
JonFreeman 0:23cc72b18e74 509 if (slider.state == RUN && k != slider.position) // Finger has moved within RUN range
JonFreeman 0:23cc72b18e74 510 slider.recalc_run = true;
JonFreeman 0:23cc72b18e74 511 if (slider.state == RUN && k >= NEUTRAL_VAL) // Finger has moved from RUN to BRAKE range
JonFreeman 0:23cc72b18e74 512 slider.position = k = NEUTRAL_VAL; // kill drive for rapid reaction to braking
JonFreeman 0:23cc72b18e74 513
JonFreeman 0:23cc72b18e74 514 else { // nice slow non-jerky glidey movement required
JonFreeman 0:23cc72b18e74 515 dbl = (double)(k - slider.position);
JonFreeman 1:8ef34deb5177 516 dbl /= 13.179; // Where did 13.179 come from ?
JonFreeman 0:23cc72b18e74 517 if (dbl < 0.0)
JonFreeman 0:23cc72b18e74 518 dbl -= 1.0;
JonFreeman 0:23cc72b18e74 519 if (dbl > 0.0)
JonFreeman 0:23cc72b18e74 520 dbl += 1.0;
JonFreeman 0:23cc72b18e74 521 slider.position += (int)dbl;
JonFreeman 0:23cc72b18e74 522 }
JonFreeman 0:23cc72b18e74 523 SliderGraphic (slider); // sets slider.state to value determined by slider.position
JonFreeman 0:23cc72b18e74 524 set_run_mode (slider.state);
JonFreeman 0:23cc72b18e74 525 draw_button_hilight (SLIDER, LCD_COLOR_YELLOW) ;
JonFreeman 0:23cc72b18e74 526
JonFreeman 0:23cc72b18e74 527 if (slider.state == REGEN_BRAKE) {
JonFreeman 0:23cc72b18e74 528 double brake_effort = ((double)(slider.position - NEUTRAL_VAL)
JonFreeman 0:23cc72b18e74 529 / (double)(MAX_POS - NEUTRAL_VAL));
JonFreeman 0:23cc72b18e74 530 // brake_effort normalised to range 0.0 to 1.0
JonFreeman 0:23cc72b18e74 531 brake_effort *= 0.97; // upper limit to braking effort, observed effect before was quite fierce
JonFreeman 0:23cc72b18e74 532 pc.printf ("Brake effort %.2f\r\n", brake_effort);
JonFreeman 0:23cc72b18e74 533 /* set_pwm (brake_effort); */
JonFreeman 0:23cc72b18e74 534 set_V_limit (sqrt(brake_effort)); // sqrt gives more linear feel to control
JonFreeman 0:23cc72b18e74 535 set_I_limit (1.0);
JonFreeman 0:23cc72b18e74 536 }
JonFreeman 0:23cc72b18e74 537 } else { // pc.printf ("Slider not touched\r\n");
JonFreeman 0:23cc72b18e74 538 }
JonFreeman 0:23cc72b18e74 539
JonFreeman 0:23cc72b18e74 540 j = 0;
JonFreeman 0:23cc72b18e74 541 while (j < present_kybd->count) { // handle new key presses
JonFreeman 0:23cc72b18e74 542 k = present_kybd->ky[j++].keynum;
JonFreeman 0:23cc72b18e74 543 if (inlist(*present_kybd, k)) {
JonFreeman 0:23cc72b18e74 544 switch (k) { // Here for auto-repeat type key behaviour
JonFreeman 0:23cc72b18e74 545 case 21: // key is 'voltmeter'
JonFreeman 0:23cc72b18e74 546 // set_V_limit (last_pwm * 1.002 + 0.001);
JonFreeman 0:23cc72b18e74 547 break;
JonFreeman 0:23cc72b18e74 548 case 22: // key is 'ammeter'
JonFreeman 0:23cc72b18e74 549 // set_V_limit (last_pwm * 0.99);
JonFreeman 0:23cc72b18e74 550 break;
JonFreeman 0:23cc72b18e74 551 } // endof switch (k)
JonFreeman 0:23cc72b18e74 552 } // endof if (inlist(*present2, k)) {
JonFreeman 0:23cc72b18e74 553 if (inlist(*present_kybd, k) && !inlist(*previous_kybd, k)) {
JonFreeman 0:23cc72b18e74 554 pc.printf ("Handle Press %d\r\n", k);
JonFreeman 0:23cc72b18e74 555 draw_button_hilight (k, LCD_COLOR_YELLOW) ;
JonFreeman 0:23cc72b18e74 556 switch (k) { // Handle new touch screen button presses here - single action per press, not autorepeat
JonFreeman 0:23cc72b18e74 557 case SPEEDO_BUT: //
JonFreeman 0:23cc72b18e74 558 pc.printf ("Speedometer key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 559 break;
JonFreeman 0:23cc72b18e74 560 case VMETER_BUT: //
JonFreeman 0:23cc72b18e74 561 pc.printf ("Voltmeter key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 562 break;
JonFreeman 0:23cc72b18e74 563 case AMETER_BUT: //
JonFreeman 0:23cc72b18e74 564 pc.printf ("Ammeter key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 565 break;
JonFreeman 0:23cc72b18e74 566 default:
JonFreeman 0:23cc72b18e74 567 pc.printf ("Unhandled keypress %d\r\n", k);
JonFreeman 0:23cc72b18e74 568 break;
JonFreeman 0:23cc72b18e74 569 } // endof switch (button)
JonFreeman 0:23cc72b18e74 570 }
JonFreeman 0:23cc72b18e74 571 } // endof while - handle new key presses
JonFreeman 0:23cc72b18e74 572 j = 0;
JonFreeman 0:23cc72b18e74 573 while (j < previous_kybd->count) { // handle new key releases
JonFreeman 0:23cc72b18e74 574 k = previous_kybd->ky[j++].keynum;
JonFreeman 0:23cc72b18e74 575 if (inlist(*previous_kybd, k) && !inlist(*present_kybd, k)) {
JonFreeman 0:23cc72b18e74 576 pc.printf ("Handle Release %d\r\n", k);
JonFreeman 0:23cc72b18e74 577 draw_button_hilight (k, LCD_COLOR_DARKBLUE) ;
JonFreeman 0:23cc72b18e74 578 }
JonFreeman 0:23cc72b18e74 579 } // endof while - handle new key releases
JonFreeman 0:23cc72b18e74 580 } // endof at least one key pressed this time or last time
JonFreeman 0:23cc72b18e74 581
JonFreeman 0:23cc72b18e74 582 if (sliderpress == false) { // need to glide dead-mans function towards neutral here
JonFreeman 0:23cc72b18e74 583 if (slider.position < NEUTRAL_VAL) {
JonFreeman 0:23cc72b18e74 584 slider.position += 1 + (NEUTRAL_VAL - slider.position) / DAMPER_DECAY;
JonFreeman 0:23cc72b18e74 585 SliderGraphic (slider);
JonFreeman 0:23cc72b18e74 586 slider.recalc_run = true;
JonFreeman 0:23cc72b18e74 587 }
JonFreeman 0:23cc72b18e74 588 }
JonFreeman 0:23cc72b18e74 589
JonFreeman 0:23cc72b18e74 590 if (slider.recalc_run) { // range of slider.position in RUN mode is min_pos_() to NEUTRAL_VAL - 1
JonFreeman 0:23cc72b18e74 591 slider.recalc_run = false; // All RUN power and pwm calcs done here
JonFreeman 0:23cc72b18e74 592 int b = slider.position;
JonFreeman 1:8ef34deb5177 593 // double torque_req; // now declared above to be used as parameter for throttle
JonFreeman 0:23cc72b18e74 594 if (b > NEUTRAL_VAL)
JonFreeman 0:23cc72b18e74 595 b = NEUTRAL_VAL;
JonFreeman 0:23cc72b18e74 596 if (b < MIN_POS) // if finger position is above top of slider limit
JonFreeman 0:23cc72b18e74 597 b = MIN_POS;
JonFreeman 0:23cc72b18e74 598 b = NEUTRAL_VAL - b; // now got integer going positive for increasing power demand
JonFreeman 0:23cc72b18e74 599 torque_req = (double) b;
JonFreeman 0:23cc72b18e74 600 torque_req /= (NEUTRAL_VAL - MIN_POS); // in range 0.0 to 1.0
JonFreeman 0:23cc72b18e74 601 pc.printf ("torque_rec = %.3f, last_pwm = %.3f\r\n", torque_req, last_pwm);
JonFreeman 0:23cc72b18e74 602 set_I_limit (torque_req);
JonFreeman 0:23cc72b18e74 603 if (torque_req < 0.05)
JonFreeman 0:23cc72b18e74 604 set_V_limit (last_pwm / 2.0);
JonFreeman 0:23cc72b18e74 605 else {
JonFreeman 0:23cc72b18e74 606 if (last_pwm < 0.99)
JonFreeman 0:23cc72b18e74 607 set_V_limit (last_pwm + 0.05); // ramp voltage up rather than slam to max
JonFreeman 0:23cc72b18e74 608 }
JonFreeman 0:23cc72b18e74 609 }
JonFreeman 0:23cc72b18e74 610 } // endof doing 32ms stuff
JonFreeman 0:23cc72b18e74 611
JonFreeman 0:23cc72b18e74 612 if (qtrsec_trig == true) { // do every quarter second stuff here
JonFreeman 0:23cc72b18e74 613 qtrsec_trig = false;
JonFreeman 0:23cc72b18e74 614 speed.qtr_sec_update ();
JonFreeman 0:23cc72b18e74 615 double speedmph = speed.MPH(), amps = 0.0 - read_ammeter(), volts = read_voltmeter();
JonFreeman 0:23cc72b18e74 616 //static const double mph_2_mm_per_sec = 447.04; // exact
JonFreeman 0:23cc72b18e74 617 // double mm_travelled_in_qtrsec = speedmph * mph_2_mm_per_sec / 4.0;
JonFreeman 0:23cc72b18e74 618 slider.loco_speed = speedmph;
JonFreeman 1:8ef34deb5177 619 electrical_power_Watt = volts * amps; // visible throughout main
JonFreeman 1:8ef34deb5177 620 update_meters (speedmph, electrical_power_Watt, volts) ; // displays speed, volts and power (volts times amps)
JonFreeman 0:23cc72b18e74 621 // update_meters (7.5, amps, volts) ;
JonFreeman 0:23cc72b18e74 622 led_grn = !led_grn;
JonFreeman 0:23cc72b18e74 623 if (slider.state == PARK) {
JonFreeman 0:23cc72b18e74 624 if (speedmph > LOCO_HANDBRAKE_ESCAPE_SPEED / 4.0) {
JonFreeman 0:23cc72b18e74 625 slider.handbrake_effort *= 1.1;
JonFreeman 0:23cc72b18e74 626 if (slider.handbrake_effort > 0.55) slider.handbrake_effort = 0.55;
JonFreeman 0:23cc72b18e74 627 set_run_mode (PARK);
JonFreeman 0:23cc72b18e74 628 pc.printf ("Handbrake slipping, effort %.2f\r\n", slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 629 }
JonFreeman 0:23cc72b18e74 630 if (speedmph < 0.02) {
JonFreeman 0:23cc72b18e74 631 slider.handbrake_effort *= 0.9;
JonFreeman 0:23cc72b18e74 632 if (slider.handbrake_effort < 0.05) slider.handbrake_effort = 0.05;
JonFreeman 0:23cc72b18e74 633 set_run_mode (PARK);
JonFreeman 0:23cc72b18e74 634 pc.printf ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 635 }
JonFreeman 0:23cc72b18e74 636 }
JonFreeman 0:23cc72b18e74 637 c_5++;
JonFreeman 0:23cc72b18e74 638 // Can do stuff once per second here
JonFreeman 0:23cc72b18e74 639 if(c_5 > 3) {
JonFreeman 0:23cc72b18e74 640 c_5 = 0;
JonFreeman 0:23cc72b18e74 641 seconds++;
JonFreeman 0:23cc72b18e74 642 if (seconds > 59) {
JonFreeman 0:23cc72b18e74 643 seconds = 0;
JonFreeman 0:23cc72b18e74 644 minutes++;
JonFreeman 0:23cc72b18e74 645 // do once per minute stuff here
JonFreeman 0:23cc72b18e74 646 } // fall back into once per second
JonFreeman 1:8ef34deb5177 647 // if(SD_state == SD_OK) {
JonFreeman 1:8ef34deb5177 648 if(read_SD_state() == true) {
JonFreeman 0:23cc72b18e74 649 uint32_t distance = speed.metres_travelled();
JonFreeman 0:23cc72b18e74 650 char dist[20];
JonFreeman 0:23cc72b18e74 651 sprintf (dist, "%05d m", distance);
JonFreeman 0:23cc72b18e74 652 displaytext (236, 226, 2, dist);
JonFreeman 0:23cc72b18e74 653 update_SD_card (); // Buffers data for SD card, writes when buffer filled
JonFreeman 0:23cc72b18e74 654 }
JonFreeman 0:23cc72b18e74 655 // calc_motor_amps( mva);
JonFreeman 0:23cc72b18e74 656 } // endof if(c_5 > 3
JonFreeman 0:23cc72b18e74 657 } // endof if (qtrsec_trig == true) {
JonFreeman 0:23cc72b18e74 658 } // endof while(1) main programme loop
JonFreeman 0:23cc72b18e74 659 } // endof int main() {
JonFreeman 0:23cc72b18e74 660
JonFreeman 0:23cc72b18e74 661