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@0:23cc72b18e74, 2017-11-12 (annotated)

Committer:
JonFreeman
Date:
Sun Nov 12 06:26:29 2017 +0000
Revision:
0:23cc72b18e74
Child:
1:8ef34deb5177
Electric loco controller up to Nov 2017 includes SD odometer

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 0:23cc72b18e74 4 // Last Updated 12 April 2017
JonFreeman 0:23cc72b18e74 5 // Touch Screen Loco 2017 - WITH SD card data logger functions
JonFreeman 0:23cc72b18e74 6
JonFreeman 0:23cc72b18e74 7 // This code runs on STM 32F746NG DISCO module, high performance ARM Cortex with touch screen
JonFreeman 0:23cc72b18e74 8 // ffi on ST module -> https://developer.mbed.org/platforms/ST-Discovery-F746NG/
JonFreeman 0:23cc72b18e74 9 // Board plugs onto simple mother-board containing low voltage power supplies, interfacing buffers, connectors etc.
JonFreeman 0:23cc72b18e74 10 // See www.jons-workshop.com ffi on hardware.
JonFreeman 0:23cc72b18e74 11
JonFreeman 0:23cc72b18e74 12 // Design provides PWM outputs to drive up to four brushless motor drive modules, each able to return speed information.
JonFreeman 0:23cc72b18e74 13 // Output signals are dual PWM, one to set max motor voltage, other to set max motor current.
JonFreeman 0:23cc72b18e74 14 // 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 15 // Regenerative braking is included in the design.
JonFreeman 0:23cc72b18e74 16 // 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 17 // may result in over-voltage damage to batteries or power electronics.
JonFreeman 0:23cc72b18e74 18
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 0:23cc72b18e74 24 #include "SD_DISCO_F746NG.h"
JonFreeman 0:23cc72b18e74 25 #include "dro.h"
JonFreeman 0:23cc72b18e74 26
JonFreeman 0:23cc72b18e74 27
JonFreeman 0:23cc72b18e74 28
JonFreeman 0:23cc72b18e74 29 // Design Topology
JonFreeman 0:23cc72b18e74 30 // This F746NG is the single loco control computer.
JonFreeman 0:23cc72b18e74 31 // Assumed 4 motor controllers driven from same signal set via multiple opto / buffers
JonFreeman 0:23cc72b18e74 32 // Outputs are : -
JonFreeman 0:23cc72b18e74 33 // FastPWM maxv on D12 - in drive, sets motor volts to pwm proportion of available volts. Also used in regen braking
JonFreeman 0:23cc72b18e74 34 // 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 35 // DigitalOut reverse (D7) - D6,7 select fwd, rev, brake, parking brake
JonFreeman 0:23cc72b18e74 36 // DigitalOut forward (D6)
JonFreeman 0:23cc72b18e74 37 // Inputs are : -
JonFreeman 0:23cc72b18e74 38 // AnalogIn ht_amps_ain (A0); // Jan 2017
JonFreeman 0:23cc72b18e74 39 // AnalogIn ht_volts_ain (A1); // Jan 2017
JonFreeman 0:23cc72b18e74 40 // InterruptIn mot4hall (D2);
JonFreeman 0:23cc72b18e74 41 // InterruptIn mot3hall (D3);
JonFreeman 0:23cc72b18e74 42 // InterruptIn mot2hall (D4);
JonFreeman 0:23cc72b18e74 43 // InterruptIn mot1hall (D5);
JonFreeman 0:23cc72b18e74 44
JonFreeman 0:23cc72b18e74 45 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : -
JonFreeman 0:23cc72b18e74 46 FWD(A5) REV(A4) PWM Action
JonFreeman 0:23cc72b18e74 47 0 0 0 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 48 0 0 1 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 49 0 1 0 Reverse0
JonFreeman 0:23cc72b18e74 50 0 1 1 Reverse1
JonFreeman 0:23cc72b18e74 51
JonFreeman 0:23cc72b18e74 52 1 0 0 Forward0
JonFreeman 0:23cc72b18e74 53 1 0 1 Forward1
JonFreeman 0:23cc72b18e74 54 1 1 0 Regen Braking
JonFreeman 0:23cc72b18e74 55 1 1 1 Regen Braking
JonFreeman 0:23cc72b18e74 56 */
JonFreeman 0:23cc72b18e74 57
JonFreeman 0:23cc72b18e74 58 LCD_DISCO_F746NG lcd;
JonFreeman 0:23cc72b18e74 59 TS_DISCO_F746NG touch_screen;
JonFreeman 0:23cc72b18e74 60 SD_DISCO_F746NG sd;
JonFreeman 0:23cc72b18e74 61
JonFreeman 0:23cc72b18e74 62 FastPWM maxv (D12, 1),
JonFreeman 0:23cc72b18e74 63 maxi (D11, 1); // pin, prescaler value
JonFreeman 0:23cc72b18e74 64 Serial pc (USBTX, USBRX); // Comms to 'PuTTY' or similar comms programme on pc
JonFreeman 0:23cc72b18e74 65
JonFreeman 0:23cc72b18e74 66 DigitalOut reverse_pin (D7); //
JonFreeman 0:23cc72b18e74 67 DigitalOut forward_pin (D6); //these two decode to fwd, rev, regen_braking and park
JonFreeman 0:23cc72b18e74 68 DigitalOut GfetT2 (D14); // a horn
JonFreeman 0:23cc72b18e74 69 DigitalOut GfetT1 (D15); // another horn
JonFreeman 0:23cc72b18e74 70 DigitalOut led_grn (LED1); // the only on board user led
JonFreeman 0:23cc72b18e74 71
JonFreeman 0:23cc72b18e74 72 DigitalIn f_r_switch (D0); // Reads position of centre-off ignition switch
JonFreeman 0:23cc72b18e74 73 DigitalIn spareio_d8 (D8);
JonFreeman 0:23cc72b18e74 74 DigitalIn spareio_d9 (D9);
JonFreeman 0:23cc72b18e74 75 DigitalIn spareio_d10 (D10); // D8, D9, D10 wired to jumper on pcb - not used to Apr 2017
JonFreeman 0:23cc72b18e74 76
JonFreeman 0:23cc72b18e74 77 AnalogIn ht_volts_ain (A0); // Jan 2017
JonFreeman 0:23cc72b18e74 78 AnalogIn ht_amps_ain (A1); // Jan 2017
JonFreeman 0:23cc72b18e74 79 AnalogIn spare_ain2 (A2);
JonFreeman 0:23cc72b18e74 80 AnalogIn spare_ain3 (A3);
JonFreeman 0:23cc72b18e74 81 AnalogIn spare_ain4 (A4); // hardware on pcb for these 3 spare analogue inputs - not used to Apr 2017
JonFreeman 0:23cc72b18e74 82 //AnalogIn spare_ain5 (A5); // causes display flicker !
JonFreeman 0:23cc72b18e74 83
JonFreeman 0:23cc72b18e74 84 InterruptIn mot4hall (D2); // One Hall sensor signal from each motor fed back to measure speed
JonFreeman 0:23cc72b18e74 85 InterruptIn mot3hall (D3);
JonFreeman 0:23cc72b18e74 86 InterruptIn mot2hall (D4);
JonFreeman 0:23cc72b18e74 87 InterruptIn mot1hall (D5);
JonFreeman 0:23cc72b18e74 88
JonFreeman 0:23cc72b18e74 89 extern int get_button_press (struct point & pt) ;
JonFreeman 0:23cc72b18e74 90 extern void displaytext (int x, int y, const int font, uint32_t BCol, uint32_t TCol, char * txt) ;
JonFreeman 0:23cc72b18e74 91 extern void displaytext (int x, int y, const int font, char * txt) ;
JonFreeman 0:23cc72b18e74 92 extern void displaytext (int x, int y, char * txt) ;
JonFreeman 0:23cc72b18e74 93 extern void setup_buttons () ;
JonFreeman 0:23cc72b18e74 94 extern void draw_numeric_keypad (int colour) ;
JonFreeman 0:23cc72b18e74 95 extern void draw_button_hilight (int bu, int colour) ;
JonFreeman 0:23cc72b18e74 96 extern void read_presses (int * a) ;
JonFreeman 0:23cc72b18e74 97 extern void read_keypresses (struct ky_bd & a) ;
JonFreeman 0:23cc72b18e74 98 extern void SliderGraphic (struct slide & q) ;
JonFreeman 0:23cc72b18e74 99 extern void vm_set () ;
JonFreeman 0:23cc72b18e74 100 extern void update_meters (double, double, double) ;
JonFreeman 0:23cc72b18e74 101 extern void command_line_interpreter () ;
JonFreeman 0:23cc72b18e74 102
JonFreeman 0:23cc72b18e74 103 static const int NUMBER_OF_MOTORS = 4,
JonFreeman 0:23cc72b18e74 104 SD_BLOCKSIZE = 512, /* SD card data Block Size in Bytes */
JonFreeman 0:23cc72b18e74 105 DAMPER_DECAY = 42, // Small num -> fast 'viscous damper' on dead-mans function with finger removed from panel
JonFreeman 0:23cc72b18e74 106 MAF_PTS = 140, // Moving Average Filter points
JonFreeman 0:23cc72b18e74 107 PWM_HZ = 13000,
JonFreeman 0:23cc72b18e74 108 // PWM_HZ = 2000, // Used this to experiment on much bigger motor
JonFreeman 0:23cc72b18e74 109 MAX_PWM_TICKS = 108000000 / PWM_HZ, // 108000000 for F746N, due to cpu clock = 216 MHz
JonFreeman 0:23cc72b18e74 110 FWD = 0,
JonFreeman 0:23cc72b18e74 111 REV = ~FWD;
JonFreeman 0:23cc72b18e74 112
JonFreeman 0:23cc72b18e74 113 static const double
JonFreeman 0:23cc72b18e74 114 MOTOR_PINION_T = 17.0, // motor pinion teeth, wheel gear teeth and wheel dia required to calculate speed and distance.
JonFreeman 0:23cc72b18e74 115 WHEEL_GEAR_T = 76.0,
JonFreeman 0:23cc72b18e74 116 WHEEL_DIA_MM = 147.0,
JonFreeman 0:23cc72b18e74 117 WHEEL_CIRCUMFERENCE_METRE = PI * WHEEL_DIA_MM / 1000.0,
JonFreeman 0:23cc72b18e74 118 PULSES_PER_WHEEL_REV = 32.0 * WHEEL_GEAR_T / MOTOR_PINION_T,
JonFreeman 0:23cc72b18e74 119 PULSES_PER_METRE = PULSES_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_METRE,
JonFreeman 0:23cc72b18e74 120 rpm2mph = 60.0 // = Motor Revs per hour;
JonFreeman 0:23cc72b18e74 121 * (MOTOR_PINION_T / WHEEL_GEAR_T) // = Wheel rev per hour
JonFreeman 0:23cc72b18e74 122 * WHEEL_CIRCUMFERENCE_METRE // = metres per hour
JonFreeman 0:23cc72b18e74 123 * 39.37 // = inches per hour
JonFreeman 0:23cc72b18e74 124 / (1760 * 36) // = miles per hour
JonFreeman 0:23cc72b18e74 125 ;
JonFreeman 0:23cc72b18e74 126
JonFreeman 0:23cc72b18e74 127 // Assume SD card size is 4Gbyte, might be 8 Gbyte
JonFreeman 0:23cc72b18e74 128 // Then can use 8388608 blocks (8 * 1024 * 1024)
JonFreeman 0:23cc72b18e74 129
JonFreeman 0:23cc72b18e74 130 uint64_t SD_blockptr = 0;
JonFreeman 0:23cc72b18e74 131 uint32_t SDBuffer[(SD_BLOCKSIZE >> 2)]; // = space for (512 / 4) uint32_t
JonFreeman 0:23cc72b18e74 132 uint8_t SD_state = SD_OK, sd_jf = 0;
JonFreeman 0:23cc72b18e74 133
JonFreeman 0:23cc72b18e74 134 static const uint64_t GIGAB = 1024 * 1024 * 1024;
JonFreeman 0:23cc72b18e74 135 //static const uint64_t SDBLOCKS = (GIGAB / SD_BLOCKSIZE) * 4; // software drives SD up to 4Gbyte only - 8 M block
JonFreeman 0:23cc72b18e74 136 static const uint64_t SDBLOCKS = (GIGAB / SD_BLOCKSIZE) * 2; // software drives SD up to 4Gbyte only - 8 M block
JonFreeman 0:23cc72b18e74 137 // If data logger takes 2 minutes to fill 1 block, a 4G card takes 32 years run-time to fill
JonFreeman 0:23cc72b18e74 138 // If system generates approx 320 pulses per metre travelled, max distance recordable in uint32_t is 65536 * 65536 / 320 = 13421.772 km
JonFreeman 0:23cc72b18e74 139
JonFreeman 0:23cc72b18e74 140 //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 } ;
JonFreeman 0:23cc72b18e74 141 struct slide slider ;
JonFreeman 0:23cc72b18e74 142
JonFreeman 0:23cc72b18e74 143
JonFreeman 0:23cc72b18e74 144
JonFreeman 0:23cc72b18e74 145 //static const double mph_2_mm_per_sec = 447.04; // exact
JonFreeman 0:23cc72b18e74 146
JonFreeman 0:23cc72b18e74 147 int V_maf[MAF_PTS + 2], I_maf[MAF_PTS + 2], maf_ptr = 0;
JonFreeman 0:23cc72b18e74 148 //uint32_t Hall_pulse[8] = {0,0,0,0,0,0,0,0}; // more than max number of motors
JonFreeman 0:23cc72b18e74 149 uint32_t Hall_pulse[8] = {1,1,1,1,1,1,1,1}; // more than max number of motors
JonFreeman 0:23cc72b18e74 150 uint32_t historic_distance = 0;
JonFreeman 0:23cc72b18e74 151
JonFreeman 0:23cc72b18e74 152 bool qtrsec_trig = false;
JonFreeman 0:23cc72b18e74 153 bool trigger_current_read = false;
JonFreeman 0:23cc72b18e74 154 volatile bool trigger_32ms = false;
JonFreeman 0:23cc72b18e74 155
JonFreeman 0:23cc72b18e74 156 double last_pwm = 0.0;
JonFreeman 0:23cc72b18e74 157
JonFreeman 0:23cc72b18e74 158 bool sd_error () { // Test and Clear error code sd_jf, return true if any error bits set, false on 0
JonFreeman 0:23cc72b18e74 159 bool retval = false;
JonFreeman 0:23cc72b18e74 160 if (sd_jf != 0) {
JonFreeman 0:23cc72b18e74 161 retval = true;
JonFreeman 0:23cc72b18e74 162 sd_jf = 0;
JonFreeman 0:23cc72b18e74 163 }
JonFreeman 0:23cc72b18e74 164 return retval;
JonFreeman 0:23cc72b18e74 165 }
JonFreeman 0:23cc72b18e74 166
JonFreeman 0:23cc72b18e74 167 bool check_SD_block_clear (uint32_t block) {
JonFreeman 0:23cc72b18e74 168 uint32_t b[(SD_BLOCKSIZE >> 2)];
JonFreeman 0:23cc72b18e74 169 SD_state = sd.ReadBlocks(b, (uint64_t)(SD_BLOCKSIZE * block), SD_BLOCKSIZE, 1);
JonFreeman 0:23cc72b18e74 170 if(SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 171 sd_jf = 1;
JonFreeman 0:23cc72b18e74 172 pc.printf ("Failed, not SD_OK, erasing block %d\r\n", block);
JonFreeman 0:23cc72b18e74 173 return false;
JonFreeman 0:23cc72b18e74 174 }
JonFreeman 0:23cc72b18e74 175 for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++)
JonFreeman 0:23cc72b18e74 176 if (b[i] != 0)
JonFreeman 0:23cc72b18e74 177 return false;
JonFreeman 0:23cc72b18e74 178 return true;
JonFreeman 0:23cc72b18e74 179 }
JonFreeman 0:23cc72b18e74 180
JonFreeman 0:23cc72b18e74 181 /*bool erase_block (uint32_t block2erase) {
JonFreeman 0:23cc72b18e74 182 uint64_t addr = SD_BLOCKSIZE * (uint64_t)block2erase;
JonFreeman 0:23cc72b18e74 183 SD_state = sd.Erase(addr, addr + SD_BLOCKSIZE);
JonFreeman 0:23cc72b18e74 184 if (SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 185 sd_jf = 1; // Assert error flag
JonFreeman 0:23cc72b18e74 186 pc.printf ("Failed, not SD_OK, erasing block %d\r\n", block2erase);
JonFreeman 0:23cc72b18e74 187 return false;
JonFreeman 0:23cc72b18e74 188 }
JonFreeman 0:23cc72b18e74 189 return check_SD_block_clear (block2erase);
JonFreeman 0:23cc72b18e74 190 }*/
JonFreeman 0:23cc72b18e74 191
JonFreeman 0:23cc72b18e74 192 bool SD_find_next_clear_block (uint64_t * blok) { // Successive approximation algorithm to quickly find next vacant SD card 512 byte block
JonFreeman 0:23cc72b18e74 193 uint64_t toaddsub = SDBLOCKS / 2, stab = SDBLOCKS - 1;
JonFreeman 0:23cc72b18e74 194 pc.printf ("At SD_find_next_clear_block \r\n");
JonFreeman 0:23cc72b18e74 195 while (toaddsub) {
JonFreeman 0:23cc72b18e74 196 pc.printf ("stab = %lld, toadsub = %lld\r\n", stab, toaddsub); // lld for long long int
JonFreeman 0:23cc72b18e74 197 bool clear_block = true;
JonFreeman 0:23cc72b18e74 198 SD_state = sd.ReadBlocks(SDBuffer, SD_BLOCKSIZE * stab, SD_BLOCKSIZE, 1);
JonFreeman 0:23cc72b18e74 199 if(SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 200 sd_jf = 1;
JonFreeman 0:23cc72b18e74 201 pc.printf ("SD error in SD_find_next_clear_block, returning -1\r\n");
JonFreeman 0:23cc72b18e74 202 return false;
JonFreeman 0:23cc72b18e74 203 }
JonFreeman 0:23cc72b18e74 204 for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++) {
JonFreeman 0:23cc72b18e74 205 if (SDBuffer[i] != 0) {
JonFreeman 0:23cc72b18e74 206 clear_block = false;
JonFreeman 0:23cc72b18e74 207 pc.printf ("Buff at %d contains %x\r\n", i, SDBuffer[i]);
JonFreeman 0:23cc72b18e74 208 i = SD_BLOCKSIZE; // to exit loop
JonFreeman 0:23cc72b18e74 209 }
JonFreeman 0:23cc72b18e74 210 }
JonFreeman 0:23cc72b18e74 211 if (clear_block)
JonFreeman 0:23cc72b18e74 212 stab -= toaddsub;
JonFreeman 0:23cc72b18e74 213 else
JonFreeman 0:23cc72b18e74 214 stab += toaddsub;
JonFreeman 0:23cc72b18e74 215 toaddsub >>= 1;
JonFreeman 0:23cc72b18e74 216 }
JonFreeman 0:23cc72b18e74 217 if (!check_SD_block_clear(stab))
JonFreeman 0:23cc72b18e74 218 stab++;
JonFreeman 0:23cc72b18e74 219 if (sd_error()) { // sd_error() tests and clears error bits
JonFreeman 0:23cc72b18e74 220 pc.printf ("check_SD_block_clear(%ld)returned ERROR in SD_find_next_clear_block\r\n", stab);
JonFreeman 0:23cc72b18e74 221 sd_jf = 1; // reassert error flag
JonFreeman 0:23cc72b18e74 222 return false;
JonFreeman 0:23cc72b18e74 223 }
JonFreeman 0:23cc72b18e74 224 pc.printf ("Completed find_next, stab = %d\r\n", stab);
JonFreeman 0:23cc72b18e74 225 *blok = stab; // block number of next free block
JonFreeman 0:23cc72b18e74 226 return true;
JonFreeman 0:23cc72b18e74 227 }
JonFreeman 0:23cc72b18e74 228
JonFreeman 0:23cc72b18e74 229 bool SD_card_erase_all (void) { // assumes sd card is 4 Gbyte, erases 4 Gbyte. Called from CLI
JonFreeman 0:23cc72b18e74 230 uint64_t EndAddr = GIGAB * 4,
JonFreeman 0:23cc72b18e74 231 StartAddr = 0LL;
JonFreeman 0:23cc72b18e74 232 sd_jf = 0;
JonFreeman 0:23cc72b18e74 233 pc.printf ("Erasing SD card ... ");
JonFreeman 0:23cc72b18e74 234 // uint8_t Erase(uint64_t StartAddr, uint64_t EndAddr);
JonFreeman 0:23cc72b18e74 235 SD_state = sd.Erase(StartAddr, EndAddr);
JonFreeman 0:23cc72b18e74 236 if (SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 237 pc.printf ("SD_card_erase_all FAILED\r\n");
JonFreeman 0:23cc72b18e74 238 sd_jf = 1;
JonFreeman 0:23cc72b18e74 239 return false;
JonFreeman 0:23cc72b18e74 240 }
JonFreeman 0:23cc72b18e74 241 pc.printf ("no error detected\r\n");
JonFreeman 0:23cc72b18e74 242 return true;
JonFreeman 0:23cc72b18e74 243 }
JonFreeman 0:23cc72b18e74 244
JonFreeman 0:23cc72b18e74 245
JonFreeman 0:23cc72b18e74 246 bool mainSDtest()
JonFreeman 0:23cc72b18e74 247 {
JonFreeman 0:23cc72b18e74 248 SD_state = sd.Init();
JonFreeman 0:23cc72b18e74 249 if(SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 250 pc.printf ("sd.Init set SD_state to %0x\r\n", SD_state);
JonFreeman 0:23cc72b18e74 251 if(SD_state == MSD_ERROR_SD_NOT_PRESENT) {
JonFreeman 0:23cc72b18e74 252 pc.printf("SD shall be inserted before running test\r\n");
JonFreeman 0:23cc72b18e74 253 } else {
JonFreeman 0:23cc72b18e74 254 pc.printf("SD Initialization : FAIL.\r\n");
JonFreeman 0:23cc72b18e74 255 }
JonFreeman 0:23cc72b18e74 256 pc.printf("SD Test Aborted.\r\n");
JonFreeman 0:23cc72b18e74 257 return false;
JonFreeman 0:23cc72b18e74 258 }
JonFreeman 0:23cc72b18e74 259 // else { // SD_state is SD_OK
JonFreeman 0:23cc72b18e74 260 pc.printf("SD Initialization : OK.\r\n");
JonFreeman 0:23cc72b18e74 261
JonFreeman 0:23cc72b18e74 262
JonFreeman 0:23cc72b18e74 263
JonFreeman 0:23cc72b18e74 264 // SD_card_erase_all();
JonFreeman 0:23cc72b18e74 265 // if (sd_error())
JonFreeman 0:23cc72b18e74 266 // pc.printf ("SD_card_erase_all() reports ERROR");
JonFreeman 0:23cc72b18e74 267
JonFreeman 0:23cc72b18e74 268
JonFreeman 0:23cc72b18e74 269
JonFreeman 0:23cc72b18e74 270 SD_find_next_clear_block(& SD_blockptr);
JonFreeman 0:23cc72b18e74 271 pc.printf ("SD_find_next_clear_block returned %lld\r\n\n\n", SD_blockptr);
JonFreeman 0:23cc72b18e74 272 if (sd_error()) {
JonFreeman 0:23cc72b18e74 273 pc.printf ("***** ERROR returned from SD_find_next_clear_block ***** SD ops aborted\r\n");
JonFreeman 0:23cc72b18e74 274 return false;
JonFreeman 0:23cc72b18e74 275 }
JonFreeman 0:23cc72b18e74 276 pc.printf("SD_find_next_clear_block() returned %ld\r\n", SD_blockptr);
JonFreeman 0:23cc72b18e74 277 if (SD_blockptr < 1) {
JonFreeman 0:23cc72b18e74 278 pc.printf ("Looks like card newly erased, SD_blockptr value of %d\r\n", SD_blockptr);
JonFreeman 0:23cc72b18e74 279 SD_blockptr = 0;
JonFreeman 0:23cc72b18e74 280 historic_distance = 0;
JonFreeman 0:23cc72b18e74 281 }
JonFreeman 0:23cc72b18e74 282 else {
JonFreeman 0:23cc72b18e74 283 SD_state = sd.ReadBlocks(SDBuffer, SD_BLOCKSIZE * (SD_blockptr - 1), SD_BLOCKSIZE, 1);
JonFreeman 0:23cc72b18e74 284 if (SD_state != SD_OK) {
JonFreeman 0:23cc72b18e74 285 pc.printf ("Error reading last block from SD block %d\r\n", SD_blockptr - 1);
JonFreeman 0:23cc72b18e74 286 return false;
JonFreeman 0:23cc72b18e74 287 }
JonFreeman 0:23cc72b18e74 288 for (int i = 0; i < (SD_BLOCKSIZE >> 2); i++)
JonFreeman 0:23cc72b18e74 289 pc.printf ("%lx\t", SDBuffer[i]);
JonFreeman 0:23cc72b18e74 290 historic_distance = SDBuffer[(SD_BLOCKSIZE >> 2) - 1];
JonFreeman 0:23cc72b18e74 291 pc.printf ("\r\nAbove, data read from last filled SD block %lld, using historic_distance = %lx\r\n", SD_blockptr - 1, historic_distance);
JonFreeman 0:23cc72b18e74 292 }
JonFreeman 0:23cc72b18e74 293 if (SD_blockptr > 2) {
JonFreeman 0:23cc72b18e74 294 for (int i = SD_blockptr - 2; i < SD_blockptr + 2; i++) {
JonFreeman 0:23cc72b18e74 295 pc.printf ("check_SD_block_clear (%d) ", i);
JonFreeman 0:23cc72b18e74 296 if (check_SD_block_clear(i))
JonFreeman 0:23cc72b18e74 297 pc.printf ("block %ld is CLEAR\r\n", i);
JonFreeman 0:23cc72b18e74 298 else
JonFreeman 0:23cc72b18e74 299 pc.printf ("block %ld is NOT clear\r\n", i);
JonFreeman 0:23cc72b18e74 300 if (sd_error()) {
JonFreeman 0:23cc72b18e74 301 pc.printf ("ERROR from check_SD_block_clear ()\r\n");
JonFreeman 0:23cc72b18e74 302 }
JonFreeman 0:23cc72b18e74 303 }
JonFreeman 0:23cc72b18e74 304 }
JonFreeman 0:23cc72b18e74 305 return true;
JonFreeman 0:23cc72b18e74 306 }
JonFreeman 0:23cc72b18e74 307
JonFreeman 0:23cc72b18e74 308
JonFreeman 0:23cc72b18e74 309
JonFreeman 0:23cc72b18e74 310
JonFreeman 0:23cc72b18e74 311
JonFreeman 0:23cc72b18e74 312
JonFreeman 0:23cc72b18e74 313
JonFreeman 0:23cc72b18e74 314
JonFreeman 0:23cc72b18e74 315 class speed_measurement // Interrupts at qtr sec cause read of Hall_pulse counters which are incremented by transitions of Hall inputs
JonFreeman 0:23cc72b18e74 316 {
JonFreeman 0:23cc72b18e74 317 static const int SPEED_AVE_PTS = 9; // AVE_PTS - points in moving average filters
JonFreeman 0:23cc72b18e74 318 int speed_maf_mem [(SPEED_AVE_PTS + 1) * 2][NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 319 latest_counter_read[NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 320 prev_counter_read[NUMBER_OF_MOTORS],
JonFreeman 0:23cc72b18e74 321 mafptr;
JonFreeman 0:23cc72b18e74 322 int raw_filtered () ; // sum of count for all motors
JonFreeman 0:23cc72b18e74 323
JonFreeman 0:23cc72b18e74 324 public:
JonFreeman 0:23cc72b18e74 325
JonFreeman 0:23cc72b18e74 326 speed_measurement () {
JonFreeman 0:23cc72b18e74 327 memset(speed_maf_mem, 0, sizeof(speed_maf_mem));
JonFreeman 0:23cc72b18e74 328 mafptr = 0;
JonFreeman 0:23cc72b18e74 329 memset (latest_counter_read, 0, sizeof(latest_counter_read));
JonFreeman 0:23cc72b18e74 330 memset (prev_counter_read, 0, sizeof(prev_counter_read));
JonFreeman 0:23cc72b18e74 331 } // constructor
JonFreeman 0:23cc72b18e74 332 int raw_filtered (int) ; // count for one motor
JonFreeman 0:23cc72b18e74 333 int RPM () ;
JonFreeman 0:23cc72b18e74 334 double MPH () ;
JonFreeman 0:23cc72b18e74 335 void qtr_sec_update () ;
JonFreeman 0:23cc72b18e74 336 uint32_t metres_travelled ();
JonFreeman 0:23cc72b18e74 337 uint32_t pulse_total ();
JonFreeman 0:23cc72b18e74 338 }
JonFreeman 0:23cc72b18e74 339 speed ;
JonFreeman 0:23cc72b18e74 340
JonFreeman 0:23cc72b18e74 341 int speed_measurement::raw_filtered () // sum of count for all motors
JonFreeman 0:23cc72b18e74 342 {
JonFreeman 0:23cc72b18e74 343 int result = 0, a, b;
JonFreeman 0:23cc72b18e74 344 for (b = 0; b < NUMBER_OF_MOTORS; b++) {
JonFreeman 0:23cc72b18e74 345 for (a = 0; a < SPEED_AVE_PTS; a++) {
JonFreeman 0:23cc72b18e74 346 result += speed_maf_mem[a][b];
JonFreeman 0:23cc72b18e74 347 }
JonFreeman 0:23cc72b18e74 348 }
JonFreeman 0:23cc72b18e74 349 return result;
JonFreeman 0:23cc72b18e74 350 }
JonFreeman 0:23cc72b18e74 351
JonFreeman 0:23cc72b18e74 352 int speed_measurement::raw_filtered (int motor) // count for one motor
JonFreeman 0:23cc72b18e74 353 {
JonFreeman 0:23cc72b18e74 354 int result = 0, a;
JonFreeman 0:23cc72b18e74 355 for (a = 0; a < SPEED_AVE_PTS; a++) {
JonFreeman 0:23cc72b18e74 356 result += speed_maf_mem[a][motor];
JonFreeman 0:23cc72b18e74 357 }
JonFreeman 0:23cc72b18e74 358 return result;
JonFreeman 0:23cc72b18e74 359 }
JonFreeman 0:23cc72b18e74 360
JonFreeman 0:23cc72b18e74 361 double speed_measurement::MPH ()
JonFreeman 0:23cc72b18e74 362 {
JonFreeman 0:23cc72b18e74 363 return rpm2mph * (double)RPM();
JonFreeman 0:23cc72b18e74 364 }
JonFreeman 0:23cc72b18e74 365
JonFreeman 0:23cc72b18e74 366 int speed_measurement::RPM ()
JonFreeman 0:23cc72b18e74 367 {
JonFreeman 0:23cc72b18e74 368 int rpm = raw_filtered ();
JonFreeman 0:23cc72b18e74 369 rpm *= 60 * 4; // 60 sec per min, 4 quarters per sec, result pulses per min
JonFreeman 0:23cc72b18e74 370 rpm /= (SPEED_AVE_PTS * NUMBER_OF_MOTORS * 8); // 8 transitions counted per rev
JonFreeman 0:23cc72b18e74 371 return rpm;
JonFreeman 0:23cc72b18e74 372 }
JonFreeman 0:23cc72b18e74 373
JonFreeman 0:23cc72b18e74 374 void speed_measurement::qtr_sec_update () // this to be called every quarter sec to read counters and update maf
JonFreeman 0:23cc72b18e74 375 {
JonFreeman 0:23cc72b18e74 376 mafptr++;
JonFreeman 0:23cc72b18e74 377 if (mafptr >= SPEED_AVE_PTS)
JonFreeman 0:23cc72b18e74 378 mafptr = 0;
JonFreeman 0:23cc72b18e74 379 for (int a = 0; a < NUMBER_OF_MOTORS; a++) {
JonFreeman 0:23cc72b18e74 380 prev_counter_read[a] = latest_counter_read[a];
JonFreeman 0:23cc72b18e74 381 latest_counter_read[a] = Hall_pulse[a];
JonFreeman 0:23cc72b18e74 382 speed_maf_mem[mafptr][a] = latest_counter_read[a] - prev_counter_read[a];
JonFreeman 0:23cc72b18e74 383 }
JonFreeman 0:23cc72b18e74 384 }
JonFreeman 0:23cc72b18e74 385
JonFreeman 0:23cc72b18e74 386 uint32_t speed_measurement::metres_travelled ()
JonFreeman 0:23cc72b18e74 387 {
JonFreeman 0:23cc72b18e74 388 return pulse_total() / (int)PULSES_PER_METRE;
JonFreeman 0:23cc72b18e74 389 }
JonFreeman 0:23cc72b18e74 390
JonFreeman 0:23cc72b18e74 391 uint32_t speed_measurement::pulse_total ()
JonFreeman 0:23cc72b18e74 392 {
JonFreeman 0:23cc72b18e74 393 return historic_distance + Hall_pulse[0] + Hall_pulse[1] + Hall_pulse[2] + Hall_pulse[3];
JonFreeman 0:23cc72b18e74 394 }
JonFreeman 0:23cc72b18e74 395
JonFreeman 0:23cc72b18e74 396 void set_V_limit (double p) // Sets max motor voltage
JonFreeman 0:23cc72b18e74 397 {
JonFreeman 0:23cc72b18e74 398 if (p < 0.0)
JonFreeman 0:23cc72b18e74 399 p = 0.0;
JonFreeman 0:23cc72b18e74 400 if (p > 1.0)
JonFreeman 0:23cc72b18e74 401 p = 1.0;
JonFreeman 0:23cc72b18e74 402 last_pwm = p;
JonFreeman 0:23cc72b18e74 403 p *= 0.95; // need limit, ffi see MCP1630 data
JonFreeman 0:23cc72b18e74 404 p = 1.0 - p; // because pwm is wrong way up
JonFreeman 0:23cc72b18e74 405 maxv.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output on pin D12 inverted motor pwm
JonFreeman 0:23cc72b18e74 406 }
JonFreeman 0:23cc72b18e74 407
JonFreeman 0:23cc72b18e74 408 void set_I_limit (double p) // Sets max motor current
JonFreeman 0:23cc72b18e74 409 {
JonFreeman 0:23cc72b18e74 410 int a;
JonFreeman 0:23cc72b18e74 411 if (p < 0.0)
JonFreeman 0:23cc72b18e74 412 p = 0.0;
JonFreeman 0:23cc72b18e74 413 if (p > 1.0)
JonFreeman 0:23cc72b18e74 414 p = 1.0;
JonFreeman 0:23cc72b18e74 415 a = (int)(p * MAX_PWM_TICKS);
JonFreeman 0:23cc72b18e74 416 if (a > MAX_PWM_TICKS)
JonFreeman 0:23cc72b18e74 417 a = MAX_PWM_TICKS;
JonFreeman 0:23cc72b18e74 418 if (a < 0)
JonFreeman 0:23cc72b18e74 419 a = 0;
JonFreeman 0:23cc72b18e74 420 maxi.pulsewidth_ticks (a); // PWM output on pin D12 inverted motor pwm
JonFreeman 0:23cc72b18e74 421 }
JonFreeman 0:23cc72b18e74 422
JonFreeman 0:23cc72b18e74 423 double read_ammeter ()
JonFreeman 0:23cc72b18e74 424 {
JonFreeman 0:23cc72b18e74 425 int a = 0;
JonFreeman 0:23cc72b18e74 426 for (int b = 0; b < MAF_PTS; b++)
JonFreeman 0:23cc72b18e74 427 a += I_maf[b];
JonFreeman 0:23cc72b18e74 428 a /= MAF_PTS;
JonFreeman 0:23cc72b18e74 429 double i = (double) a;
JonFreeman 0:23cc72b18e74 430 return (i * 95.0 / 32768.0) - 95.0 + 0.46; // fiddled to suit current module
JonFreeman 0:23cc72b18e74 431 }
JonFreeman 0:23cc72b18e74 432
JonFreeman 0:23cc72b18e74 433 double read_voltmeter ()
JonFreeman 0:23cc72b18e74 434 {
JonFreeman 0:23cc72b18e74 435 int a = 0;
JonFreeman 0:23cc72b18e74 436 for (int b = 0; b < MAF_PTS; b++)
JonFreeman 0:23cc72b18e74 437 a += V_maf[b];
JonFreeman 0:23cc72b18e74 438 a /= MAF_PTS;
JonFreeman 0:23cc72b18e74 439 double i = (double) a;
JonFreeman 0:23cc72b18e74 440 return (i / 617.75) + 0.3; // fiddled to suit current module
JonFreeman 0:23cc72b18e74 441 }
JonFreeman 0:23cc72b18e74 442
JonFreeman 0:23cc72b18e74 443 // Interrupt Service Routines
JonFreeman 0:23cc72b18e74 444
JonFreeman 0:23cc72b18e74 445 void ISR_mot1_hall_handler () // read motor position pulse signals from up to six motors
JonFreeman 0:23cc72b18e74 446 {
JonFreeman 0:23cc72b18e74 447 Hall_pulse[0]++;
JonFreeman 0:23cc72b18e74 448 }
JonFreeman 0:23cc72b18e74 449 void ISR_mot2_hall_handler ()
JonFreeman 0:23cc72b18e74 450 {
JonFreeman 0:23cc72b18e74 451 Hall_pulse[1]++;
JonFreeman 0:23cc72b18e74 452 }
JonFreeman 0:23cc72b18e74 453 void ISR_mot3_hall_handler ()
JonFreeman 0:23cc72b18e74 454 {
JonFreeman 0:23cc72b18e74 455 Hall_pulse[2]++;
JonFreeman 0:23cc72b18e74 456 }
JonFreeman 0:23cc72b18e74 457 void ISR_mot4_hall_handler ()
JonFreeman 0:23cc72b18e74 458 {
JonFreeman 0:23cc72b18e74 459 Hall_pulse[3]++;
JonFreeman 0:23cc72b18e74 460 }
JonFreeman 0:23cc72b18e74 461 /*void ISR_mot5_hall_handler ()
JonFreeman 0:23cc72b18e74 462 {
JonFreeman 0:23cc72b18e74 463 Hall_pulse[4]++;
JonFreeman 0:23cc72b18e74 464 }
JonFreeman 0:23cc72b18e74 465 void ISR_mot6_hall_handler ()
JonFreeman 0:23cc72b18e74 466 {
JonFreeman 0:23cc72b18e74 467 Hall_pulse[5]++;
JonFreeman 0:23cc72b18e74 468 }
JonFreeman 0:23cc72b18e74 469 */
JonFreeman 0:23cc72b18e74 470
JonFreeman 0:23cc72b18e74 471 void ISR_current_reader (void) // FIXED at 250us
JonFreeman 0:23cc72b18e74 472 {
JonFreeman 0:23cc72b18e74 473 trigger_current_read = true; // every 250us, i.e. 4kHz NOTE only sets trigger here, readings taken in main loop
JonFreeman 0:23cc72b18e74 474 }
JonFreeman 0:23cc72b18e74 475
JonFreeman 0:23cc72b18e74 476 void ISR_tick_32ms (void) //
JonFreeman 0:23cc72b18e74 477 {
JonFreeman 0:23cc72b18e74 478 trigger_32ms = true;
JonFreeman 0:23cc72b18e74 479 }
JonFreeman 0:23cc72b18e74 480 void ISR_tick_250ms (void)
JonFreeman 0:23cc72b18e74 481 {
JonFreeman 0:23cc72b18e74 482 qtrsec_trig = true;
JonFreeman 0:23cc72b18e74 483 }
JonFreeman 0:23cc72b18e74 484
JonFreeman 0:23cc72b18e74 485 // End of Interrupt Service Routines
JonFreeman 0:23cc72b18e74 486
JonFreeman 0:23cc72b18e74 487
JonFreeman 0:23cc72b18e74 488 bool inlist (struct ky_bd & a, int key)
JonFreeman 0:23cc72b18e74 489 {
JonFreeman 0:23cc72b18e74 490 int i = 0;
JonFreeman 0:23cc72b18e74 491 while (i < a.count) {
JonFreeman 0:23cc72b18e74 492 if (key == a.ky[i].keynum)
JonFreeman 0:23cc72b18e74 493 return true;
JonFreeman 0:23cc72b18e74 494 i++;
JonFreeman 0:23cc72b18e74 495 }
JonFreeman 0:23cc72b18e74 496 return false;
JonFreeman 0:23cc72b18e74 497 }
JonFreeman 0:23cc72b18e74 498
JonFreeman 0:23cc72b18e74 499
JonFreeman 0:23cc72b18e74 500 void stuff_to_do_every_250us () // Take readings of system voltage and current
JonFreeman 0:23cc72b18e74 501 {
JonFreeman 0:23cc72b18e74 502 if (!trigger_current_read)
JonFreeman 0:23cc72b18e74 503 return;
JonFreeman 0:23cc72b18e74 504 trigger_current_read = false;
JonFreeman 0:23cc72b18e74 505 I_maf[maf_ptr] = ht_amps_ain.read_u16();
JonFreeman 0:23cc72b18e74 506 V_maf[maf_ptr] = ht_volts_ain.read_u16();
JonFreeman 0:23cc72b18e74 507 maf_ptr++;
JonFreeman 0:23cc72b18e74 508 if (maf_ptr > MAF_PTS - 1)
JonFreeman 0:23cc72b18e74 509 maf_ptr = 0;
JonFreeman 0:23cc72b18e74 510 }
JonFreeman 0:23cc72b18e74 511 /* Feb 2017, re-thought use of FR and SG signals. Rename these FWD and REV. Truth table for actions required now : -
JonFreeman 0:23cc72b18e74 512 FWD(A5) REV(A4) PWM Action
JonFreeman 0:23cc72b18e74 513 0 0 0 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 514 0 0 1 'Handbrake' - energises motor to not move
JonFreeman 0:23cc72b18e74 515 0 1 0 Reverse0
JonFreeman 0:23cc72b18e74 516 0 1 1 Reverse1
JonFreeman 0:23cc72b18e74 517
JonFreeman 0:23cc72b18e74 518 1 0 0 Forward0
JonFreeman 0:23cc72b18e74 519 1 0 1 Forward1
JonFreeman 0:23cc72b18e74 520 1 1 0 Regen Braking
JonFreeman 0:23cc72b18e74 521 1 1 1 Regen Braking
JonFreeman 0:23cc72b18e74 522 */
JonFreeman 0:23cc72b18e74 523 void set_run_mode (int mode)
JonFreeman 0:23cc72b18e74 524 {
JonFreeman 0:23cc72b18e74 525 if (mode == HANDBRAKE_SLIPPING) slider.handbrake_slipping = true;
JonFreeman 0:23cc72b18e74 526 else slider.handbrake_slipping = false;
JonFreeman 0:23cc72b18e74 527 switch (mode) {
JonFreeman 0:23cc72b18e74 528 // STATES, INACTIVE, RUN, NEUTRAL_DRIFT, REGEN_BRAKE, PARK};
JonFreeman 0:23cc72b18e74 529 // case HANDBRAKE_SLIPPING:
JonFreeman 0:23cc72b18e74 530 // break;
JonFreeman 0:23cc72b18e74 531 case PARK: // PARKED new rom code IS now finished.
JonFreeman 0:23cc72b18e74 532 forward_pin = 0;
JonFreeman 0:23cc72b18e74 533 reverse_pin = 0;
JonFreeman 0:23cc72b18e74 534 slider.state = mode;
JonFreeman 0:23cc72b18e74 535 set_V_limit (0.075); // was 0.1
JonFreeman 0:23cc72b18e74 536 set_I_limit (slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 537 break;
JonFreeman 0:23cc72b18e74 538 case REGEN_BRAKE: // BRAKING, pwm affects degree
JonFreeman 0:23cc72b18e74 539 forward_pin = 1;
JonFreeman 0:23cc72b18e74 540 reverse_pin = 1;
JonFreeman 0:23cc72b18e74 541 slider.state = mode;
JonFreeman 0:23cc72b18e74 542 break;
JonFreeman 0:23cc72b18e74 543 case NEUTRAL_DRIFT:
JonFreeman 0:23cc72b18e74 544 slider.state = mode;
JonFreeman 0:23cc72b18e74 545 set_I_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch
JonFreeman 0:23cc72b18e74 546 set_V_limit (0.0); // added after first test runs, looking for cause of mechanical startup snatch
JonFreeman 0:23cc72b18e74 547 break;
JonFreeman 0:23cc72b18e74 548 case RUN:
JonFreeman 0:23cc72b18e74 549 if (slider.direction) {
JonFreeman 0:23cc72b18e74 550 forward_pin = 0;
JonFreeman 0:23cc72b18e74 551 reverse_pin = 1;
JonFreeman 0:23cc72b18e74 552 } else {
JonFreeman 0:23cc72b18e74 553 forward_pin = 1;
JonFreeman 0:23cc72b18e74 554 reverse_pin = 0;
JonFreeman 0:23cc72b18e74 555 }
JonFreeman 0:23cc72b18e74 556 slider.state = mode;
JonFreeman 0:23cc72b18e74 557 break;
JonFreeman 0:23cc72b18e74 558 default:
JonFreeman 0:23cc72b18e74 559 break;
JonFreeman 0:23cc72b18e74 560 }
JonFreeman 0:23cc72b18e74 561 }
JonFreeman 0:23cc72b18e74 562
JonFreeman 0:23cc72b18e74 563 void update_SD_card () { // Hall pulse total updated once per sec and saved in blocks of 128 to SD card
JonFreeman 0:23cc72b18e74 564 static int index = 0;
JonFreeman 0:23cc72b18e74 565 static uint32_t buff[(SD_BLOCKSIZE >> 2) + 2];
JonFreeman 0:23cc72b18e74 566 buff[index++] = speed.pulse_total(); // pulse_total for all time, add this to buffer to write to SD
JonFreeman 0:23cc72b18e74 567 if (index >= (SD_BLOCKSIZE >> 2)) {
JonFreeman 0:23cc72b18e74 568 pc.printf ("Writing new SD block %d ... ", SD_blockptr);
JonFreeman 0:23cc72b18e74 569 SD_state = sd.WriteBlocks(buff, SD_BLOCKSIZE * SD_blockptr, SD_BLOCKSIZE, 1);
JonFreeman 0:23cc72b18e74 570 SD_blockptr++;
JonFreeman 0:23cc72b18e74 571 if (SD_state == SD_OK)
JonFreeman 0:23cc72b18e74 572 pc.printf ("OK, distance %d\r\n", buff[index - 1] / (int)PULSES_PER_METRE);
JonFreeman 0:23cc72b18e74 573 else
JonFreeman 0:23cc72b18e74 574 pc.printf ("ERROR\r\n");
JonFreeman 0:23cc72b18e74 575 index = 0;
JonFreeman 0:23cc72b18e74 576 }
JonFreeman 0:23cc72b18e74 577 }
JonFreeman 0:23cc72b18e74 578
JonFreeman 0:23cc72b18e74 579 int main()
JonFreeman 0:23cc72b18e74 580 {
JonFreeman 0:23cc72b18e74 581 int c_5 = 0, seconds = 0, minutes = 0;
JonFreeman 0:23cc72b18e74 582 ky_bd kybd_a, kybd_b;
JonFreeman 0:23cc72b18e74 583 memset (&kybd_a, 0, sizeof(kybd_a));
JonFreeman 0:23cc72b18e74 584 memset (&kybd_b, 0, sizeof(kybd_b));
JonFreeman 0:23cc72b18e74 585
JonFreeman 0:23cc72b18e74 586 spareio_d8.mode (PullUp);
JonFreeman 0:23cc72b18e74 587 spareio_d9.mode (PullUp);
JonFreeman 0:23cc72b18e74 588 spareio_d10.mode(PullUp);
JonFreeman 0:23cc72b18e74 589
JonFreeman 0:23cc72b18e74 590 Ticker tick250us;
JonFreeman 0:23cc72b18e74 591 Ticker tick32ms;
JonFreeman 0:23cc72b18e74 592 Ticker tick250ms;
JonFreeman 0:23cc72b18e74 593
JonFreeman 0:23cc72b18e74 594 // Setup User Interrupt Vectors
JonFreeman 0:23cc72b18e74 595 mot1hall.fall (&ISR_mot1_hall_handler);
JonFreeman 0:23cc72b18e74 596 mot1hall.rise (&ISR_mot1_hall_handler);
JonFreeman 0:23cc72b18e74 597 mot2hall.fall (&ISR_mot2_hall_handler);
JonFreeman 0:23cc72b18e74 598 mot2hall.rise (&ISR_mot2_hall_handler);
JonFreeman 0:23cc72b18e74 599 mot3hall.fall (&ISR_mot3_hall_handler);
JonFreeman 0:23cc72b18e74 600 mot3hall.rise (&ISR_mot3_hall_handler);
JonFreeman 0:23cc72b18e74 601 mot4hall.fall (&ISR_mot4_hall_handler);
JonFreeman 0:23cc72b18e74 602 mot4hall.rise (&ISR_mot4_hall_handler);
JonFreeman 0:23cc72b18e74 603
JonFreeman 0:23cc72b18e74 604 tick250us.attach_us (&ISR_current_reader, 250); // set to longer time to test
JonFreeman 0:23cc72b18e74 605 tick32ms.attach_us (&ISR_tick_32ms, 32001);
JonFreeman 0:23cc72b18e74 606 tick250ms.attach_us (&ISR_tick_250ms, 250002);
JonFreeman 0:23cc72b18e74 607 pc.baud (9600);
JonFreeman 0:23cc72b18e74 608 GfetT1 = 0;
JonFreeman 0:23cc72b18e74 609 GfetT2 = 0; // two output bits for future use driving horns
JonFreeman 0:23cc72b18e74 610 if (f_r_switch)
JonFreeman 0:23cc72b18e74 611 slider.direction = FWD; // make decision from key switch position here
JonFreeman 0:23cc72b18e74 612 else
JonFreeman 0:23cc72b18e74 613 slider.direction = REV; // make decision from key switch position here
JonFreeman 0:23cc72b18e74 614
JonFreeman 0:23cc72b18e74 615 // 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 616 maxv.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
JonFreeman 0:23cc72b18e74 617 maxi.period_ticks (MAX_PWM_TICKS + 1);
JonFreeman 0:23cc72b18e74 618 set_I_limit (0.0);
JonFreeman 0:23cc72b18e74 619 set_V_limit (0.0);
JonFreeman 0:23cc72b18e74 620
JonFreeman 0:23cc72b18e74 621 pc.printf ("Jon's Touch Screen Loco 2017 sytem starting up %s\r\n", slider.direction ? "Forward":"Reverse");
JonFreeman 0:23cc72b18e74 622 uint8_t lcd_status = touch_screen.Init(lcd.GetXSize(), lcd.GetYSize());
JonFreeman 0:23cc72b18e74 623 if (lcd_status != TS_OK) {
JonFreeman 0:23cc72b18e74 624 lcd.Clear(LCD_COLOR_RED);
JonFreeman 0:23cc72b18e74 625 lcd.SetBackColor(LCD_COLOR_RED);
JonFreeman 0:23cc72b18e74 626 lcd.SetTextColor(LCD_COLOR_WHITE);
JonFreeman 0:23cc72b18e74 627 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT FAIL", CENTER_MODE);
JonFreeman 0:23cc72b18e74 628 wait (20);
JonFreeman 0:23cc72b18e74 629 } else {
JonFreeman 0:23cc72b18e74 630 lcd.Clear(LCD_COLOR_DARKBLUE);
JonFreeman 0:23cc72b18e74 631 lcd.SetBackColor(LCD_COLOR_GREEN);
JonFreeman 0:23cc72b18e74 632 lcd.SetTextColor(LCD_COLOR_WHITE);
JonFreeman 0:23cc72b18e74 633 lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN INIT OK", CENTER_MODE);
JonFreeman 0:23cc72b18e74 634 }
JonFreeman 0:23cc72b18e74 635
JonFreeman 0:23cc72b18e74 636 lcd.SetFont(&Font16);
JonFreeman 0:23cc72b18e74 637 lcd.Clear(LCD_COLOR_LIGHTGRAY);
JonFreeman 0:23cc72b18e74 638 setup_buttons(); // draws buttons
JonFreeman 0:23cc72b18e74 639
JonFreeman 0:23cc72b18e74 640 slider.oldpos = 0;
JonFreeman 0:23cc72b18e74 641 slider.loco_speed = 0.0;
JonFreeman 0:23cc72b18e74 642 slider.handbrake_effort = 0.1;
JonFreeman 0:23cc72b18e74 643 slider.position = MAX_POS - 2; // Low down in REGEN_BRAKE position - NOT to power-up in PARK
JonFreeman 0:23cc72b18e74 644 SliderGraphic (slider); // sets slider.state to value determined by slider.position
JonFreeman 0:23cc72b18e74 645 set_run_mode (REGEN_BRAKE); // sets slider.mode
JonFreeman 0:23cc72b18e74 646
JonFreeman 0:23cc72b18e74 647 lcd.SetBackColor(LCD_COLOR_DARKBLUE);
JonFreeman 0:23cc72b18e74 648
JonFreeman 0:23cc72b18e74 649 vm_set(); // Draw 3 analogue meter movements, speedo, voltmeter, ammeter
JonFreeman 0:23cc72b18e74 650
JonFreeman 0:23cc72b18e74 651 mainSDtest();
JonFreeman 0:23cc72b18e74 652
JonFreeman 0:23cc72b18e74 653 bool toggle32ms = false;
JonFreeman 0:23cc72b18e74 654 // Main loop
JonFreeman 0:23cc72b18e74 655 while(1) { //
JonFreeman 0:23cc72b18e74 656 struct ky_bd * present_kybd, * previous_kybd;
JonFreeman 0:23cc72b18e74 657 bool sliderpress = false;
JonFreeman 0:23cc72b18e74 658 command_line_interpreter () ; // Do any actions from command line via usb link
JonFreeman 0:23cc72b18e74 659 stuff_to_do_every_250us () ;
JonFreeman 0:23cc72b18e74 660
JonFreeman 0:23cc72b18e74 661 if (trigger_32ms == true) { // Stuff to do every 32 milli secs
JonFreeman 0:23cc72b18e74 662 trigger_32ms = false;
JonFreeman 0:23cc72b18e74 663 toggle32ms = !toggle32ms;
JonFreeman 0:23cc72b18e74 664 if (toggle32ms) {
JonFreeman 0:23cc72b18e74 665 present_kybd = &kybd_a;
JonFreeman 0:23cc72b18e74 666 previous_kybd = &kybd_b;
JonFreeman 0:23cc72b18e74 667 } else {
JonFreeman 0:23cc72b18e74 668 present_kybd = &kybd_b;
JonFreeman 0:23cc72b18e74 669 previous_kybd = &kybd_a;
JonFreeman 0:23cc72b18e74 670 }
JonFreeman 0:23cc72b18e74 671 read_keypresses (*present_kybd);
JonFreeman 0:23cc72b18e74 672 sliderpress = false;
JonFreeman 0:23cc72b18e74 673 slider.recalc_run = false;
JonFreeman 0:23cc72b18e74 674 int j = 0;
JonFreeman 0:23cc72b18e74 675 // if (present2->count > previous_kybd->count) pc.printf ("More presses\r\n");
JonFreeman 0:23cc72b18e74 676 // if (present2->count < previous_kybd->count) pc.printf ("Fewer presses\r\n");
JonFreeman 0:23cc72b18e74 677 if (present_kybd->count || previous_kybd->count) { // at least one key pressed this time or last time
JonFreeman 0:23cc72b18e74 678 int k;
JonFreeman 0:23cc72b18e74 679 double dbl;
JonFreeman 0:23cc72b18e74 680 // pc.printf ("Keys action may be required");
JonFreeman 0:23cc72b18e74 681 // if key in present and ! in previous, found new key press to handle
JonFreeman 0:23cc72b18e74 682 // if key ! in present and in previous, found new key release to handle
JonFreeman 0:23cc72b18e74 683 if (inlist(*present_kybd, SLIDER)) { // Finger is on slider, so Update slider graphic here
JonFreeman 0:23cc72b18e74 684 sliderpress = true;
JonFreeman 0:23cc72b18e74 685 k = present_kybd->slider_y; // get position of finger on slider
JonFreeman 0:23cc72b18e74 686 if (slider.state == RUN && k != slider.position) // Finger has moved within RUN range
JonFreeman 0:23cc72b18e74 687 slider.recalc_run = true;
JonFreeman 0:23cc72b18e74 688 if (slider.state == RUN && k >= NEUTRAL_VAL) // Finger has moved from RUN to BRAKE range
JonFreeman 0:23cc72b18e74 689 slider.position = k = NEUTRAL_VAL; // kill drive for rapid reaction to braking
JonFreeman 0:23cc72b18e74 690
JonFreeman 0:23cc72b18e74 691 else { // nice slow non-jerky glidey movement required
JonFreeman 0:23cc72b18e74 692 dbl = (double)(k - slider.position);
JonFreeman 0:23cc72b18e74 693 dbl /= 13.179;
JonFreeman 0:23cc72b18e74 694 if (dbl < 0.0)
JonFreeman 0:23cc72b18e74 695 dbl -= 1.0;
JonFreeman 0:23cc72b18e74 696 if (dbl > 0.0)
JonFreeman 0:23cc72b18e74 697 dbl += 1.0;
JonFreeman 0:23cc72b18e74 698 slider.position += (int)dbl;
JonFreeman 0:23cc72b18e74 699 }
JonFreeman 0:23cc72b18e74 700 SliderGraphic (slider); // sets slider.state to value determined by slider.position
JonFreeman 0:23cc72b18e74 701 set_run_mode (slider.state);
JonFreeman 0:23cc72b18e74 702 draw_button_hilight (SLIDER, LCD_COLOR_YELLOW) ;
JonFreeman 0:23cc72b18e74 703
JonFreeman 0:23cc72b18e74 704 if (slider.state == REGEN_BRAKE) {
JonFreeman 0:23cc72b18e74 705 double brake_effort = ((double)(slider.position - NEUTRAL_VAL)
JonFreeman 0:23cc72b18e74 706 / (double)(MAX_POS - NEUTRAL_VAL));
JonFreeman 0:23cc72b18e74 707 // brake_effort normalised to range 0.0 to 1.0
JonFreeman 0:23cc72b18e74 708 brake_effort *= 0.97; // upper limit to braking effort, observed effect before was quite fierce
JonFreeman 0:23cc72b18e74 709 pc.printf ("Brake effort %.2f\r\n", brake_effort);
JonFreeman 0:23cc72b18e74 710 /* set_pwm (brake_effort); */
JonFreeman 0:23cc72b18e74 711 set_V_limit (sqrt(brake_effort)); // sqrt gives more linear feel to control
JonFreeman 0:23cc72b18e74 712 set_I_limit (1.0);
JonFreeman 0:23cc72b18e74 713 }
JonFreeman 0:23cc72b18e74 714 } else { // pc.printf ("Slider not touched\r\n");
JonFreeman 0:23cc72b18e74 715 }
JonFreeman 0:23cc72b18e74 716
JonFreeman 0:23cc72b18e74 717 j = 0;
JonFreeman 0:23cc72b18e74 718 while (j < present_kybd->count) { // handle new key presses
JonFreeman 0:23cc72b18e74 719 k = present_kybd->ky[j++].keynum;
JonFreeman 0:23cc72b18e74 720 if (inlist(*present_kybd, k)) {
JonFreeman 0:23cc72b18e74 721 switch (k) { // Here for auto-repeat type key behaviour
JonFreeman 0:23cc72b18e74 722 case 21: // key is 'voltmeter'
JonFreeman 0:23cc72b18e74 723 // set_V_limit (last_pwm * 1.002 + 0.001);
JonFreeman 0:23cc72b18e74 724 break;
JonFreeman 0:23cc72b18e74 725 case 22: // key is 'ammeter'
JonFreeman 0:23cc72b18e74 726 // set_V_limit (last_pwm * 0.99);
JonFreeman 0:23cc72b18e74 727 break;
JonFreeman 0:23cc72b18e74 728 } // endof switch (k)
JonFreeman 0:23cc72b18e74 729 } // endof if (inlist(*present2, k)) {
JonFreeman 0:23cc72b18e74 730 if (inlist(*present_kybd, k) && !inlist(*previous_kybd, k)) {
JonFreeman 0:23cc72b18e74 731 pc.printf ("Handle Press %d\r\n", k);
JonFreeman 0:23cc72b18e74 732 draw_button_hilight (k, LCD_COLOR_YELLOW) ;
JonFreeman 0:23cc72b18e74 733 switch (k) { // Handle new touch screen button presses here - single action per press, not autorepeat
JonFreeman 0:23cc72b18e74 734 case SPEEDO_BUT: //
JonFreeman 0:23cc72b18e74 735 pc.printf ("Speedometer key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 736 break;
JonFreeman 0:23cc72b18e74 737 case VMETER_BUT: //
JonFreeman 0:23cc72b18e74 738 pc.printf ("Voltmeter key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 739 break;
JonFreeman 0:23cc72b18e74 740 case AMETER_BUT: //
JonFreeman 0:23cc72b18e74 741 pc.printf ("Ammeter key pressed %d\r\n", k);
JonFreeman 0:23cc72b18e74 742 break;
JonFreeman 0:23cc72b18e74 743 default:
JonFreeman 0:23cc72b18e74 744 pc.printf ("Unhandled keypress %d\r\n", k);
JonFreeman 0:23cc72b18e74 745 break;
JonFreeman 0:23cc72b18e74 746 } // endof switch (button)
JonFreeman 0:23cc72b18e74 747 }
JonFreeman 0:23cc72b18e74 748 } // endof while - handle new key presses
JonFreeman 0:23cc72b18e74 749 j = 0;
JonFreeman 0:23cc72b18e74 750 while (j < previous_kybd->count) { // handle new key releases
JonFreeman 0:23cc72b18e74 751 k = previous_kybd->ky[j++].keynum;
JonFreeman 0:23cc72b18e74 752 if (inlist(*previous_kybd, k) && !inlist(*present_kybd, k)) {
JonFreeman 0:23cc72b18e74 753 pc.printf ("Handle Release %d\r\n", k);
JonFreeman 0:23cc72b18e74 754 draw_button_hilight (k, LCD_COLOR_DARKBLUE) ;
JonFreeman 0:23cc72b18e74 755 }
JonFreeman 0:23cc72b18e74 756 } // endof while - handle new key releases
JonFreeman 0:23cc72b18e74 757 } // endof at least one key pressed this time or last time
JonFreeman 0:23cc72b18e74 758
JonFreeman 0:23cc72b18e74 759 if (sliderpress == false) { // need to glide dead-mans function towards neutral here
JonFreeman 0:23cc72b18e74 760 if (slider.position < NEUTRAL_VAL) {
JonFreeman 0:23cc72b18e74 761 slider.position += 1 + (NEUTRAL_VAL - slider.position) / DAMPER_DECAY;
JonFreeman 0:23cc72b18e74 762 SliderGraphic (slider);
JonFreeman 0:23cc72b18e74 763 slider.recalc_run = true;
JonFreeman 0:23cc72b18e74 764 }
JonFreeman 0:23cc72b18e74 765 }
JonFreeman 0:23cc72b18e74 766
JonFreeman 0:23cc72b18e74 767 if (slider.recalc_run) { // range of slider.position in RUN mode is min_pos_() to NEUTRAL_VAL - 1
JonFreeman 0:23cc72b18e74 768 slider.recalc_run = false; // All RUN power and pwm calcs done here
JonFreeman 0:23cc72b18e74 769 int b = slider.position;
JonFreeman 0:23cc72b18e74 770 double torque_req;
JonFreeman 0:23cc72b18e74 771 if (b > NEUTRAL_VAL)
JonFreeman 0:23cc72b18e74 772 b = NEUTRAL_VAL;
JonFreeman 0:23cc72b18e74 773 if (b < MIN_POS) // if finger position is above top of slider limit
JonFreeman 0:23cc72b18e74 774 b = MIN_POS;
JonFreeman 0:23cc72b18e74 775 b = NEUTRAL_VAL - b; // now got integer going positive for increasing power demand
JonFreeman 0:23cc72b18e74 776 torque_req = (double) b;
JonFreeman 0:23cc72b18e74 777 torque_req /= (NEUTRAL_VAL - MIN_POS); // in range 0.0 to 1.0
JonFreeman 0:23cc72b18e74 778 pc.printf ("torque_rec = %.3f, last_pwm = %.3f\r\n", torque_req, last_pwm);
JonFreeman 0:23cc72b18e74 779 set_I_limit (torque_req);
JonFreeman 0:23cc72b18e74 780 if (torque_req < 0.05)
JonFreeman 0:23cc72b18e74 781 set_V_limit (last_pwm / 2.0);
JonFreeman 0:23cc72b18e74 782 else {
JonFreeman 0:23cc72b18e74 783 if (last_pwm < 0.99)
JonFreeman 0:23cc72b18e74 784 set_V_limit (last_pwm + 0.05); // ramp voltage up rather than slam to max
JonFreeman 0:23cc72b18e74 785 }
JonFreeman 0:23cc72b18e74 786 }
JonFreeman 0:23cc72b18e74 787 } // endof doing 32ms stuff
JonFreeman 0:23cc72b18e74 788
JonFreeman 0:23cc72b18e74 789 if (qtrsec_trig == true) { // do every quarter second stuff here
JonFreeman 0:23cc72b18e74 790 qtrsec_trig = false;
JonFreeman 0:23cc72b18e74 791 speed.qtr_sec_update ();
JonFreeman 0:23cc72b18e74 792 double speedmph = speed.MPH(), amps = 0.0 - read_ammeter(), volts = read_voltmeter();
JonFreeman 0:23cc72b18e74 793 //static const double mph_2_mm_per_sec = 447.04; // exact
JonFreeman 0:23cc72b18e74 794 // double mm_travelled_in_qtrsec = speedmph * mph_2_mm_per_sec / 4.0;
JonFreeman 0:23cc72b18e74 795 slider.loco_speed = speedmph;
JonFreeman 0:23cc72b18e74 796 update_meters (speedmph, amps, volts) ;
JonFreeman 0:23cc72b18e74 797 // update_meters (7.5, amps, volts) ;
JonFreeman 0:23cc72b18e74 798 led_grn = !led_grn;
JonFreeman 0:23cc72b18e74 799 if (slider.state == PARK) {
JonFreeman 0:23cc72b18e74 800 if (speedmph > LOCO_HANDBRAKE_ESCAPE_SPEED / 4.0) {
JonFreeman 0:23cc72b18e74 801 slider.handbrake_effort *= 1.1;
JonFreeman 0:23cc72b18e74 802 if (slider.handbrake_effort > 0.55) slider.handbrake_effort = 0.55;
JonFreeman 0:23cc72b18e74 803 set_run_mode (PARK);
JonFreeman 0:23cc72b18e74 804 pc.printf ("Handbrake slipping, effort %.2f\r\n", slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 805 }
JonFreeman 0:23cc72b18e74 806 if (speedmph < 0.02) {
JonFreeman 0:23cc72b18e74 807 slider.handbrake_effort *= 0.9;
JonFreeman 0:23cc72b18e74 808 if (slider.handbrake_effort < 0.05) slider.handbrake_effort = 0.05;
JonFreeman 0:23cc72b18e74 809 set_run_mode (PARK);
JonFreeman 0:23cc72b18e74 810 pc.printf ("Handbrake not slipping, effort %.2f\r\n", slider.handbrake_effort);
JonFreeman 0:23cc72b18e74 811 }
JonFreeman 0:23cc72b18e74 812 }
JonFreeman 0:23cc72b18e74 813 c_5++;
JonFreeman 0:23cc72b18e74 814 // Can do stuff once per second here
JonFreeman 0:23cc72b18e74 815 if(c_5 > 3) {
JonFreeman 0:23cc72b18e74 816 c_5 = 0;
JonFreeman 0:23cc72b18e74 817 seconds++;
JonFreeman 0:23cc72b18e74 818 if (seconds > 59) {
JonFreeman 0:23cc72b18e74 819 seconds = 0;
JonFreeman 0:23cc72b18e74 820 minutes++;
JonFreeman 0:23cc72b18e74 821 // do once per minute stuff here
JonFreeman 0:23cc72b18e74 822 } // fall back into once per second
JonFreeman 0:23cc72b18e74 823 if(SD_state == SD_OK) {
JonFreeman 0:23cc72b18e74 824 uint32_t distance = speed.metres_travelled();
JonFreeman 0:23cc72b18e74 825 char dist[20];
JonFreeman 0:23cc72b18e74 826 sprintf (dist, "%05d m", distance);
JonFreeman 0:23cc72b18e74 827 displaytext (236, 226, 2, dist);
JonFreeman 0:23cc72b18e74 828 update_SD_card (); // Buffers data for SD card, writes when buffer filled
JonFreeman 0:23cc72b18e74 829 }
JonFreeman 0:23cc72b18e74 830 // calc_motor_amps( mva);
JonFreeman 0:23cc72b18e74 831 } // endof if(c_5 > 3
JonFreeman 0:23cc72b18e74 832 } // endof if (qtrsec_trig == true) {
JonFreeman 0:23cc72b18e74 833 } // endof while(1) main programme loop
JonFreeman 0:23cc72b18e74 834 } // endof int main() {
JonFreeman 0:23cc72b18e74 835
JonFreeman 0:23cc72b18e74 836