Dual Brushless Motor ESC, 10-62V, up to 50A per motor. Motors ganged or independent, multiple control input methods, cycle-by-cycle current limit, speed mode and torque mode control. Motors tiny to kW. Speed limit and other parameters easily set in firmware. As used in 'The Brushless Brutalist' locomotive - www.jons-workshop.com. See also Model Engineer magazine June-October 2019.
Dependencies: mbed BufferedSerial Servo PCT2075 FastPWM
Update 17th August 2020 Radio control inputs completed
Revision 16:d1e4b9ad3b8b, committed 2020-06-09
- Comitter:
- JonFreeman
- Date:
- Tue Jun 09 09:20:19 2020 +0000
- Parent:
- 15:2591e2008323
- Child:
- 17:cc9b854295d6
- Commit message:
- About to tidy i2c stuff
Changed in this revision
--- a/24LC64_eeprom.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/24LC64_eeprom.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -6,39 +6,50 @@ // Code for 24LC64 8k x 8 bit eeprom // Code based on earlier work using memory FM24W256, also at i2c address 0xa0; -const int addr_rd = 0xa1; // set bit 0 for read, clear bit 0 for write 24LC64 -const int addr_wr = 0xa0; // set bit 0 for read, clear bit 0 for write 24LC64 -const int ACK = 1; - -struct optpar { - int min, max, def; // min, max, default - const char * t; // description +const int addr_rd = 0xa1; // set bit 0 for read, clear bit 0 for write 24LC64 +const int addr_wr = 0xa0; // set bit 0 for read, clear bit 0 for write 24LC64 +const int ACK = 1; +/*struct optpar { + int min, max, de_fault; // min, max, default + const char * txt; // description } ; -struct optpar option_list2[] = { - {0, 1, 1, "MotorA direction 0 or 1"}, - {0, 1, 0, "MotorB direction 0 or 1"}, - {4, 8, 8, "MotorA poles 4 or 6 or 8"}, - {4, 8, 8, "MotorB poles 4 or 6 or 8"}, - {1, 4, 1, "MotorA 0R05 ohm current shunt resistors 1 to 4"}, - {1, 4, 1, "MotorB 0R05 ohm current shunt resistors 1 to 4"}, - {0, 1, 0, "Servo1 0 = Not used, 1= Output"}, - {0, 1, 0, "Servo2 0 = Not used, 1= Output"}, - {0, 1, 0, "RC Input1 0 = Not used, 1= Output"}, - {0, 1, 0, "RC Input2 0 = Not used, 1= Output"}, - {2, 5, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RC Input1, 5= RC Input2"}, - {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // defaults to '0' before eerom setup for first time - {10, 250, 60, "Top speed MPH * 10 range 1.0 to 25.0"}, // New Jan 2019 TOP_SPEED - {50, 253, 98, "Wheel diameter mm"}, // New 01/06/2018 - {10, 253, 27, "Motor pinion"}, // New 01/06/2018 - {50, 253, 85, "Wheel gear"}, // New 01/06/2018 - {0, 100, 0, "Future 1"}, - {0, 100, 0, "Future 2"}, - {0, 100, 0, "Future 3"}, - {0, 100, 0, "Future 4"}, - {0, 100, 0, "Future 5"}, +*/ +struct optpar option_list[] = { + {0, 1, 1, "MotorA direction [0 or 1]"}, // MOTADIR + {0, 1, 0, "MotorB direction [0 or 1]"}, // MOTBDIR + {4, 8, 8, "MotorA poles 4 or 6 or 8"}, // MOTAPOLES + {4, 8, 8, "MotorB poles 4 or 6 or 8"}, // MOTBPOLES + {1, 4, 1, "MotorA 0R05 shunt Rs 1 to 4"}, // ISHUNTB + {1, 4, 1, "MotorB 0R05 shunt Rs 1 to 4"}, // ISHUNTB + {10, 50, 20, "POT_REGBRAKE_RANGE percentage 10 to 50"}, // POT_REGBRAKE_RANGE + {0, 1, 0, "Servo1 out 0 = Disabled, 1= Output enabled"}, // SVO1 + {0, 1, 0, "Servo2 out 0 = Disabled, 1= Output enabled"}, // SVO2 + {0, 2, 0, "RC Input1 0 = Not used, 1= Uni_dir, 2= Bi_dir"}, // RCIN1 + {0, 2, 0, "RC Input2 0 = Not used, 1= Uni_dir, 2= Bi_dir"}, // RCIN2 + {2, 6, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RCIn1, 5= RCIn2, 6=RCin_both"}, // COMM_SRC + {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // BOARD_ID defaults to '0' before eerom setup for first time + {10, 250, 60, "Top speed MPH, range 1.0 to 25.0"}, // New Jan 2019 TOP_SPEED + {50, 253, 98, "Wheel diameter mm"}, // New 01/06/2018 + {10, 253, 27, "Motor pinion"}, // New 01/06/2018 + {50, 253, 85, "Wheel gear"}, // New 01/06/2018 ** NOTE this and above two used to calculate RPM to MPH ** + {0, 255, 12, "RC in 1 trim"}, // New Dec 2019 RCI1_TRIM read as 2's complement (-128 to +127), user enters -128 to +127 + {0, 255, 12, "RC in 2 trim"}, // New Dec 2019 RCI2_TRIM read as 2's complement (-128 to +127), user enters -128 to +127 + {10, 50, 20, "RCIN_REGBRAKE_RANGE stick range percent 10 to 50"}, // RCIN_REGBRAKE_RANGE + {10, 90, 50, "RCIN_STICK_ATTACK rate percent 10 to 90"}, // RCIN_STICK_ATTACK + {0, 1, 0, "RCIN1 direction swap 0 normal, 1 reverse"}, // RCIN1REVERSE + {0, 1, 0, "RCIN2 direction swap 0 normal, 1 reverse"}, // RCIN2REVERSE + {11, 63, 48, "Nominal system voltage, used to calculate current scaling"}, // NOM_SYSTEM_VOLTS + {5, 91, 90, "Brake Effectiveness percentage"}, + {1, 5, 1, "Baud rate, [1=9k6, 2=19k2, 3=38k4, 4=78k6, 5=115k2] = "}, // BAUD 1=9k6, 2=19k2, 3=38k4, 4=78k6, 5=115k2 + {0, 100, 0, "Future 11"}, + {0, 100, 0, "Future 12"}, + {0, 100, 0, "Future 13"}, + {0, 100, 0, "Future 14"}, + {0, 100, 0, "Future 15"}, + {0, 100, 0, "Future 16"}, } ; -const int numof_eeprom_options2 = sizeof(option_list2) / sizeof (struct optpar); +const int numof_eeprom_options = sizeof(option_list) / sizeof (struct optpar); @@ -50,8 +61,9 @@ uint32_t i2c_device_count; uint32_t i2c_device_list[12]; // max 12 i2c devices char settings [36]; - bool rd_24LC64 (int start_addr, char * dest, int length) ; - bool wr_24LC64 (int start_addr, char * dest, int length) ; + double rpm2mphdbl, user_brake_rangedbl, Vnomdbl; + bool rd_24LC64 (uint32_t start_addr, char * dest, uint32_t length) ; + bool wr_24LC64 (uint32_t start_addr, char * dest, uint32_t length) ; bool set_24LC64_internal_address (int start_addr) ; bool ack_poll () ; public: @@ -60,13 +72,224 @@ char rd (uint32_t) ; // Read one setup char value from private buffer 'settings' bool wr (char, uint32_t) ; // Write one setup char value to private buffer 'settings' bool save () ; // Write 'settings' buffer to EEPROM + bool set_defaults (); // Put default settings into EEPROM and local buffer uint32_t errs () ; // Return errors + const char * t (uint32_t); + int min (uint32_t) ; + int max (uint32_t) ; + int def (uint32_t) ; + bool in_range (char val, uint32_t p) ; + void edit (double * dbl, uint32_t numof_dbls) ; + double user_brake_range () ; + double top_speed () ; + double rpm2mph () ; + double rpm2mph (double) ; } ; */ +uint32_t eeprom_settings::baud () { + const uint32_t brates[] = {0, 9600, 19200, 38400, 76800, 11520}; + return brates[settings[BAUD]]; +} + +double eeprom_settings::top_speed () { + return top_speeddbl; +} + +double eeprom_settings::brake_effectiveness () { + return brake_eff; +} + +double eeprom_settings::user_brake_range () { + return user_brake_rangedbl; +} + +double eeprom_settings::Vnom () { + return Vnomdbl; +} + +double eeprom_settings::rpm2mph () { + return rpm2mphdbl; +} + +double eeprom_settings::rpm2mph (double rpm) { + return rpm2mphdbl * rpm; +} + +void eeprom_settings::edit (double * dbl, uint32_t numof_dbls) { +extern void set_RCIN_offsets () ; + const char* labs[3] = {"Disabled","Uni_directional","Bi_directional"}; + char temps[MAX_CLI_PARAMS+1]; // max num of parameters entered from pc terminal + uint32_t i; + double user_scratch; + double topspeed; // New Jan 2019 - set max loco speed + pc.printf ("\r\nus - User Settings data in EEPROM\r\nSyntax 'us' with no parameters lists current state.\r\n"); + if (numof_dbls > 0) { // If more than 0 parameters supplied + if (numof_dbls > MAX_CLI_PARAMS) + numof_dbls = MAX_CLI_PARAMS; + for (i = 0; i < numof_dbls; i++) + temps[i] = (char)dbl[i]; // recast doubles to char + while (MAX_CLI_PARAMS > i) + temps[i++] = 0; + i = temps[0]; + switch (i) { // First number read from user response after "mode" + case 11: // MotorA_dir [0 or 1], MotorB_dir [0 or 1] + wr(temps[1], MOTADIR); + wr(temps[2], MOTBDIR); + break; + case 12: // MotorA_poles [4,6,8], MotorB_poles [4,6,8] + if (temps[1] == 4 || temps[1] == 6 || temps[1] == 8) + wr(temps[1], MOTAPOLES); + if (temps[2] == 4 || temps[2] == 6 || temps[2] == 8) + wr(temps[2], MOTBPOLES); + break; + case 13: // MotorA_ current sense resistors [1 to 4], MotorB_ current sense resistors [1 to 4] + wr(temps[1], ISHUNTA); // Corrected since published + wr(temps[2], ISHUNTB); + break; + case 14: + wr(temps[1], BRAKE_EFFECTIVENESS); + break; + case 15: + pc.printf ("POT_REGBRAKE_RANGE value entered = %d\r\n", temps[1]); + if (!in_range(temps[1], POT_REGBRAKE_RANGE)) + temps[1] = def(POT_REGBRAKE_RANGE); + wr (temps[1], POT_REGBRAKE_RANGE); + break; + case 16: // 2 Servo1 [0 or 1], Servo2 [0 or 1] + wr(temps[1], SVO1); + wr(temps[2], SVO2); + break; + case 17: // 3 RCIn1 [0 or 1], RCIn2 [0 or 1] + wr(temps[1], RCIN1); + wr(temps[2], RCIN2); + break; + case 18: + wr(temps[1], RCIN1REVERSE); + break; + case 19: + wr(temps[1], RCIN2REVERSE); + break; +/* case 33: // Not shown in menu, just to test stuff searching for strtod bug, to be deleted later + pc.printf ("Testing to fix strtod bug, "); + i = 0; + while (numof_dbls--) + pc.printf ("%.3f, ", dbl[i++]); + pc.printf ("TheEnd\r\n"); + break;*/ + case 21: // 3 RCIn1 trim [-128 to +127] + case 22: // 3 RCIn2 trim [-128 to +127] + user_scratch = dbl[1]; + if (user_scratch > 127.0) user_scratch = 127.0; + if (user_scratch < -128.0) user_scratch = -128.0; + wr (((signed char) user_scratch), i == 21 ? RCI1_TRIM : RCI2_TRIM); + set_RCIN_offsets () ; + break; + case 23: // RCIN_REGBRAKE_RANGE + wr (temps[1], RCIN_REGBRAKE_RANGE); + break; + case 24: // RCIN_STICK_ATTACK + wr (temps[1], RCIN_STICK_ATTACK); + break; + case 25: // 4 Board ID '0' to '9' + if (temps[1] <= 9) // pointless to compare unsigned integer with zero + wr('0' | temps[1], BOARD_ID); + break; + case 26: // TOP_SPEED + topspeed = dbl[1]; + if (topspeed > 25.0) topspeed = 25.0; + if (topspeed < 1.0) topspeed = 1.0; + wr((char)(topspeed * 10.0), TOP_SPEED); + break; + case 27: // 5 Wheel dia mm, Motor pinion teeth, Wheel gear teeth + wr(temps[1], WHEELDIA); + wr(temps[2], MOTPIN); + wr(temps[3], WHEELGEAR); + break; + case 28: // {2, 5, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RC Input1, 5= RC Input2, 6=RC1+2 Robot"}, + if (temps[1] > 1 && temps[1] <= 6) + wr(temps[1], COMM_SRC); + break; + case 29: // Nominal System Voltage + wr (temps[1], NOM_SYSTEM_VOLTS); + break; + case 83: // set to defaults + set_defaults (); + break; + case 30: // BAUD + wr (temps[1], BAUD); + break; +/* case 9: // 9 Save settings + save (); + pc.printf ("Saving settings to EEPROM\r\n"); + break;*/ + default: + pc.printf ("Not found - user setting %d\r\n", i); + i = 0; + break; + } // endof switch + if (i) { + save (); + pc.printf ("Saving settings to EEPROM\r\n"); + } + } // endof // If more than 0 parameters supplied + else { // command was just "mode" on its own + pc.printf ("No Changes\r\n"); + } + pc.printf ("us 11\t%s = %d, %s = %d\r\n", t(MOTADIR), settings[MOTADIR], t(MOTBDIR), settings[MOTBDIR]); + pc.printf ("us 12\t%s = %d, %s = %d\r\n", t(MOTAPOLES), settings[MOTAPOLES], t(MOTBPOLES), settings[MOTBPOLES]); + pc.printf ("us 13\tNumof motor current shunt resistors [%d to %d], MotorA = %d, MotorB = %d\r\n", min(ISHUNTA), max(ISHUNTA), settings[ISHUNTA], settings[ISHUNTB]); + pc.printf ("us 14\t%s [min %d, max %d] = %d\r\n", t(BRAKE_EFFECTIVENESS), min(BRAKE_EFFECTIVENESS), max(BRAKE_EFFECTIVENESS), settings[BRAKE_EFFECTIVENESS]); + pc.printf ("us 15\t%s[%d to %d] = %d\r\n", t(POT_REGBRAKE_RANGE), min(POT_REGBRAKE_RANGE), max(POT_REGBRAKE_RANGE), settings[POT_REGBRAKE_RANGE]); + pc.printf ("us 16\tServo1 [0 or 1] = %d %s, Servo2 [0 or 1] = %d %s\r\n", settings[SVO1], settings[SVO1] == 0 ? "Disabled":"Enabled", settings[SVO2], settings[SVO2] == 0 ? "Disabled":"Enabled"); + pc.printf ("us 17\tRCIn1 [0 disable, 1 Uni_dir, 2 Bi_dir] = %d, %s\r\n\tRCIn2 [0 disable, 1 Uni_dir, 2 Bi_dir] = %d, %s\r\n", settings[RCIN1], labs[settings[RCIN1]], settings[RCIN2], labs[rd(RCIN2)]); + pc.printf ("us 18\t%s RCIN1 = %d, %s\r\n", t(RCIN1REVERSE), settings[RCIN1REVERSE], settings[RCIN1REVERSE] == 0 ? "NORMAL":"REVERSE"); + pc.printf ("us 19\t%s RCIN2 = %d, %s\r\n", t(RCIN2REVERSE), settings[RCIN2REVERSE], settings[RCIN2REVERSE] == 0 ? "NORMAL":"REVERSE"); + pc.printf ("us 21\tRCIn1 two's comp trim, [-128 to +127] = %d\r\n", (signed char) settings[RCI1_TRIM]); + pc.printf ("us 22\tRCIn2 two's comp trim, [-128 to +127] = %d\r\n", (signed char) settings[RCI2_TRIM]); + pc.printf ("us 23\tRCIn Regen braking uses this pcntage of movement range, [%d to %d] = %d\r\n",min(RCIN_REGBRAKE_RANGE), max(RCIN_REGBRAKE_RANGE), settings[RCIN_REGBRAKE_RANGE]); + pc.printf ("us 24\tRCIn Stick move Attack rate, [%d to %d] = %d\r\n",min(RCIN_STICK_ATTACK), max(RCIN_STICK_ATTACK), settings[RCIN_STICK_ATTACK]); + pc.printf ("us 25\tBoard ID ['0' to '9'] = '%c'\r\n", settings[BOARD_ID]); + pc.printf ("us 26\t%s = %.1f\r\n", t(TOP_SPEED), double(settings[TOP_SPEED]) / 10.0); +// WHEELDIA, MOTPIN, WHEELGEAR, used in converting RPM to MPH + pc.printf ("us 27\t%s = %d, %s = %d, %s = %d\r\n", t(WHEELDIA), settings[WHEELDIA], t(MOTPIN), settings[MOTPIN], t(WHEELGEAR), settings[WHEELGEAR]); + pc.printf ("us 28\tCommand Src [%d] - 2=COM2 (Touch Screen), 3=Pot, 4=RC In1, 5=RC In2, 6=RC1+2\r\n", settings[COMM_SRC]); + pc.printf ("us 29\t%s = %d\r\n", t(NOM_SYSTEM_VOLTS), settings[NOM_SYSTEM_VOLTS]); + pc.printf ("us 30\t%s %d\r\n", t(BAUD), settings[BAUD]); + pc.printf ("us 83\tSet to defaults\r\n"); +// pc.printf ("us 9\tSave settings\r\r\n"); +} + +bool eeprom_settings::in_range (char val, uint32_t p) { + if ((val >= option_list[p].min) && (val <= option_list[p].max)) + return true; + return false; +} +uint32_t eeprom_settings::min (uint32_t i) { + if (i >= numof_eeprom_options) + i = numof_eeprom_options - 1; + return option_list[i].min; +} +uint32_t eeprom_settings::max (uint32_t i) { + if (i >= numof_eeprom_options) + i = numof_eeprom_options - 1; + return option_list[i].max; +} +uint32_t eeprom_settings::def (uint32_t i) { + if (i >= numof_eeprom_options) + i = numof_eeprom_options - 1; + return option_list[i].de_fault; +} + +const char * eeprom_settings::t (uint32_t i) { + if (i >= numof_eeprom_options) + i = numof_eeprom_options - 1; + return option_list[i].txt; +} + bool eeprom_settings::set_defaults () { // Put default settings into EEPROM and local buffer - for (int i = 0; i < numof_eeprom_options2; i++) - settings[i] = option_list2[i].def; // Load defaults and 'Save Settings' + for (int i = 0; i < numof_eeprom_options; i++) + settings[i] = option_list[i].de_fault; // Load defaults and 'Save Settings' return save (); } @@ -88,19 +311,20 @@ errors = i2c_device_count = 0; for (int i = 0; i < 36; i++) settings[i] = 0; - for (int i = 0; i < 12; i++) + for (int i = 0; i < MAX_I2C_DEVICES; i++) i2c_device_list[i] = 0; i2c.frequency(400000); // Speed 400000 max. -// int last_found = 0, q; // Note address bits 3-1 to match addr pins on device int q; for (int i = 0; i < 255; i += 2) { // Search for devices at all possible i2c addresses i2c.start(); q = i2c.write(i); // may return error code 2 when no start issued switch (q) { case ACK: - pc.printf ("I2C device found at 0x%x\r\n", i); -// last_found = i; i2c_device_list[i2c_device_count++] = i; + if (i2c_device_count >= MAX_I2C_DEVICES) { + i = 300; // break out + pc.printf ("Too many i2c devices %d\r\n", i2c_device_count); + } case 2: // Device not seen at this address break; default: @@ -110,7 +334,6 @@ } } i2c.stop(); -// if (errors || last_found != 0xa0) { if (errors || !do_we_have_i2c(0xa0)) { pc.printf ("Error - EEPROM not seen %d\r\n", errors); errors |= 0xa0; @@ -120,25 +343,39 @@ errors = 0; if (!rd_24LC64 (0, settings, 32)) ESC_Error.set (FAULT_EEPROM, 2); // Set FAULT_EEPROM bit 1 if 24LC64 problem - for (int i = 0; i < numof_eeprom_options2; i++) { - if ((settings[i] < option_list2[i].min) || (settings[i] > option_list2[i].max)) { - pc.printf ("EEROM error with %s\r\n", option_list2[i].t); + for (int i = 0; i < numof_eeprom_options; i++) { + if ((settings[i] < option_list[i].min) || (settings[i] > option_list[i].max)) { + pc.printf ("EEROM error with %s\r\n", option_list[i].txt); + settings[i] = option_list[i].de_fault; // Load default for faulty entry errors++; } } } ESC_Error.set (FAULT_EEPROM, errors); // sets nothing if 0 - if (errors > 1) { - pc.printf ("Bad settings found at startup. Restoring defaults\r\n"); - for (int i = 0; i < numof_eeprom_options2; i++) - settings[i] = option_list2[i].def; // Load defaults and 'Save Settings' +// if (errors > 1) { ??why > 1 ? + if (errors > 0) { +// pc.printf ("Bad settings found at startup. Restoring defaults\r\n"); +// for (int i = 0; i < numof_eeprom_options2; i++) +// settings[i] = option_list[i].de_fault; // Load defaults and 'Save Settings' if (!wr_24LC64 (0, settings, 32)) // Save settings - pc.printf ("Error saving EEPROM in mode19\r\n"); + pc.printf ("Error saving EEPROM in user_settings19\r\n"); } - else // 0 or 1 error max found - pc.printf ("At startup, settings errors = %d\r\n", errors); + rpm2mphdbl = 60.0 // to Motor Revs per hour; + * ((double)settings[MOTPIN] / (double)settings[WHEELGEAR]) // Wheel revs per hour + * PI * ((double)settings[WHEELDIA] / 1000.0) // metres per hour + * 39.37 / (1760.0 * 36.0); // miles per hour + update_dbls (); +// else // 0 or 1 error max found +// pc.printf ("At startup, settings errors = %d\r\n", errors); } // endof constructor +void eeprom_settings::update_dbls () { + user_brake_rangedbl = (double)settings[POT_REGBRAKE_RANGE] / 100.0; + Vnomdbl = (double)settings[NOM_SYSTEM_VOLTS]; + brake_eff = (double)settings[BRAKE_EFFECTIVENESS] / 100.0; + top_speeddbl = (double)settings[TOP_SPEED] / 10.0; +} + bool eeprom_settings::ack_poll () { // wait short while for any previous memory operation to complete const int poll_tries = 40; int poll_count = 0; @@ -171,7 +408,7 @@ return true; } -bool eeprom_settings::rd_24LC64 (int start_addr, char * dest, int length) { +bool eeprom_settings::rd_24LC64 (uint32_t start_addr, char * dest, uint32_t length) { int acknak = ACK; if(length < 1) return false; @@ -193,7 +430,7 @@ return true; } -bool eeprom_settings::wr_24LC64 (int start_addr, char * source, int length) { +bool eeprom_settings::wr_24LC64 (uint32_t start_addr, char * source, uint32_t length) { int err = 0; if(length < 1 || length > 32) { pc.printf ("Length out of range %d in wr_24LC64\r\n", length); @@ -217,20 +454,33 @@ char eeprom_settings::rd (uint32_t i) { // Read one setup char value from private buffer 'settings' if (i > 31) { - pc.printf ("ERROR Attempt to read setting %d\r\n"); + pc.printf ("ERROR Attempt to read setting %d\r\n", i); return 0; } return settings[i]; } -bool eeprom_settings::wr (char c, uint32_t i) { // Read one setup char value from private buffer 'settings' - if (i > 31) +/* +bool eeprom_settings::in_range (char val, uint32_t p) { + if ((val >= option_list[p].min) && (val <= option_list[p].max)) + return true; + return false; +} +*/ +bool eeprom_settings::wr (char val, uint32_t p) { // Write one setup char value to private buffer 'settings' + if (p > 31) return false; - settings[i] = c; - return true; + if ((val >= min(p)) && (val <= max(p))) { + settings[p] = val; + return true; + } + settings[p] = def(p); +// pc.printf ("Wrong in wr, %s\r\n", t(p)); + return false; } bool eeprom_settings::save () { // Write 'settings' buffer to EEPROM + update_dbls (); return wr_24LC64 (0, settings, 32); }
--- a/F401RE.h Sat Nov 30 18:40:30 2019 +0000 +++ b/F401RE.h Tue Jun 09 09:20:19 2020 +0000 @@ -92,7 +92,7 @@ // Pin 14 Port_A AUL // Pin 15 Port_A AUH // Pins 16, 17 BufferedSerial pc -BufferedSerial pc (PA_2, PA_3, 2048, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead +BufferedSerial pc (PA_2, PA_3, 5000, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead // Pin 18 VSS NET GND // Pin 19 VDD NET +3V3 // Pin 20 Port_A AWL
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PCT2075.lib Tue Jun 09 09:20:19 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/okano/code/PCT2075/#0cdfb3ea5969
--- a/Radio_Control_In.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/Radio_Control_In.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -1,6 +1,7 @@ #include "mbed.h" #include "BufferedSerial.h" #include "Radio_Control_In.h" +#include "STM3_ESC.h" /**class RControl_In Jon Freeman Jan 2019 @@ -9,6 +10,7 @@ Checks repetition rate in range 5-25ms */ extern BufferedSerial pc; +//extern eeprom_settings user_settings ; // RControl_In::RControl_In () { // Default Constructor // pulse_width_us = period_us = pulse_count = 0; @@ -39,29 +41,103 @@ return period_us; } -void RControl_In::reset () -{ - pulse_width_us = period_us = pulse_count = 0; - t.reset (); -} - bool RControl_In::validate_rx () { // Tests for pulse width and repetition rates being believable return !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200)); } -double RControl_In::normalised () { - if (!validate_rx()) { - pc.printf ("Invalid RCin\r\n"); - return lost_chan_return_value; // ** WHAT TO RETURN WHEN RC NOT ACTIVE ? ** This is now user settable - } // Defaults to returning 0.0, user should call set_lost_chan_value (value) to alter - double norm = (double) (pulse_width_us - 1000); // left with -200 to + 1200 allowing for some margin - norm /= 1000.0; - if (norm > 1.0) - norm = 1.0; - if (norm < 0.0) - norm = 0.0; - return norm; +bool RControl_In::energise (struct RC_stick_info & stick, struct brushless_motor & motor) { // December 2019 + if (stick.active) { + if (stick.zone == ZONE_DRIVE) { + motor.set_mode (stick.stick_implied_motor_direction == 1 ? MOTOR_FORWARD : MOTOR_REVERSE); + motor.set_V_limit (stick.drive_effort); + motor.set_I_limit (stick.drive_effort); // This could be 1.0, or other options + } + if (stick.zone == ZONE_BRAKE) { + motor.brake (stick.brake_effort); + } + } + return stick.active; +} + +bool RControl_In::read (class RC_stick_info & stick) { // December 2019 + double dtmp; + uint32_t old_zone = stick.zone; + stick.chan_mode = get_chanmode(); // 0 disabled, 1 uni-dir, or 2 bi-dir + stick.active = validate_rx(); // True if RC Rx delivering believable pulse duration and timing + if (stick.active && (stick.chan_mode < 1 || stick.chan_mode > 2)) { // Should signal an error here + stick.active = false; + } + if (stick.active) { + stick.raw = (double) (pulse_width_us - 1000); // Read pulse width from Rx, left with -200.0 to + 1200.0 allowing for some margin + stick.raw /= 1000.0; // pulse width varies between typ 1000 to 2000 micro seconds + stick.raw += range_offset; // range now normalised to 0.0 <= raw <= 1.0 + if (stick.raw > 1.0) stick.raw = 1.0; + if (stick.raw < 0.0) stick.raw = 0.0; // clipped to strict limits 0.0 and 1.0 + if (stick_sense != 0) + stick.raw = 1.0 - stick.raw; // user setting allows for stick sense reversal + stick.deflection = stick.raw; + stick.stick_implied_motor_direction = +1; // -1 Reverse, 0 Stopped, +1 Forward + if (stick.chan_mode == 2) { // Bi-directional centre zero stick mode selected by user + stick.deflection = (stick.raw * 2.0) - 1.0; // range here -1.0 <= deflection <= +1.0 + if (stick.deflection < 0.0) { + stick.deflection = 0.0 - stick.deflection; // range inverted if negative, direction info separated out + stick.stick_implied_motor_direction = -1; // -1 Reverse, 0 Stopped, +1 Forward (almost never 0) + } // endof deflection < 0.0 + } // endof if chan_mode == 2 + // Now find zone from deflection + stick.zone = ZONE_COAST; + if (stick.deflection < (brake_segment - 0.02)) // size of brake_segment user settable + stick.zone = ZONE_BRAKE; + if (stick.deflection > (brake_segment + 0.02)) // Tiny 'freewheel' COAST band between drive and brake + stick.zone = ZONE_DRIVE; + if (old_zone != ZONE_COAST && old_zone != stick.zone) // + stick.zone = ZONE_COAST; // Ensures transitions between BRAKE and DRIVE go via COAST + switch (stick.zone) { + case ZONE_COAST: + stick.drive_effort = 0.0; + stick.brake_effort = 0.0; + break; + case ZONE_BRAKE: + stick.brake_effort = (brake_segment - stick.deflection) / brake_segment; // 1.0 at zero deflection, reducing to 0.0 on boundary with DRIVE + stick.drive_effort = 0.0; + break; + case ZONE_DRIVE: + stick.brake_effort = 0.0; + dtmp = (stick.deflection - brake_segment) / (1.0 - brake_segment); + if (dtmp > stick.drive_effort) { // Stick has moved in increasing demand direction + stick.drive_effort *= (1.0 - stick_attack); // Apply 'viscous damping' to demand increases for smoother operation + stick.drive_effort += (dtmp * stick_attack); // Low pass filter, time constant variable by choosing 'stick_attack' value %age + } + else // Reduction or no increase in demanded drive effort + stick.drive_effort = dtmp; // Reduce demand immediately, i.e. no viscous damping on reduced demand + break; + } // endof switch + } // endof if active + else { // stick Not active + stick.zone = ZONE_BRAKE; + stick.raw = 0.0; + stick.deflection = 0.0; + } // endof not active + return stick.active; +} + + +void RControl_In::set_offset (signed char offs, char brake_pcent, char attack) { // Takes user_settings[RCIx_TRIM] + brake_segment = ((double) brake_pcent) / 100.0; + stick_attack = ((double) attack) / 100.0; + range_offset = (double) offs; + range_offset /= 1000.0; // This is where to set range_offset sensitivity +// pc.printf ("In RControl_In::set_offset, input signed char = %d, out f %.3f\r\n", offs, range_offset); +} + +uint32_t RControl_In::get_chanmode () { + return chan_mode; +} + +void RControl_In::set_chanmode (char c, char polarity) { + chan_mode = ((uint32_t) c); + stick_sense = (uint32_t) polarity; } void RControl_In::RadC_fall () // December 2018 - Could not make Servo port bidirectional, fix by using PC_14 and 15 as inputs
--- a/Radio_Control_In.h Sat Nov 30 18:40:30 2019 +0000 +++ b/Radio_Control_In.h Tue Jun 09 09:20:19 2020 +0000 @@ -1,6 +1,8 @@ #include "mbed.h" #ifndef MBED_JONS_RADIO_CONTROL_IN_H #define MBED_JONS_RADIO_CONTROL_IN_H +#include "brushless_motor.h" +extern const uint32_t MOTOR_HANDBRAKE, MOTOR_FORWARD, MOTOR_REVERSE, MOTOR_REGENBRAKE; /**class RControl_In Jon Freeman @@ -13,26 +15,35 @@ { InterruptIn pulse_in; Timer t; - int32_t pulse_width_us, period_us, pulse_count; - double lost_chan_return_value; + int32_t pulse_width_us, period_us, pulse_count, tmp; + uint32_t state, chan_mode, stick_sense; // New Dec 2019 + double norm, V, I, stick_deviation, stick_deviation_old; // New Dec 2019 + double lost_chan_return_value, brake_segment, effort, stick_attack; + double range_offset; void RadC_rise (); void RadC_fall (); public: -// RControl_In () ; // Default Constructor - RControl_In (PinName inp) : pulse_in(inp) { // Default Constructor + RControl_In (PinName inp) : pulse_in(inp) { // Constructor pulse_in.mode (PullDown); pulse_in.rise(callback(this, &RControl_In::RadC_rise)); // Attach handler to the rising interruptIn edge pulse_in.fall(callback(this, &RControl_In::RadC_fall)); // Attach handler to the falling interruptIn edge - pulse_width_us = period_us = pulse_count = 0; + pulse_width_us = period_us = pulse_count = stick_sense = tmp = 0; + state = MOTOR_REGENBRAKE; // New Dec 2019 + norm = V = I = stick_deviation = stick_deviation_old = 0.0; // New Dec 2019 lost_chan_return_value = 0.0; + range_offset = 0.0; + stick_attack = 0.5; } ; bool validate_rx () ; // Informs whether signal being rx'd - void reset () ; void set_lost_chan_return_value (double) ; // set what 'normalised' returns when no signal - uint32_t pulsecount () ; // will count up at frame rate when radio control all working well - uint32_t pulsewidth () ; - uint32_t period () ; - double normalised (); // Returns 0.0 <= p <= 1.0 or something else when rc not active + void set_offset (signed char, char, char); // signed char offs, char brake_pcent, char attack + void set_chanmode (char, char); // 1 Disable, Uni, Bi; 2 norm/rev + uint32_t get_chanmode () ; + uint32_t pulsecount () ; // will count up at frame rate when radio control all working well + uint32_t pulsewidth () ; // + uint32_t period () ; // checked for believable repetition rate + bool read (struct RC_stick_info &) ; + bool energise (struct RC_stick_info &, struct brushless_motor &) ; } ; #endif
--- a/SB1602E.lib Sat Nov 30 18:40:30 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/okano/code/SB1602E/#baf578069dfc
--- a/STM3_ESC.h Sat Nov 30 18:40:30 2019 +0000 +++ b/STM3_ESC.h Tue Jun 09 09:20:19 2020 +0000 @@ -1,48 +1,51 @@ /* STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control. Jon Freeman B. Eng Hons - 2015 - 2019 + 2015 - 2020 */ #include "mbed.h" #ifndef MBED_DUALBLS_H #define MBED_DUALBLS_H -//#define USING_DC_MOTORS // Uncomment this to play with Dinosaur DC motors - WARNING deprecated feature +//#define USING_DC_MOTORS // NO LONGER SUPPORTED Uncomment this to play with Dinosaur DC motors - WARNING deprecated feature //#define TEMP_SENSOR_ENABLE // - WARNING deprecated feature, sensor chosen imposed heavy burden on cpu, future looks to simpler analogue type #include "BufferedSerial.h" -const int MOTOR_HANDBRAKE = 0, - MOTOR_FORWARD = 8, - MOTOR_REVERSE = 16, - MOTOR_REGENBRAKE = 24; +const uint32_t MOTOR_HANDBRAKE = 0, + MOTOR_FORWARD = 8, + MOTOR_REVERSE = 16, + MOTOR_REGENBRAKE = 24; -const int TIMEOUT_SECONDS = 2; +const uint32_t TIMEOUT_SECONDS = 2, /* Please Do Not Alter these */ -const int PWM_PRESECALER_DEFAULT = 2, + PWM_PRESECALER_DEFAULT = 2, VOLTAGE_READ_INTERVAL_US = 50, // Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass MAIN_LOOP_REPEAT_TIME_US = 31250, // 31250 us, with TACHO_TAB_SIZE = 32 means tacho_ticks_per_time is tacho_ticks_per_second MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US, -// CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings PWM_HZ = 15000, // chosen to be above cutoff frequency of average human ear // PWM_HZ = 8000, // chosen to be above cutoff frequency of average human ear - clearly audible annoying noise MAX_PWM_TICKS = (SystemCoreClock / (PWM_HZ * PWM_PRESECALER_DEFAULT)), - TICKLE_TIMES = 100 , +// TICKLE_TIMES = 100 , + TICKLE_TIMES = 10 , // Massively reduced May 2020 in connection with handbrake implementation. WATCHDOG_RELOAD = (TIMEOUT_SECONDS * 8); // WatchDog counter ticked down in 8Hz loop -/* End of Please Do Not Alter these */ const double PI = 4.0 * atan(1.0), TWOPI = 8.0 * atan(1.0); +/* End of Please Do Not Alter these */ -enum {COM_SOURCES, COM1, COM2, HAND, RC_IN1, RC_IN2,THEEND} ; +enum {COM_SOURCES, COM1, COM2, HAND, RC_IN1, RC_IN2, RC_IN_BOTH, THEEND} ; // RC_IN_BOTH new Dec 2019 -enum {MOTADIR, MOTBDIR, MOTAPOLES, MOTBPOLES, ISHUNTA, ISHUNTB, SVO1, SVO2, RCIN1, RCIN2, + // List user settable firmware bytes in EEROM +enum {MOTADIR, MOTBDIR, MOTAPOLES, MOTBPOLES, ISHUNTA, ISHUNTB, POT_REGBRAKE_RANGE, SVO1, SVO2, RCIN1, RCIN2, COMM_SRC, BOARD_ID, TOP_SPEED, WHEELDIA, MOTPIN, WHEELGEAR, - FUT1, FUT2, FUT3, FUT4, FUT5} ; // These represent address offsets in 24LC64 rom user settable firmware settings + RCI1_TRIM, RCI2_TRIM, RCIN_REGBRAKE_RANGE, RCIN_STICK_ATTACK, // RC in trims new Dec 2019 + RCIN1REVERSE, RCIN2REVERSE, NOM_SYSTEM_VOLTS, BRAKE_EFFECTIVENESS, BAUD, FUT11, + FUT12, FUT13, FUT14, FUT16,} ; // These represent byte address offsets in 24LC64 rom user settable firmware settings -enum { +enum { // List of fault numbers currently dealt with by error handler FAULT_0, FAULT_EEPROM, FAULT_BOARD_ID, @@ -73,26 +76,45 @@ } ; enum {SOURCE_PC, SOURCE_TS} ; -const int BROADCAST = '\r'; -const int MAX_PARAMS = 20; +const int BROADCAST = '\r'; +const int MAX_CLI_PARAMS = 12; const int MAX_CMD_LEN = 220; struct parameters { // Used in serial comms with pc and other controller (e.g. touch-screen) struct kb_command const * command_list; BufferedSerial * com; // pc or com2 int32_t position_in_list, numof_dbls, target_unit, source, numof_menu_items; - double dbl[MAX_PARAMS]; + double dbl[MAX_CLI_PARAMS]; bool respond, resp_always; } ; +enum {ZONE_BRAKE, ZONE_COAST, ZONE_DRIVE} ; +class RC_stick_info { // info read concerning stick positions +public: + double raw, // range 0.0 to 1.0 after clipping and correction + deflection; // how far from centre or min positon + double brake_effort, // braking effort + drive_effort; // driving effort + uint32_t zone; // in drive, coast or braking zone (if drive, direction in 'stick_implied_motor_direction') + uint32_t chan_mode; + int32_t stick_implied_motor_direction; // -1, 0, +1 but could be in drive, coast or brake regions + bool active; + RC_stick_info () { + active = false; + chan_mode = 0; + zone = ZONE_COAST; + drive_effort = brake_effort = 0.0; + } +} ; + class cli_2019 { // cli Command Line Interpreter, two off, 1 for pc comms, other touch-screen controller comms struct kb_command const * commandlist ; - int clindex; - char cmdline[MAX_CMD_LEN + 8]; - char * cmdline_ptr; + uint32_t clindex; + char cmdline[MAX_CMD_LEN + 8]; + char * cmdline_ptr; parameters a ; public: - cli_2019 (BufferedSerial * comport, kb_command const * list, int list_len, int source) { + cli_2019 (BufferedSerial * comport, kb_command const * list, uint32_t list_len, uint32_t source) { a.com = comport ; a.command_list = commandlist = list ; a.numof_menu_items = list_len ; @@ -104,20 +126,24 @@ else a.resp_always = false; } ; - void core () ; - void test () ; + void core () ; + void flush () ; // Used to clear startup transient garbage } ; +const uint32_t MAX_I2C_DEVICES = 6; + class eeprom_settings { I2C i2c; uint32_t errors; uint32_t i2c_device_count; - uint32_t i2c_device_list[12]; // max 12 i2c devices + uint32_t i2c_device_list[MAX_I2C_DEVICES+1]; // max 12 i2c devices char settings [36]; - bool rd_24LC64 (int start_addr, char * dest, int length) ; - bool wr_24LC64 (int start_addr, char * dest, int length) ; + double rpm2mphdbl, user_brake_rangedbl, Vnomdbl, brake_eff, top_speeddbl; + bool rd_24LC64 (uint32_t start_addr, char * dest, uint32_t length) ; + bool wr_24LC64 (uint32_t start_addr, char * dest, uint32_t length) ; bool set_24LC64_internal_address (int start_addr) ; bool ack_poll () ; + void update_dbls () ; public: eeprom_settings (PinName sda, PinName scl); // Constructor bool do_we_have_i2c (uint32_t x) ; @@ -126,10 +152,24 @@ bool save () ; // Write 'settings' buffer to EEPROM bool set_defaults (); // Put default settings into EEPROM and local buffer uint32_t errs () ; // Return errors + const char * t (uint32_t); + uint32_t min (uint32_t) ; + uint32_t max (uint32_t) ; + uint32_t def (uint32_t) ; + bool in_range (char val, uint32_t p) ; + void edit (double * dbl, uint32_t numof_dbls) ; + double user_brake_range () ; + double brake_effectiveness () ; + double top_speed () ; + double Vnom () ; + double rpm2mph () ; + double rpm2mph (double) ; + uint32_t baud (); } ; - - +struct optpar { + int32_t min, max, de_fault; // min, max, default + const char * txt; // description +} ; #endif -
--- a/brushless_motor.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/brushless_motor.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -1,7 +1,7 @@ /* STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control. Jon Freeman B. Eng Hons - 2015 - 2019 + 2015 - 2020 */ #include "mbed.h" //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h" @@ -9,19 +9,20 @@ //#include "stm32f411xe.h" #include "STM3_ESC.h" #include "BufferedSerial.h" -#include "FastPWM.h" -#include "Servo.h" #include "brushless_motor.h" -extern eeprom_settings mode ; -extern double rpm2mph ; -extern BufferedSerial pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud +extern eeprom_settings user_settings ; +//extern BufferedSerial pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud +/** brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum) : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask) // Constructor -{ - // Constructor +*/ +brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi, + const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, uint16_t port_bit_mask, uint32_t rnum) + : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask) // Constructor +{ // Constructor OP = 0; H1.mode (PullUp); // PullUp resistors enabled on all Hall sensor input pins H2.mode (PullUp); @@ -41,7 +42,7 @@ visible_mode = MOTOR_REGENBRAKE; inner_mode = MOTOR_REGENBRAKE; lut = lutptr; // Pointer to motor energisation bit pattern table - current_sense_rs_offset = rnum; // This is position in mode.rd(current_sense_rs_offset) + current_sense_rs_offset = rnum; // This is position in user_settings.rd(current_sense_rs_offset) Hall_index[0] = Hall_index[1] = read_Halls (); tickleon = 0; direction = 0; @@ -49,19 +50,11 @@ encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction last_V = last_I = 0.0; Idbl = 0.0; - numof_current_sense_rs = 1.0; +// numof_current_sense_rs = 1.0; RPM_filter = 0.0; - dv_by_dt = 0.0; - sdbl[1] = 0.25; // Remind me. What are these all about ? - sdbl[2] = 9.0; - sdbl[3] = 0.4; - sdbl[4] = 0.2; dRPM = 0.0; V_clamp = 1.0 ; // Used to limit top speed motor_poles = 8; // Default to 8 pole motor -#ifdef USING_DC_MOTORS - dc_motor = (Hall_index[0] == 7) ? true : false ; -#endif } /** @@ -75,18 +68,15 @@ const double sampweight = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED ; const double shrinkby = 1.0 - sampweight; uint16_t samp = Motor_I.read_u16 (); // CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019 - double dbls = ((double)samp) * numof_current_sense_rs / 6.0; // reading in mA +// double dbls = ((double)samp) * numof_current_sense_rs / 6.0; // reading in mA +// double dbls = (((double)samp) / 6.0) * (double)user_settings.rd(current_sense_rs_offset); // reading in mA +// double dbls = sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0; // reading in mA Idbl *= shrinkby; // Jan 2019 New recursive low pass filter - Idbl += dbls * sampweight; // Current reading available, however not certain this is of any real use +// Idbl += dbls * sampweight; // Current reading available, however not certain this is of any real use + Idbl += sampweight * (double)(samp * user_settings.rd(current_sense_rs_offset)) / 6.0; // Current reading available, however not certain this is of any real use } -bool brushless_motor::poles (int p) { // Jan 2019 max_rom no longer used. target_speed is preferred - if (!max_rpm) { // Not been set since powerup - max_rpm = (uint32_t) (((double)mode.rd(TOP_SPEED) / rpm2mph) / 10.0) ; - target_speed = (double)mode.rd(TOP_SPEED) / 10.0; - numof_current_sense_rs = (double)mode.rd(current_sense_rs_offset); - pc.printf ("max_rpm=%d, top speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph); - } +bool brushless_motor::poles (uint32_t p) { if (p == 4 || p == 6 || p == 8) { motor_poles = p; return true; @@ -110,8 +100,8 @@ Hall_index[0] = read_Halls (); delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]]; - if (delta_theta == 0) - encoder_error_cnt++; + if (delta_theta == 0) // Should only ever be +1 in 1 direction, -1 in other direction. 0 indicates invalid Hall sensor transition detected. + encoder_error_cnt++; // Never used Dec 2019 else angle_cnt += delta_theta; OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18 @@ -120,17 +110,21 @@ } /** -* void brushless_motor::direction_set (int dir) { -* Used to set direction according to mode data from eeprom +* void brushless_motor::set_direction (int dir) { +* Used to set direction according to user_settings data from eeprom */ -void brushless_motor::direction_set (int dir) +void brushless_motor::set_direction (uint32_t dir) { direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; // bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4 } -int brushless_motor::read_Halls () +//uint32_t get_direction () { +// return direction; +//} + +uint32_t brushless_motor::read_Halls () { - int x = 0; + uint32_t x = 0; if (H1) x |= 1; if (H2) x |= 2; if (H3) x |= 4; @@ -151,13 +145,7 @@ } */ -void brushless_motor::set_speed (double p) // Sets target_speed -{ - target_speed = p; -} - -extern double Current_Scaler_Sep_2019; extern int WatchDog; #define DRIVING (visible_mode == MOTOR_FORWARD || visible_mode == MOTOR_REVERSE) #define ESTOP (WatchDog == 0 && DRIVING) @@ -167,6 +155,17 @@ * * Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0) */ +void brushless_motor::motor_voltage_refresh () // May alter motor voltage to reflect changes in V_clamp +{ + double p = last_V; + if ((V_clamp < last_V) && DRIVING) // Jan 2019 limit top speed when driving + p = V_clamp; // If motor runnable, set voltage limit to lower of last_V and V_clamp + + p *= 0.95; // need limit, ffi see MCP1630 data + p = 1.0 - p; // because pwm is wrong way up + maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts +} + void brushless_motor::set_V_limit (double p) // Sets max motor voltage. { if (p < 0.0 || ESTOP) @@ -174,13 +173,7 @@ if (p > 1.0) p = 1.0; last_V = p; // Retains last voltage limit demanded by driver - - if ((V_clamp < last_V) && DRIVING) // Jan 2019 limit top speed when driving - p = V_clamp; // If motor runnable, set voltage limit to lower of last_V and V_clamp - - p *= 0.95; // need limit, ffi see MCP1630 data - p = 1.0 - p; // because pwm is wrong way up - maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts + motor_voltage_refresh () ; } @@ -206,25 +199,29 @@ * * Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents */ +//const double MPR = (double)((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input +const uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level { - const uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input if (p < 0.0 || ESTOP) p = 0.0; if (p > 1.0) p = 1.0; last_I = p; // Retains last current limit demanded by driver if (DRIVING) { - if (Current_Scaler_Sep_2019 > 1.0) - Current_Scaler_Sep_2019 = 1.0; - if (Current_Scaler_Sep_2019 < 0.0) - Current_Scaler_Sep_2019 = 0.0; - p *= Current_Scaler_Sep_2019; + p *= current_scale; } maxI.pulsewidth_ticks ((uint32_t)(p * MPR)); // PWM } +void brushless_motor::I_scale (double p) // Sets max motor current. pwm integrated to dc ref voltage level +{ + current_scale = p; + if (DRIVING) { + maxI.pulsewidth_ticks ((uint32_t)(last_I * p * MPR)); // PWM + } +} /** * void brushless_motor::speed_monitor_and_control () // ** CALL THIS 32 TIMES PER SECOND ** @@ -238,41 +235,41 @@ * Scope for further improvement of control algorithm - crude implementation of PID with no I */ void brushless_motor::speed_monitor_and_control () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec -{ -#ifdef USING_DC_MOTORS // deprecated - if (dc_motor) - return 0; -#endif -// Feb 2019 - coefficients currently values in ram to allow for tweaking via command line. Will be 'const' once settled upon. -// const double samp_scale = 0.35; // Tweak this value only to tune filter - double samp_scale = sdbl[1]; // Tweak this value only to tune filter - double shrink_by = 1.0 - samp_scale; -// const double dv_scale = 0.15; - double dv_scale = sdbl[3]; - double dv_shrink = 1.0 - dv_scale; - double speed_error, d, t; +{ // +// const double samp_scale = 0.275; // Tweak this value only to tune filter + const double samp_scale = 0.6; // Increased May 2020 to improve ssl speed settling time + const double shrink_by = 1.0 - samp_scale; + double rpm, speed_error; uint32_t Hall_tot_copy = Hall_total; // Copy value for use throughout function as may get changed at any instant during exec ! moving_flag = true; if (Hall_previous == Hall_tot_copy) { // Test for motor turning or not moving_flag = false; // Zero Hall transitions since previous call - motor not moving tickleon = TICKLE_TIMES; // May need to tickle some charge into high side switch bootstrap supplies - } - d = (double) ((Hall_tot_copy - Hall_previous) *640); // (Motor Hall sensor transitions in previous 31250us) * 640 - d /= motor_poles; // d now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles - t = RPM_filter; // Value from last time around - RPM_filter *= shrink_by; - RPM_filter += (d * samp_scale); // filtered RPM - // RPM_filter[n] = shrink_by RPM_filter[n - 1] + samp_scale x[n] - t -= RPM_filter; // more like minus dv/dt - dv_by_dt *= dv_shrink; - dv_by_dt += (t * dv_scale); // filtered rate of change, the 'D' Differential contribution to PID control - dRPM += RPM_filter; - dRPM /= 2.0; - dMPH = RPM_filter * rpm2mph; // Completed updates of RPM and MPH - - if (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE) { // Speed control only makes sense when motor runnable - speed_error = (target_speed - dMPH) / 1000.0; // 'P' Proportional contribution to PID control - d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]); // Apply P+I+D (with I=0) control + } + rpm = (double) (((Hall_tot_copy - Hall_previous) * 640) / motor_poles); // (Motor Hall sensor transitions in previous 31250us) * 640 + RPM_filter *= shrink_by; // rpm now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles + RPM_filter += (rpm * samp_scale); // filtered RPM + dRPM = RPM_filter; + dMPH = user_settings.rpm2mph(RPM_filter); // Completed updates of RPM and MPH + Hall_previous = Hall_tot_copy; + speed_error = (dMPH - user_settings.top_speed()); // 'P' Proportional contribution to PID control + bool clamp_change = false; + if (speed_error > 0.0 && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) { // Speed control only makes sense when motor runnable + V_clamp *= (1.0 - (speed_error / 60.0)); // Driving too fast, so lower clamp voltage a tiny bit + clamp_change = true; + } + else { // Not going too fast, and maybe driving or not + if (V_clamp < 0.99) { + V_clamp += 0.015; + if (V_clamp > 1.0) + V_clamp = 1.0; + clamp_change = true; + } + } + if (clamp_change) { + motor_voltage_refresh () ; + } +/* d = V_clamp + (speed_error * sdbl[2]) + ((dv_by_dt / 1000.0) * sdbl[4]); // Apply P+I+D (with I=0) control if (d > 1.0) d = 1.0; if (d < 0.0) d = 0.0; V_clamp = d; @@ -280,16 +277,20 @@ { d *= 0.95; // need limit, ffi see MCP1630 data d = 1.0 - d; // because pwm is wrong way up +//FUCKETYFUCK maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts } - } + }*/ /* temp_tick++; - if (temp_tick > 35) { // one and a bit second + if (temp_tick > 50) { // one and a bit second temp_tick = 0; - pc.printf ("RPM %.0f, %.3f, %.3f, %.2f, dv/dt%.3f\r\n", dRPM, RPM_filter, d, dMPH, dv_by_dt); + pc.printf ("top speed %.1f, mph %.1f, speed_err %.1f, V_clamp%.3f\r\n", user_settings.top_speed(), dMPH, speed_error, V_clamp); } */ - Hall_previous = Hall_tot_copy; +} + +bool brushless_motor::exists () { + return ((Hall_index[0] > 0) && (Hall_index[0] < 7)) ; } bool brushless_motor::is_moving () @@ -299,10 +300,10 @@ /** bool brushless_motor::set_mode (int m) -Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE. +Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE. Used to also be used to set REGENBRAKE, replaced by brake(x) function. If this causes change of mode, also sets V and I to zero. */ -bool brushless_motor::set_mode (int m) +bool brushless_motor::set_mode (uint32_t m) { if ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE)) { // pc.printf ("Error in set_mode, invalid mode %d\r\n", m); @@ -313,12 +314,30 @@ set_I_limit (0.0); visible_mode = m; } - if (m == MOTOR_FORWARD || m == MOTOR_REVERSE) - m ^= direction; + if (m == MOTOR_FORWARD || m == MOTOR_REVERSE) // 8 or 16 - these are effectively address bits of motor pattern lut + m ^= direction; // direction set to 0 or (MOTOR_FORWARD | MOTOR_REVERSE), so has zero or two bits set inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom return true; } +void brushless_motor::brake (double brake_effort) { +// pc.printf ("In motor::brake, user_settings.brake_effectiveness = %3f\r\n", user_settings.brake_effectiveness()); + set_mode (MOTOR_REGENBRAKE); + if (brake_effort > 1.0) + brake_effort = 1.0; + if (brake_effort < 0.0) + brake_effort = 0.0; + brake_effort *= user_settings.brake_effectiveness(); // set upper limit, this is essential - May2020 fixed, was 100 times too big + brake_effort = sqrt (brake_effort); // to linearise effect + set_V_limit (brake_effort) ; + set_I_limit (1.0); + V_clamp = 1.0; +} + +uint32_t brushless_motor::get_mode () { + return visible_mode; +} + void brushless_motor::motor_set () // Energise Port with data determined by Hall sensors { Hall_index[0] = read_Halls ();
--- a/brushless_motor.h Sat Nov 30 18:40:30 2019 +0000 +++ b/brushless_motor.h Tue Jun 09 09:20:19 2020 +0000 @@ -18,45 +18,43 @@ visible_mode, // One of MOTOR_HANDBRAKE, MOTOR_FORWARD, MOTOR_REVERSE, MOTOR_REGENBRAKE inner_mode; uint32_t direction; - int temp_tick; - double RPM_filter, dv_by_dt; - double target_speed; + double RPM_filter; //, dv_by_dt; bool moving_flag; const uint16_t * lut; AnalogIn Motor_I; FastPWM maxV, maxI; InterruptIn H1, H2, H3; // Inputs for motor Hall sensors PortOut OP; + void motor_voltage_refresh () ; void Hall_change () ; - int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs - uint32_t max_rpm ; + uint32_t read_Halls () ; // Returns 3 bits of latest Hall sensor outputs +// uint32_t max_rpm ; double V_clamp ; // Used to limit top speed - double numof_current_sense_rs; +// double numof_current_sense_rs; + double last_V, last_I, current_scale; public: -#ifdef USING_DC_MOTORS // deprecated - bool dc_motor; -#endif uint32_t tickleon; double Idbl; - double last_V, last_I; - double dRPM, dMPH, - sdbl[8]; // Filter coefficients for filtering RPM and MPH reading stuff, I think! - // Used in brushless_motor::speed_monitor_and_control () + double dRPM, dMPH; // brushless_motor () {} ; // can not use this with exotic elements PortOut, FastPWM etc - brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t *, PinName h1, PinName h2, PinName h3, PortName, int, uint32_t) ; // Constructor - bool poles (int) ; // Set number of motor poles - 4, 6, or 8 - void set_speed (double) ; // Sets target_speed - void set_V_limit (double) ; // Sets max motor voltage - void set_I_limit (double) ; // Sets max motor current - void motor_set () ; // Energise Port with data determined by Hall sensors - void direction_set (int) ; // sets 'direction' with bit pattern to eor with MOTOR_FORWARD or MOTOR_REVERSE in set_mode - bool set_mode (int); // sets mode to MOTOR_HANDBRAKE, MOTOR_FORWARD, MOTOR_REVERSE or MOTOR_REGENBRAKE - bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs - void speed_monitor_and_control () ; // call this once per main loop pass (32Hz) to keep count = edges per sec - void high_side_off () ; + brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t *, PinName h1, PinName h2, PinName h3, PortName, uint16_t, uint32_t) ; // Constructor + bool poles (uint32_t) ; // Set number of motor poles - 4, 6, or 8 + void set_V_limit (double) ; // Sets max motor voltage + void set_I_limit (double) ; // Sets max motor current + void motor_set () ; // Energise Port with data determined by Hall sensors + void set_direction (uint32_t) ; // sets 'direction' with bit pattern to eor with MOTOR_FORWARD or MOTOR_REVERSE in set_mode + void brake (double brake_effort) ; +// uint32_t get_direction () ; + bool set_mode (uint32_t); // sets mode to MOTOR_HANDBRAKE, MOTOR_FORWARD, MOTOR_REVERSE or MOTOR_REGENBRAKE + uint32_t get_mode () ; + bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs + void speed_monitor_and_control () ; // call this once per main loop pass (32Hz) to keep count = edges per sec + void high_side_off () ; // void low_side_on () ; - void sniff_current () ; // Call this every 200us to update Idbl + void sniff_current () ; // Call this every 200us to update Idbl + void I_scale (double); + bool exists () ; // New May 2020 - reports presence or absence of motor } ; //MotorA, MotorB, or even Motor[2]; #endif
--- a/cli_BLS_nortos.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/cli_BLS_nortos.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -24,21 +24,20 @@ #include <cctype> using namespace std; -extern eeprom_settings mode ; +extern eeprom_settings user_settings ; extern error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes. extern int WatchDog; // from main extern bool WatchDogEnable; // from main -extern double rpm2mph ; +extern bool read_temperature (float & t) ; // from main March 2020 extern brushless_motor MotorA, MotorB; // Controlling two motors together or individually -extern char const_version_string[] ; +extern const char * get_version () ; // Need this as extern const char can not be made to work. This returns & const_version_string extern BufferedSerial com2, pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud -extern void setVI (double v, double i) ; // Set motor voltage limit and current limit +extern void setVI_both (double v, double i) ; // Set motor voltage limit and current limit extern double Read_DriverPot (); extern double Read_BatteryVolts (); -extern void mode_set_both_motors (int mode, double val) ; // called from cli to set fw, re, rb, hb -extern void rcin_report () ; +extern void mode_set_motors_both (int mode) ; // called from cli to set fw, re, rb, hb // All void func (struct parameters &) ; functions addressed by command line interpreter are together below here @@ -49,12 +48,14 @@ */ void ver_cmd (struct parameters & a) { - if (a.source == SOURCE_PC) - pc.printf ("Version %s\r\n", const_version_string); + if (a.source == SOURCE_PC) { + pc.printf ("Version [%s]\r\n", get_version()); + } else { if (a.source == SOURCE_TS) - if (a.respond) // Only respond if this board addressed - a.com->printf ("%s\r", const_version_string); + if (a.respond) { // Only respond if this board addressed + a.com->printf ("%s\r", get_version()); + } else pc.printf ("Crap source %d in ver_cmd\r\n", a.source); } @@ -78,28 +79,13 @@ void null_cmd (struct parameters & a) { if (a.respond) - a.com->printf ("At null_cmd, board ID %c\r\n", mode.rd(BOARD_ID)); + a.com->printf ("Board ID %c\r\n", user_settings.rd(BOARD_ID)); } -#ifdef USING_DC_MOTORS -/** -void mt_cmd (struct parameters & a) - PC Only - Responds YES, causes action NO - report_motor_types () // Reports 'Brushless' if Hall inputs read 1 to 6, 'DC' if no Hall sensors connected, therefore DC motor assumed -*/ -void mt_cmd (struct parameters & a) -{ - if (a.source == SOURCE_PC) - pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.dc_motor ? "DC":"Brushless", MotorB.dc_motor ? "DC":"Brushless"); - else - pc.printf ("Wrong use of mt_cmd\r\n"); -} -#endif /** * void rdi_cmd (struct parameters & a) // read motor currents (uint32_t) and BatteryVolts (double) -*/ + void rdi_cmd (struct parameters & a) // read motor currents (uint32_t) and BatteryVolts (double) { // Voltage reading true volts, currents only useful as relative values if (a.respond) @@ -107,16 +93,16 @@ a.com->printf ("rdi%.1f %.1f %.1f\r%s", MotorA.Idbl, MotorB.Idbl, Read_BatteryVolts (), a.source == SOURCE_PC ? "\n" : ""); // Format good to be unpicked by cli in touch screen controller } - +*/ /** * void rvi_cmd (struct parameters & a) // read last normalised motor voltage and current values sent to pwms * */ -void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms -{ - if (a.respond) - a.com->printf ("rvi%.2f %.2f %.2f %.2f\r%s", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I, a.source == SOURCE_PC ? "\n" : ""); -} +//void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms +//{ +// if (a.respond) +// a.com->printf ("rvi%.2f %.2f %.2f %.2f\r%s", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I, a.source == SOURCE_PC ? "\n" : ""); +//} /** * void fw_cmd (struct parameters & a) // Forward command @@ -124,7 +110,9 @@ */ void fw_cmd (struct parameters & a) // Forward command { - mode_set_both_motors (MOTOR_FORWARD, 0.0); +// MotorA.set_mode (MOTOR_FORWARD); +// MotorB.set_mode (MOTOR_FORWARD); + mode_set_motors_both (MOTOR_FORWARD); if (a.source == SOURCE_PC) pc.printf ("fw\r\n"); // Show response to action if command from pc terminal } @@ -135,7 +123,7 @@ */ void re_cmd (struct parameters & a) // Reverse command { - mode_set_both_motors (MOTOR_REVERSE, 0.0); + mode_set_motors_both (MOTOR_REVERSE); if (a.source == SOURCE_PC) pc.printf ("re\r\n"); } @@ -146,9 +134,11 @@ */ void rb_cmd (struct parameters & a) // Regen brake command { - mode_set_both_motors (MOTOR_REGENBRAKE, a.dbl[0] / 100.0); + double tmp = a.dbl[0] / 100.0; + MotorA.brake (tmp); + MotorB.brake (tmp); // Corrected May 2020 - previously MotorA twice if (a.source == SOURCE_PC) - pc.printf ("rb %.2f\r\n", a.dbl[0] / 100.0); + pc.printf ("rb %.2f\r\n", tmp); } /** @@ -156,136 +146,72 @@ * Broadcast to all STM3_ESC boards, required to ACT but NOT respond * * NOTE Jan 2019 Hand brake not implemented -* +* May 2020 - Implemented, but not very good */ void hb_cmd (struct parameters & a) // Hand brake command { + double tmp; + if (a.numof_dbls != 0) tmp = a.dbl[0] / 100.0; // a.numof_dbls is int32_t + else tmp = 0.33; if (a.respond) { - a.com->printf ("numof params = %d\r\n", a.numof_dbls); - a.com->printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]); +// a.com->printf ("numof params = %d\r\n", a.numof_dbls); + a.com->printf ("Hand Brake : Force 0 to 99 %.0f\r\n", tmp * 100.0); } - mode_set_both_motors (MOTOR_HANDBRAKE, 0.0); + mode_set_motors_both (MOTOR_HANDBRAKE); + if (tmp < 0.0) tmp = 0.0; + if (tmp > 1.0) tmp = 1.0; + setVI_both (tmp / 5.0, 0.99); } -extern int numof_eeprom_options2 ; -extern struct optpar const option_list2[] ; -/**void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents -* mode_cmd called only from pc comms. No sense calling from Touch Screen Controller + + +/**void user_settings_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents +* user_settings_cmd called only from pc comms. No sense calling from Touch Screen Controller * * Called without parameters - Lists to pc terminal current settings * */ -void mode19_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents +void user_settings_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents { - char temps[36]; - int i; - double topspeed; // New Jan 2019 - set max loco speed - pc.printf ("\r\nmode - Set system data in EEPROM - Jan 2019\r\nSyntax 'mode' with no parameters lists current state.\r\n"); - if (a.numof_dbls) { // If more than 0 parameters supplied - for (i = 0; i < a.numof_dbls; i++) - temps[i] = (char)a.dbl[i]; // recast doubles to char - while (i < 33) - temps[i++] = 0; - switch ((int)a.dbl[0]) { - case 0: // MotorA_dir [0 or 1], MotorB_dir [0 or 1] - if (temps[1] == 0 || temps[1] == 1) - mode.wr(temps[1], MOTADIR); - if (temps[2] == 0 || temps[2] == 1) - mode.wr(temps[2], MOTBDIR); - break; - case 1: // MotorA_poles [4,6,8], MotorB_poles [4,6,8] - if (temps[1] == 4 || temps[1] == 6 || temps[1] == 8) - mode.wr(temps[1], MOTAPOLES); - if (temps[2] == 4 || temps[2] == 6 || temps[2] == 8) - mode.wr(temps[2], MOTBPOLES); - break; - case 2: // MotorA_ current sense resistors [1 to 4], MotorB_ current sense resistors [1 to 4] - if (temps[1] > 0 && temps[1] < 5) - mode.wr(temps[1], ISHUNTA); // Corrected since published - if (temps[2] > 0 && temps[2] < 5) - mode.wr(temps[2], ISHUNTB); - break; - case 3: // 2 Servo1 [0 or 1], Servo2 [0 or 1] - if (temps[1] == 0 || temps[1] == 1) - mode.wr(temps[1], SVO1); - if (temps[2] == 0 || temps[2] == 1) - mode.wr(temps[2], SVO2); - break; - case 4: // 3 RCIn1 [0 or 1], RCIn2 [0 or 1] - if (temps[1] == 0 || temps[1] == 1) - mode.wr(temps[1], RCIN1); - if (temps[2] == 0 || temps[2] == 1) - mode.wr(temps[2], RCIN2); - break; - case 5: // 4 Board ID '0' to '9' - if (temps[1] <= 9) // pointless to compare unsigned integer with zero - mode.wr('0' | temps[1], BOARD_ID); - break; - case 6: // TOP_SPEED - topspeed = a.dbl[1]; - if (topspeed > 25.0) topspeed = 25.0; - if (topspeed < 1.0) topspeed = 1.0; - mode.wr((char)(topspeed * 10.0), TOP_SPEED); - break; - case 7: // 5 Wheel dia mm, Motor pinion teeth, Wheel gear teeth - mode.wr(temps[1], WHEELDIA); - mode.wr(temps[2], MOTPIN); - mode.wr(temps[3], WHEELGEAR); - break; - case 8: // {2, 5, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RC Input1, 5= RC Input2"}, - if (temps[1] > 1 && temps[1] < 6) - mode.wr(temps[1], COMM_SRC); - break; - case 83: // set to defaults - mode.set_defaults (); - break; - case 9: // 9 Save settings - mode.save (); - pc.printf ("Saving settings to EEPROM\r\n"); - break; - default: - break; - } // endof switch - } // endof // If more than 0 parameters supplied - else { - pc.printf ("No Changes\r\n"); - } - pc.printf ("mode 0\tMotorA_dir [0 or 1]=%d, MotorB_dir [0 or 1]=%d\r\n", mode.rd(MOTADIR), mode.rd(MOTBDIR)); - pc.printf ("mode 1\tMotorA_poles [4,6,8]=%d, MotorB_poles [4,6,8]=%d\r\n", mode.rd(MOTAPOLES), mode.rd(MOTBPOLES)); - pc.printf ("mode 2\tMotorA I 0R05 sense Rs [1to4]=%d, MotorB I 0R05 sense Rs [1to4]=%d\r\n", mode.rd(ISHUNTA), mode.rd(ISHUNTB)); - pc.printf ("mode 3\tServo1 [0 or 1]=%d, Servo2 [0 or 1]=%d\r\n", mode.rd(SVO1), mode.rd(SVO2)); - pc.printf ("mode 4\tRCIn1 [0 or 1]=%d, RCIn2 [0 or 1]=%d\r\n", mode.rd(RCIN1), mode.rd(RCIN2)); - pc.printf ("mode 5\tBoard ID ['0' to '9']='%c'\r\n", mode.rd(BOARD_ID)); - pc.printf ("mode 6\tTop Speed MPH [1.0 to 25.0]=%.1f\r\n", double(mode.rd(TOP_SPEED)) / 10.0); - pc.printf ("mode 7\tWheel dia mm=%d, Motor pinion teeth=%d, Wheel gear teeth=%d\r\n", mode.rd(WHEELDIA), mode.rd(MOTPIN), mode.rd(WHEELGEAR)); - pc.printf ("mode 8\tCommand Src [%d] - 2=COM2 (Touch Screen), 3=Pot, 4=RC In1, 5=RC In2\r\n", mode.rd(COMM_SRC)); - pc.printf ("mode 83\tSet to defaults\r\n"); - pc.printf ("mode 9\tSave settings\r\r\n"); + user_settings.edit (a.dbl, a.numof_dbls); +} + +extern struct optpar option_list[] ; //= { +void brake_eff_set_cmd (struct parameters & a) { // set brake effectiveness from TS May 2020 + char be = (char) a.dbl[0]; + pc.printf ("BRAKE_EFFECTIVENESS min = %d, max = %d\r\n", option_list[BRAKE_EFFECTIVENESS].min, option_list[BRAKE_EFFECTIVENESS].max); + if (be > option_list[BRAKE_EFFECTIVENESS].max) be = option_list[BRAKE_EFFECTIVENESS].max; + if (be < option_list[BRAKE_EFFECTIVENESS].min) be = option_list[BRAKE_EFFECTIVENESS].min; + user_settings.wr(be, BRAKE_EFFECTIVENESS); + user_settings.save (); + pc.printf ("Set brake effectiveness to %d pct\r\n", be); } - -void ssl_cmd (struct parameters & a) { // set speed limit NEW and untested July 2019 - if (a.dbl[0] > 25.0) a.dbl[0] = 25.0; - if (a.dbl[0] < 1.0) a.dbl[0] = 1.0; - mode.wr((char)(a.dbl[0] * 10.0), TOP_SPEED); - mode.save (); +void ssl_cmd (struct parameters & a) { // set speed limit NEW and untested July 2019. Stored as speed * 10 to get 1dec place + char sp = (char) (a.dbl[0] * 10.0); + if (sp > option_list[TOP_SPEED].max) sp = option_list[TOP_SPEED].max; + if (sp < option_list[TOP_SPEED].min) sp = option_list[TOP_SPEED].min; + user_settings.wr(sp, TOP_SPEED); + user_settings.save (); + pc.printf ("Set speed limit to %.1f mph\r\n", a.dbl[0]); } -#ifdef TEMP_SENSOR_ENABLE - -extern uint32_t last_temperature_count; /** * void temperature_cmd (struct parameters & a) { -* Few boards have temperature sensor fitted. Non-preferred feature +* Few boards have temperature sensor fitted. Now only supports LM75B i2c sensor */ -void temperature_cmd (struct parameters & a) { +void read_temperature_cmd (struct parameters & a) { + float t = -99.25; if (a.respond) { - a.com->printf ("tem%c %d\r\n", mode.rd(BOARD_ID), (last_temperature_count / 16) - 50); + a.com->printf ("tem%c ", user_settings.rd(BOARD_ID)); + if (read_temperature(t)) + a.com->printf ("Temperature = %7.3f\r\n", t); + else + a.com->printf ("Temp sensor not fitted\r\n"); } } -#endif /** *void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors @@ -293,18 +219,31 @@ void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors { if (a.respond) -// a.com->printf ("rpm%c %d %d\r%s", mode.rd(BOARD_ID), MotorA.RPM, MotorB.RPM, a.source == SOURCE_PC ? "\n" : ""); - a.com->printf ("rpm%c %.0f %.0f\r%s", mode.rd(BOARD_ID), MotorA.dRPM, MotorB.dRPM, a.source == SOURCE_PC ? "\n" : ""); +// a.com->printf ("rpm%c %d %d\r%s", user_settings.rd(BOARD_ID), MotorA.RPM, MotorB.RPM, a.source == SOURCE_PC ? "\n" : ""); + a.com->printf ("rpm%c %.0f %.0f\r%s", user_settings.rd(BOARD_ID), MotorA.dRPM, MotorB.dRPM, a.source == SOURCE_PC ? "\n" : ""); } /** *void mph_cmd (struct parameters & a) // to report miles per hour */ -void mph_cmd (struct parameters & a) // to report miles per hour to 1 decimal place +void mph_cmd (struct parameters & a) // to report miles per hour to 1 decimal place - now 2dp May 2020 { + int j = 0; + double speedmph = 0.0; + if (MotorA.exists ()) { + j |= 1; + speedmph = MotorA.dMPH; + } + if (MotorB.exists ()) { + j |= 2; + speedmph += MotorB.dMPH; + } + if (j == 3) + speedmph /= 2.0; if (a.respond) -// a.com->printf ("mph%c %.1f\r%s", mode.rd(BOARD_ID), (double)(MotorA.RPM + MotorB.RPM) * rpm2mph / 2.0, a.source == SOURCE_PC ? "\n" : ""); - a.com->printf ("mph%c %.1f\r%s", mode.rd(BOARD_ID), (double)(MotorA.dMPH + MotorB.dMPH) / 2.0, a.source == SOURCE_PC ? "\n" : ""); +// May 17th 2020 modified line below - removed superfluous cast, upped to 2 decimal places +// a.com->printf ("mph%c %.2f\r%s", user_settings.rd(BOARD_ID), speedmph, a.source == SOURCE_PC ? "\n" : ""); + a.com->printf ("?s%c %.2f\r%s", user_settings.rd(BOARD_ID), speedmph, a.source == SOURCE_PC ? "\n" : ""); } /** @@ -314,7 +253,7 @@ void sysV_report (struct parameters & a) // { if (a.respond) - a.com->printf ("?v%c %.1f\r%s", mode.rd(BOARD_ID), Read_BatteryVolts(), a.source == SOURCE_PC ? "\n" : ""); + a.com->printf ("?v%c %.1f\r%s", user_settings.rd(BOARD_ID), Read_BatteryVolts(), a.source == SOURCE_PC ? "\n" : ""); } /** @@ -323,12 +262,46 @@ */ void sysI_report (struct parameters & a) // { - if (a.respond) // Calibration, refinement of 6000.0 (not miles out) first guess possible. -// a.com->printf ("?i%c %.2f %.2f\r%s", mode_bytes[BOARD_ID], (double)MotorA.I.ave / 6000.0, (double)MotorB.I.ave / 6000.0, a.source == SOURCE_PC ? "\n" : ""); -// a.com->printf ("?i%c %.2f %.2f\r%s", mode.rd(BOARD_ID), (double)MotorA.I.ave / 6000.0, MotorA.I.dblave2 / 6000.0, a.source == SOURCE_PC ? "\n" : ""); - a.com->printf ("?i%c %.2f %.2f\r%s", mode.rd(BOARD_ID), MotorA.Idbl, MotorB.Idbl, a.source == SOURCE_PC ? "\n" : ""); + if (a.respond) + a.com->printf ("?i%c %.2f %.2f\r%s", user_settings.rd(BOARD_ID), MotorA.Idbl, MotorB.Idbl, a.source == SOURCE_PC ? "\n" : ""); } +/* + +New December 2019 - possible for implementation + Having implemented radio control inputs and driver pot, there is some sense in moving slider handler from touch screen controller + into STM3_ESC code. + New instructions to be accepted from touch screen : - + {"w", "touch screen new slider touch", slider_touch_cmd}, + {"x", "updated slider RANGE 0 to 99", slider_touch_position_cmd}, // max two digits + {"y", "touch_screen slider finger lifted clear", slider_untouch_cmd}, +*/ +bool finger_on_slider = false; +/**void slider_touch_cmd (struct parameters & a) +* +* Message from touch screen controller, slider touched +*/ +void slider_touch_cmd (struct parameters & a) { + finger_on_slider = true; +} + +/**void slider_untouch_cmd (struct parameters & a) +* +* Message from touch screen controller, finger taken off slider +*/ +void slider_untouch_cmd (struct parameters & a) { + finger_on_slider = false; +} + +/**void slider_touch_position_cmd (struct parameters & a) +* +* Message from touch screen controller, latest slider touch position +*/ +void slider_touch_position_cmd (struct parameters & a) { +} + +// End of New December 2019 + /**void vi_cmd (struct parameters & a) * @@ -336,8 +309,8 @@ */ void vi_cmd (struct parameters & a) { - setVI (a.dbl[0] / 100.0, a.dbl[1] / 100.0); - pc.printf ("setVI from %s\r\n", a.source == SOURCE_PC ? "PC" : "Touch Screen"); + setVI_both (a.dbl[0] / 100.0, a.dbl[1] / 100.0); +// pc.printf ("setVI_both from %s\r\n", a.source == SOURCE_PC ? "PC" : "Touch Screen"); } /** @@ -370,7 +343,7 @@ */ void kd_cmd (struct parameters & a) // kick the watchdog. Reached from TS or pc. { - WatchDog = WATCHDOG_RELOAD + (mode.rd(BOARD_ID) & 0x0f); // Reload watchdog timeout. Counted down @ 8Hz + WatchDog = WATCHDOG_RELOAD + (user_settings.rd(BOARD_ID) & 0x0f); // Reload watchdog timeout. Counted down @ 8Hz WatchDogEnable = true; // Receipt of this command sufficient to enable watchdog } @@ -387,8 +360,8 @@ */ void who_cmd (struct parameters & a) // Reachable always from pc. Only addressed board responds to TS { - if (a.source == SOURCE_PC || mode.rd(BOARD_ID) == a.target_unit) - a.com->printf ("who%c\r%s", mode.rd(BOARD_ID), a.source == SOURCE_PC ? "\n" : ""); + if (a.source == SOURCE_PC || user_settings.rd(BOARD_ID) == a.target_unit) + a.com->printf ("who%c\r%s", user_settings.rd(BOARD_ID), a.source == SOURCE_PC ? "\n" : ""); } /** @@ -396,11 +369,11 @@ * * For test, reports to pc terminal info about radio control input channels */ -void rcin_pccmd (struct parameters & a) -{ - rcin_report (); -} - +//void rcin_pccmd (struct parameters & a) +//{ +// rcin_report (); +//} +/* void scmd (struct parameters & a) // filter coefficient fiddler { switch ((int)a.dbl[0]) { @@ -416,10 +389,6 @@ case 4: MotorA.sdbl[4] = MotorB.sdbl[4] = a.dbl[1]; break; - case 5: - MotorA.set_speed (a.dbl[1]); - MotorB.set_speed (a.dbl[1]); - break; default: pc.printf ("Wrong use of scmd %f\r\n", a.dbl[0]); } @@ -429,9 +398,9 @@ pc.printf ("2 %.3f\tP_gain 1.0-1000.0\r\n", MotorA.sdbl[2]); pc.printf ("3 %.3f\tDscale 0.01-0.5\r\n", MotorA.sdbl[3]); pc.printf ("4 %.3f\tD_gain 1.0-1000.0\r\n", MotorA.sdbl[4]); - pc.printf ("5 Set target_speed\r\n"); +// pc.printf ("5 Set target_speed\r\n"); } - +*/ struct kb_command { // Commands tabulated as list of these structures as seen below const char * cmd_word; // points to text e.g. "menu" const char * explan; // very brief explanation or clue as to purpose of function @@ -446,7 +415,7 @@ void menucmd (struct parameters & a) { if (a.respond) { - a.com->printf("\r\n\nDual BLDC ESC type STM3 2018-20, Ver %s\r\nAt menucmd function - listing commands, source %s:-\r\n", const_version_string, a.source == SOURCE_PC ? "PC" : "TS"); + a.com->printf("\r\n\nDual BLDC ESC type STM3 2018-20, Ver %s\r\nAt menucmd function - listing commands, source %s:-\r\n", get_version(), a.source == SOURCE_PC ? "PC" : "TS"); for(int i = 0; i < a.numof_menu_items; i++) a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan); a.com->printf("End of List of Commands\r\n"); @@ -464,23 +433,28 @@ // ***** Broadcast commands for all STM3_ESC boards to execute. Boards NOT to send serial response ***** {"fw", "forward", fw_cmd}, {"re", "reverse", re_cmd}, - {"rb", "regen brake 0 to 99 %", rb_cmd}, + {"rb", "regen brake 0 to 99 %", rb_cmd}, // max two digits {"hb", "hand brake", hb_cmd}, - {"v", "set motors V percent RANGE 0 to 100", v_cmd}, - {"i", "set motors I percent RANGE 0 to 100", i_cmd}, - {"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd}, + {"v", "set motors V percent RANGE 0 to 99", v_cmd}, + {"i", "set motors I percent RANGE 0 to 99", i_cmd}, + {"vi", "set motors V and I percent RANGE 0 to 99", vi_cmd}, + {"w", "touch screen new slider touch", slider_touch_cmd}, + {"x", "updated slider RANGE 0 to 99", slider_touch_position_cmd}, + {"y", "touch_screen slider finger lifted clear", slider_untouch_cmd}, {"kd", "kick the dog, reloads WatchDog", kd_cmd}, + {"ssl", "set speed limit e.g. 10.7", ssl_cmd}, // NEW July 2019 + {"sbe", "set brake effectiveness 5 to 90 percent", brake_eff_set_cmd}, // NEW May 2020 // ***** Endof Broadcast commands for all STM3_ESC boards to execute. Boards NOT to send serial response ***** // ***** Following are rx'd requests for serial response from addressed STM3_ESC only {"?v", "Report system bus voltage", sysV_report}, {"?i", "Report motor both currents", sysI_report}, {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd}, -#ifdef TEMP_SENSOR_ENABLE - {"tem", "report temperature", temperature_cmd}, -#endif + {"tem", "report temperature", read_temperature_cmd}, {"mph", "read loco speed miles per hour", mph_cmd}, - {"ssl", "set speed limit e.g. 10.7", ssl_cmd}, // NEW July 2019 + {"?s", "read loco speed miles per hour", mph_cmd}, // Shorter-hand added 17th May 2020 +// {"ssl", "set speed limit e.g. 10.7", ssl_cmd}, // NEW July 2019 +// {"sbe", "set brake effectiveness 5 to 90 percent", brake_eff_set_cmd}, // NEW May 2020 // {"rvi", "read most recent values sent to pwms", rvi_cmd}, // {"rdi", "read motor currents and power voltage", rdi_cmd}, // ***** Endof @@ -494,10 +468,6 @@ struct kb_command const pc_command_list[] = { {"ls", "Lists available commands", menucmd}, {"?", "Lists available commands, same as ls", menucmd}, -#ifdef USING_DC_MOTORS - {"mtypes", "report types of motors found", mt_cmd}, -#endif - {"s","1-4, RPM and speed filter param", scmd}, {"pot", "read drivers control pot", pot_cmd}, {"fw", "forward", fw_cmd}, {"re", "reverse", re_cmd}, @@ -510,19 +480,18 @@ {"?i", "Report motor both currents", sysI_report}, {"?w", "show WatchDog timer contents", wd_report}, {"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd}, - {"mode", "read or set params in eeprom", mode19_cmd}, // Big change Jan 2019 + {"us", "read or set user settings in eeprom", user_settings_cmd}, // Big change Jan 2019 {"ssl", "set speed limit e.g. 10.7", ssl_cmd}, // NEW July 2019 ONLY HERE FOR TEST, normal use is from Touch Screen only. + {"sbe", "set brake effectiveness 5 to 90 percent", brake_eff_set_cmd}, // NEW May 2020 // {"erase", "set eeprom contents to all 0xff", erase_cmd}, -#ifdef TEMP_SENSOR_ENABLE - {"tem", "report temperature", temperature_cmd}, // Reports -50 when sensor not fitted -#endif + {"tem", "report temperature", read_temperature_cmd}, // Reports -50 when sensor not fitted {"kd", "kick the dog, reloads WatchDog", kd_cmd}, {"ver", "Version", ver_cmd}, - {"rcin", "Report Radio Control Input stuff", rcin_pccmd}, {"rpm", "read motor pair speeds", rpm_cmd}, {"mph", "read loco speed miles per hour", mph_cmd}, - {"rvi", "read most recent values sent to pwms", rvi_cmd}, - {"rdi", "read motor currents and power voltage", rdi_cmd}, + {"?s", "read loco speed miles per hour", mph_cmd}, +// {"rvi", "read most recent values sent to pwms", rvi_cmd}, +// {"rdi", "read motor currents and power voltage", rdi_cmd}, // {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd}, // {"gbb", "get bogie bytes from eeprom and report", gbb_cmd}, // OBSOLETE, replaced by 'gbb' {"nu", "do nothing", null_cmd}, @@ -552,8 +521,11 @@ * * Daft check that class instantiation worked */ -void cli_2019::test () { - pc.printf ("At cli2019::test, source %d len %d,\r\n", a.source, a.numof_menu_items); +void cli_2019::flush () { +// char ch; + while (a.com->readable()) + a.com->getc(); + //pc.printf ("At cli2019::test, source %d len %d,\r\n", a.source, a.numof_menu_items); } /** @@ -567,7 +539,7 @@ * Parameters available to functions from 'parameters' struct. */ void cli_2019::core () { - int ch, IAm = mode.rd(BOARD_ID); + int ch, IAm = user_settings.rd(BOARD_ID); char * pEnd;//, * cmd_line_ptr; while (a.com->readable()) { ch = a.com->getc(); @@ -594,17 +566,21 @@ for (i = 0; i < a.numof_menu_items; i++) { // Look for input match in command list wrdlen = strlen(commandlist[i].cmd_word); if(strncmp(commandlist[i].cmd_word, cmdline_ptr, wrdlen) == 0 && !isalpha(cmdline_ptr[wrdlen])) { // If match found - for (int k = 0; k < MAX_PARAMS; k++) { + for (int k = 0; k < MAX_CLI_PARAMS; k++) { a.dbl[k] = 0.0; } a.position_in_list = i; a.numof_dbls = 0; +// pEnd = cmdline + clindex; // does indeed point to null terminator pEnd = cmdline_ptr + wrdlen; - while (*pEnd) { // Assemble all numerics as doubles +// while (*pEnd) { // Assemble all numerics as doubles + while (*pEnd && a.numof_dbls < MAX_CLI_PARAMS) { // Assemble all numerics as doubles a.dbl[a.numof_dbls++] = strtod (pEnd, &pEnd); - while (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd) { + while (*pEnd && (pEnd < cmdline + clindex) && *pEnd && !isdigit(*pEnd) && ('.' != *pEnd) && ('-' != *pEnd) && ('+' != *pEnd)) { // Can crash cli here with e.g. 'ls -l' pEnd++; - } + } // problem occurs with input such as "- ", or "-a", seemingly anything dodgy following a '-' or a '+' + if (((*pEnd == '-') || (*pEnd == '+')) &&(!isdigit(*(pEnd+1))) && ('.' !=*(pEnd+1))) + pEnd = cmdline + clindex; // fixed by aborting remainder of line } a.respond = a.resp_always; if (((a.target_unit == BROADCAST) && (IAm == '0')) || (IAm == a.target_unit))
--- a/main.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/main.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -1,7 +1,7 @@ /* STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control. Jon Freeman B. Eng Hons - 2015 - 2019 + 2015 - 2020 */ #include "mbed.h" #include "STM3_ESC.h" @@ -10,11 +10,13 @@ #include "Servo.h" #include "brushless_motor.h" #include "Radio_Control_In.h" -//#ifdef TARGET_NUCLEO_F401RE // - +#include "LM75B.h" // New I2C temp sensor code March 2020 (to suit possible next board issue, harmless otherwise) +//#ifdef TARGET_NUCLEO_F401RE // Target is TARGET_NUCLEO_F401RE for all boards produced. //#endif /* Brushless_STM3_ESC board +Apr 2020 * RC tested on 'Idle Halt' branch line - all good - also tested Inverter Gen power sorce. All good. +Dec 2019 * Radio control inputs now fully implemented, requires fitting tiny 'RC_in_fix' board. Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads * Tidied brushless_motor class, parameter passing now done properly * class RControl_In created. Inputs now routed to new pins, can now use two chans both class RControl_In and Servo out @@ -29,6 +31,7 @@ LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable March 2019 temp sensor only included with TEMP_SENSOR_ENABLE defined. Temp reading not essential, LMT01 was not a good choice due to significant loading of interrupts, threatening integrity of Real Time System + *** New sensor code for LM75B temp sensor added March 2020 *** */ @@ -39,7 +42,7 @@ ____________________________________________________________________________________ CONTROL PHILOSOPHY -This STM3_ESC Bogie drive board software should ensure sensible control when commands supplied are not sensible ! +This STM3_ESC Dual Brushless Motor drive board software should ensure sensible control when commands supplied are not sensible ! That is, smooth transition between Drive, Coast and Brake to be implemented here. The remote controller must not be relied upon to deliver sensible command sequences. @@ -51,63 +54,58 @@ */ -#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP +//#if defined (TARGET_NUCLEO_L476RG) // CPU in 64 pin LQFP ** NOT PROVED ** No good, pinmap different +//#include "F401RE.h" +//#endif +#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP - This is what all produced boards have #include "F401RE.h" #endif -#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP +#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP - Never tried, but probably would work as is #include "F411RE.h" #endif #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP #include "F446ZE.h" // A thought for future version #endif /* Global variable declarations */ -char const_version_string[] = {"1.0.y2019.m09.d29\0"}; // Version string, readable from serial ports -volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US -//int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup -int WatchDog = 0; // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup -bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable -uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts + +//volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US +uint32_t WatchDog = 0, // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup + volt_reading = 0, // Global updated by interrupt driven read of Battery Volts driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US, 31250us at Sept 2019 AtoD_Semaphore = 0; +bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec -bool temp_sensor_exists = false; -double rpm2mph; +bool temp_sensor_exists = false; // March 2020 - Now used to indicate presence or not of LM75B i2c temp sensor -double Current_Scaler_Sep_2019 = 1.0; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom. - // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I - // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage - // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning - // similar current flows for shorter times at higher voltages. - +/* + * Not used since LMT01 proved not a good choice. See LM75B code added March 2020 + * #ifdef TEMP_SENSOR_ENABLE uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01 last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes -#endif -/* End of Global variable declarations */ - -Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc -Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop -#ifdef TEMP_SENSOR_ENABLE Ticker temperature_find_ticker; Timer temperature_timer; #endif -#ifdef USING_DC_MOTORS -Timer dc_motor_kicker_timer; -Timeout motors_restore; -#endif +*/ +/* End of Global variable declarations */ -RControl_In RC_chan_1 (PC_14); -RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin +Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc - 50 us +Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop + +eeprom_settings user_settings (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup +RControl_In RC_chan_1 (PC_14); +RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes. -eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup +//PCT2075 temp_sensor( i2c ); // or LM75B temp_sensor( p?, p? ); Added March 2020 +PCT2075 temp_sensor( SDA_PIN, SCL_PIN ); // or LM75B temp_sensor( p?, p? ); Added March 2020 -//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; + /* - 5 1 3 2 6 4 SENSOR SEQUENCE + 5 1 3 2 6 4 Brushless Motor Hall SENSOR SEQUENCE 1 1 1 1 0 0 0 ---___---___ Hall1 2 0 0 1 1 1 0 __---___---_ Hall2 @@ -115,36 +113,39 @@ UV WV WU VU VW UW OUTPUT SEQUENCE -8th July 2018 -Added drive to DC brushed motors. -Connect U and W to dc motor, leave V open. - -Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors +Dec 2019 Support for DC motors deleted. +Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. */ -const uint16_t A_tabl[] = { // Origial table - 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake - 0, AWHVL,AVHUL,AWHUL,AUHWL,AUHVL,AVHWL,AUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 - 0, AVHWL,AUHVL,AUHWL,AWHUL,AVHUL,AWHVL,AWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 - 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking +const uint16_t A_tabl[] = { // Table of motor energisation bit patterns. Rows are Handbrake, Forward, Reverse, Regen brake. Cols relate to Hall sensor outputs +// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH + 0, AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH, 0, // Handbrake +// 1->3 2->6 3->2 4->5 5->1 6->4 + 0, AWHVL, AVHUL, AWHUL, AUHWL, AUHVL, AVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, +// 1->5 2->3 3->1 4->6 5->4 6->2 + 0, AVHWL, AUHVL, AUHWL, AWHUL, AVHUL, AWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, + 0, BRA, BRA, BRA, BRA, BRA, BRA, BRA, // Regenerative Braking KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33] } ; - -const uint16_t B_tabl[] = { - 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake - 0, BWHVL,BVHUL,BWHUL,BUHWL,BUHVL,BVHWL,BUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 - 0, BVHWL,BUHVL,BUHWL,BWHUL,BVHUL,BWHVL,BWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 - 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking +// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH +const uint16_t B_tabl[] = { // Different tables for Motors A and B as different ports and different port bits used. +// 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake + 0, BUHVL|BWL, BWHUL|BVL, BWHVL|BUH, BVHWL|BUL, BUHWL|BVH, BVHUL|BWH, 0, // Handbrake + 0, BWHVL, BVHUL, BWHUL, BUHWL, BUHVL, BVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, + 0, BVHWL, BUHVL, BUHWL, BWHUL, BVHUL, BWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, + 0, BRB, BRB, BRB, BRB, BRB, BRB, BRB, // Regenerative Braking KEEP_L_MASK_B, KEEP_H_MASK_B } ; brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA); -brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); - +brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); // Two brushless motor instantiations extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen +static const int LM75_I2C_ADDR = 0x90; // i2c temperature sensor + // Interrupt Service Routines +/* #ifdef TEMP_SENSOR_ENABLE void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec { @@ -158,13 +159,25 @@ if (t == 6) readflag = false; } + +void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some +{ + int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ? + temperature_timer.reset (); + temp_sensor_count++; + if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading + temp_sensor_count++; +// T2 = !T2; // scope hanger +} #endif +*/ + /** void ISR_loop_timer () * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above) * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop. * Increments global 'sys_timer', usable anywhere as general measure of elapsed time */ -void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US +void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US (32Hz) { loop_flag = true; // set flag to allow main programme loop to proceed sys_timer++; // Just a handy measure of elapsed time for anything to use @@ -180,31 +193,41 @@ void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50 { AtoD_Semaphore++; - fast_sys_timer++; // Just a handy measure of elapsed time for anything to use +// fast_sys_timer++; // Just a handy measure of elapsed time for anything to use +} + +// End of Interrupt Service Routines + +const char * get_version () { + return "1.0.y2020.m05.d21\0"; // Version string, readable using 'ver' serial command } -#ifdef TEMP_SENSOR_ENABLE -void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some -{ - int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ? - temperature_timer.reset (); - temp_sensor_count++; - if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading - temp_sensor_count++; -// T2 = !T2; // scope hanger +bool read_temperature (float & t) { +// pc.printf ("test param temp = %7.3f\r\n", t); + if (!temp_sensor_exists) + return false; + t = temp_sensor; + return true; } -#endif -// End of Interrupt Service Routines - -void setVI (double v, double i) +void setVI_A (double v, double i) { MotorA.set_V_limit (v); // Sets max motor voltage MotorA.set_I_limit (i); // Sets max motor current +} + +void setVI_B (double v, double i) +{ MotorB.set_V_limit (v); MotorB.set_I_limit (i); } +void setVI_both (double v, double i) +{ + setVI_A (v, i); + setVI_B (v, i); +} + /** * void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr @@ -217,11 +240,11 @@ MotorA.high_side_off (); if (MotorB.tickleon) MotorB.high_side_off (); - if (AtoD_Semaphore > 20) { + if (AtoD_Semaphore > 10) { pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore); - AtoD_Semaphore = 20; + AtoD_Semaphore = 10; } - while (AtoD_Semaphore > 0) { + while (AtoD_Semaphore > 0) { // semaphore gets incremented in timer interrupt handler, t=50us AtoD_Semaphore--; // Code here to sequence through reading voltmeter, demand pot, ammeters switch (i) { // @@ -254,36 +277,6 @@ } } -/** double Read_Servo1_In () -* driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine -* ISR also filters signal -* This function returns normalised double (range 0.0 to 1.0) representation of driver pot position -*/ -double Read_Servo1_In () -{ - const double xjoin = 0.5, - yjoin = 0.35, - slope_a = yjoin / xjoin, - slope_b = (1.0 - yjoin)/(1.0 - xjoin); -// centre = 0.35, // For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive -// halfish = 0.5; // RC stick in the centre value - // Bottom half of RC stick movement maps to lowest (1/3)ish pot movement - // Higher half of RC stick movement maps to highest (2/3)ish pot movement - double t; - double demand = RC_chan_1.normalised(); - // apply demand = 1.0 - demand here to swap stick move polarity -// return pow (demand, SERVOIN_PWR_BENDER); - if (demand < 0.0) demand = 0.0; - if (demand > 1.0) demand = 1.0; - if (demand < xjoin) { - demand *= slope_a; - } - else { - t = yjoin + ((demand - xjoin) * slope_b); - demand = t; - } - return demand; -} /** double Read_DriverPot () * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine @@ -301,71 +294,26 @@ } -void Update_Current_Scaler () { // ***NEW Sept 2019*** Called at 8Hz -const double Vnom = 48.0, - Vmin = Vnom / 3.0, - Voff = Vnom / 4.0; - - double v = Read_BatteryVolts (); - if (v > Vnom) - v = Vnom; - if (v < Voff) - v = Voff; - if (v > Vmin) { // In expected normal operating voltage range - Current_Scaler_Sep_2019 = v / Vnom; // May need to revisit law - } - else { // In very low voltage region - Current_Scaler_Sep_2019 = 0.5 * (v / Vnom); - } +double trim (const double min, const double max, double input) { + if (input < min) input = min; + if (input > max) input = max; + return input; } -void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb +void brake_motors_both (double brake_effort) { + MotorA.brake (brake_effort); + MotorB.brake (brake_effort); +} + + +void mode_set_motors_both (int mode) // called from cli to set fw, re, rb, hb { MotorA.set_mode (mode); MotorB.set_mode (mode); - if (mode == MOTOR_REGENBRAKE) { - if (val > 1.0) - val = 1.0; - if (val < 0.0) - val = 0.0; - val *= 0.9; // set upper limit, this is essential - val = sqrt (val); // to linearise effect - setVI (val, 1.0); - } } - -#ifdef USING_DC_MOTORS -void restorer () -{ - motors_restore.detach (); - if (MotorA.dc_motor) { - MotorA.set_V_limit (MotorA.last_V); - MotorA.set_I_limit (MotorA.last_I); - MotorA.motor_set (); - } - if (MotorB.dc_motor) { - MotorB.set_V_limit (MotorB.last_V); - MotorB.set_I_limit (MotorB.last_I); - MotorB.motor_set (); - } -} -#endif - -void rcin_report () { - double c1 = RC_chan_1.normalised(); - double c2 = RC_chan_2.normalised(); - uint32_t pc1 = RC_chan_1.pulsecount(); - uint32_t pc2 = RC_chan_2.pulsecount(); - pc.printf ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2); -// if (c1 < -0.0001) - pc.printf ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth()); -// if (c2 < -0.0001) - pc.printf ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth()); -} - -bool worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes +bool is_it_worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes double c = a - b; if (c < 0.0) c = 0.0 - c; @@ -374,9 +322,9 @@ return false; } -void hand_control_state_machine (int source) { // if hand control mode '3', get here @ approx 30Hz to read drivers control pot - // if servo1 mode '4', reads input from servo1 instead -enum { // states used in hand_control_state_machine() +void hand_control_state_machine (int source) { // if hand control user_settings '3', get here @ approx 30Hz to read drivers control pot + // if servo1 user_settings '4', reads input from servo1 instead +enum { // states used in RC_or_hand_control_state_machine() HAND_CONT_IDLE, HAND_CONT_BRAKE_WAIT, HAND_CONT_BRAKE_POT, @@ -392,7 +340,7 @@ static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch static int dirbufptr = 0, direction_old = -1, direction_new = -1; const int buflen = sizeof(dirbuf) / sizeof(int); - const double Pot_Brake_Range = 0.35; //pow (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5; + double User_Brake_Range; // direction_old = direction_new; @@ -410,10 +358,9 @@ hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch switch (source) { - case 3: // driver pot is control input + case HAND: // driver pot is control input pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0 - break; - case 4: // servo 1 input is control input + User_Brake_Range = user_settings.user_brake_range(); break; default: pot_position = 0.0; @@ -427,14 +374,14 @@ case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n"); brake_effort = 0.05; // Apply braking not too fiercely - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change + brake_motors_both (brake_effort); hand_controller_state = HAND_CONT_BRAKE_POT; break; case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but - if (brake_effort < 0.9) { // remain in this state until driver has turned pott fully anticlockwise + if (brake_effort < 0.95) { // remain in this state until driver has turned pott fully anticlockwise brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change or testing at power on + brake_motors_both (brake_effort); // Direction change or testing at power on pc.printf ("Brake effort %.2f\r\n", brake_effort); } else { // once braking up to quite hard @@ -445,24 +392,23 @@ break; case HAND_CONT_STATE_INTO_BRAKING: - brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; + brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range; if (brake_effort < 0.0) brake_effort = 0.5; - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); + brake_motors_both (brake_effort); old_pot_position = pot_position; hand_controller_state = HAND_CONT_STATE_BRAKING; pc.printf ("Brake\r\n"); break; case HAND_CONT_STATE_BRAKING: - if (pot_position > Pot_Brake_Range) // escape braking into drive + if (pot_position > User_Brake_Range) // escape braking into drive hand_controller_state = HAND_CONT_STATE_INTO_DRIVING; else { - if (worth_the_bother(pot_position, old_pot_position)) { + if (is_it_worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; - brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); -// pc.printf ("Brake %.2f\r\n", brake_effort); + brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range; + brake_motors_both (brake_effort); } } break; @@ -470,20 +416,20 @@ case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING pc.printf ("Drive\r\n"); if (direction_new == 1) - mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors + mode_set_motors_both (MOTOR_FORWARD); // sets both motors else - mode_set_both_motors (MOTOR_REVERSE, 0.0); + mode_set_motors_both (MOTOR_REVERSE); hand_controller_state = HAND_CONT_STATE_DRIVING; break; case HAND_CONT_STATE_DRIVING: - if (pot_position < Pot_Brake_Range) // escape braking into drive + if (pot_position < User_Brake_Range) // escape braking into drive hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; else { - if (worth_the_bother(pot_position, old_pot_position)) { + if (is_it_worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; - drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range); - setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ??? + drive_effort = (pot_position - User_Brake_Range) / (1.0 - User_Brake_Range); + setVI_both (drive_effort, drive_effort); // Wind volts and amps up and down together ??? pc.printf ("Drive %.2f\r\n", drive_effort); } } @@ -495,68 +441,81 @@ } // endof switch (hand_controller_state) { } -int main() +void set_RCIN_offsets () { + RC_chan_1.set_offset (user_settings.rd(RCI1_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK)); + RC_chan_2.set_offset (user_settings.rd(RCI2_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK)); + RC_chan_1.set_chanmode (user_settings.rd(RCIN1), user_settings.rd(RCIN1REVERSE)) ; + RC_chan_2.set_chanmode (user_settings.rd(RCIN2), user_settings.rd(RCIN2REVERSE)) ; +} + + +int main() // Programme entry point { - int eighth_sec_count = 0; + uint32_t eighth_sec_count = 0; // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc - tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator + tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator - 50 us loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator -#ifdef TEMP_SENSOR_ENABLE - temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960); - Temperature_pin.fall (&temp_sensor_isr); - Temperature_pin.mode (PullUp); - temperature_timer.start (); -#endif + // Done setting up system interrupt timers - pc.baud (9600); // COM port to pc com3.baud (1200); // Once had an idea to use this for IR comms, never tried com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller +// pc.baud (9600); // COM port to pc + pc.baud (user_settings.baud()); // COM port to pc - rpm2mph = 60.0 // to Motor Revs per hour; - * ((double)mode.rd(MOTPIN) / (double)mode.rd(WHEELGEAR)) // Wheel revs per hour - * PI * ((double)mode.rd(WHEELDIA) / 1000.0) // metres per hour - * 39.37 / (1760.0 * 36.0); // miles per hour +// pc.printf ("Baud rate %d\r\n", user_settings.baud()); - MotorA.direction_set (mode.rd(MOTADIR)); // modes all setup in class eeprom_settings {} mode ; constructor - MotorB.direction_set (mode.rd(MOTBDIR)); - MotorA.poles (mode.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise - MotorB.poles (mode.rd(MOTBPOLES)); // Only two calls are here - MotorA.set_mode (MOTOR_REGENBRAKE); - MotorB.set_mode (MOTOR_REGENBRAKE); - setVI (0.9, 0.5); // Power up with moderate regen braking applied + MotorA.set_direction (user_settings.rd(MOTADIR)); // user_settingss all setup in class eeprom_settings {} user_settings ; constructor + MotorB.set_direction (user_settings.rd(MOTBDIR)); + MotorA.poles (user_settings.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise + MotorB.poles (user_settings.rd(MOTBPOLES)); // Only two calls are here +// MotorA.set_mode (MOTOR_REGENBRAKE); // Done in motor constructor +// MotorB.set_mode (MOTOR_REGENBRAKE); +// setVI_both (0.9, 0.5); // Power up with moderate regen braking applied + set_RCIN_offsets (); + + class RC_stick_info RCstick1, RCstick2; // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor T2 = 0; // T2, T3, T4 As yet unused pins -// T3 = 0; -// T4 = 0; -// T5 = 0; now input from fw/re on remote control box +// T3 = 0;// T4 = 0;// T5 = 0; now input from fw/re on remote control box T6 = 0; + pc.printf ("\r\n\nSTM3_ESC Starting Ver %s, Command Source setting = %d\r\n", get_version(), user_settings.rd(COMM_SRC)); + pc.printf ("Designed by Jon Freeman B. Eng. Hons - 2017-2020. e jon@jons-workshop.com\r\n"); + if (user_settings.do_we_have_i2c (0xa0)) + pc.printf ("Got EEPROM, i2c error count = %d\r\n", user_settings.errs()); + else + pc.printf ("No eeprom found\r\n"); + pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true - Ready to Roll !" : "false"); -#ifdef TEMP_SENSOR_ENABLE - if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C +//bool eeprom_settings::do_we_have_i2c (uint32_t x) + pc.printf ("LM75 temp sensor "); + if (user_settings.do_we_have_i2c (LM75_I2C_ADDR)) { + pc.printf ("reports temperature %7.3f\r\n", (float)temp_sensor ); temp_sensor_exists = true; -#endif -#ifdef USING_DC_MOTORS - dc_motor_kicker_timer.start (); -#endif - int old_hand_controller_direction = T5; - if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401 + } + else + pc.printf ("Not Fitted\r\n"); + + uint32_t old_hand_controller_direction = T5; + if (user_settings.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401 pc.printf ("Pot control\r\n"); if (T5) - mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors + mode_set_motors_both (MOTOR_FORWARD); // sets both motors else - mode_set_both_motors (MOTOR_REVERSE, 0.0); + mode_set_motors_both (MOTOR_REVERSE); } - - pc.printf ("About to start %s!, mode_bytes[COMM_SRC]= %d\r\n", const_version_string, mode.rd(COMM_SRC)); - pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true" : "false"); // pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS); -// pcc.test () ; -// tsc.test () ; WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup - + pcc.flush (); // Flush serial rx buffers + tsc.flush (); +// pc.printf ("sizeof int is %d\r\n", sizeof(int)); // sizeof int is 4 + double Current_Scaler; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom. + // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I + // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage + // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning + // similar current flows for shorter times at higher voltages. while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port @@ -566,62 +525,57 @@ } // 32Hz original setting for loop repeat rate loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms - switch (mode.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever - case 0: // Invalid - break; - case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter - break; - case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter + // double trim (const double min, const double max, double input) { + Current_Scaler = trim (0.1, 1.0, Read_BatteryVolts() / user_settings.Vnom()); + MotorA.I_scale (Current_Scaler); // Reduces motor current limits when voltage below nominal. + MotorB.I_scale (Current_Scaler); // This goes some way towards preventing engine stalls perhaps + + MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second + MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM + + RC_chan_1.read(RCstick1); // Get latest info from Radio Control inputs + RC_chan_2.read(RCstick2); + +//#define SERVO_OUT_TEST +#ifdef SERVO_OUT_TEST + if (RCstick1.active) Servo1 = RCstick1.deflection; + if (RCstick2.active) Servo2 = RCstick2.deflection; +#endif + + switch (user_settings.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever +// case 0: // Invalid +// break; +// case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter +// break; + case COM2: // COM2 - Nothing done here, all serial instructions handled in command line interpreter break; case HAND: // Put all hand controller input stuff here - hand_control_state_machine (3); + hand_control_state_machine (HAND); break; // endof hand controller stuff case RC_IN1: // RC_chan_1 - hand_control_state_machine (4); + RC_chan_1.energise (RCstick1, MotorA) ; // RC chan 1 drives both motors + RC_chan_1.energise (RCstick1, MotorB) ; break; case RC_IN2: // RC_chan_2 + RC_chan_2.energise (RCstick2, MotorA) ; // RC chan 2 drives both motors + RC_chan_2.energise (RCstick2, MotorB) ; + break; + case RC_IN_BOTH: // Robot Mode + RC_chan_1.energise (RCstick1, MotorA) ; // Two RC chans drive two motors independently + RC_chan_2.energise (RCstick2, MotorB) ; break; default: if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) { - pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here + pc.printf ("Unrecognised state %d\r\n", user_settings.rd(COMM_SRC)); // set error flag instead here ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1); } break; - } // endof switch (mode_bytes[COMM_SRC]) { - -#ifdef USING_DC_MOTORS - dc_motor_kicker_timer.reset (); -#endif - MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second - MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM + } // endof switch (user_settings_bytes[COMM_SRC]) { -#ifdef USING_DC_MOTORS - if (MotorA.dc_motor) { -// MotorA.raw_V_pwm (1); - MotorA.low_side_on (); - } - if (MotorB.dc_motor) { -// MotorB.raw_V_pwm (1); - MotorB.low_side_on (); - } - if (MotorA.dc_motor || MotorB.dc_motor) { -// motors_restore.attach_us (&restorer, ttime); - motors_restore.attach_us (&restorer, 25); - } -#endif -#define SERVO_OUT_TEST -#ifdef SERVO_OUT_TEST +//#define SERVO_OUT_TEST +//#ifdef SERVO_OUT_TEST // static double servo_angle = 0.0; // For testing servo outs // servo out test here December 2018 - - // Tests for pulse width and repetition rates being believable signal from radio control - if (RC_chan_1.validate_rx()) - Servo1 = RC_chan_1.normalised(); - if (RC_chan_2.validate_rx()) - Servo2 = RC_chan_2.normalised(); -// RC_chan_2.validate_rx(); - - /* servo_angle += 0.01; if (servo_angle > TWOPI) @@ -630,31 +584,23 @@ Servo2 = ((cos(servo_angle)+1.0) / 2.0); */ // Yep! Both servo outs work lovely December 2018 -#endif +//#endif if (flag_8Hz) { // do slower stuff flag_8Hz = false; - LED = !LED; // Toggle LED on board, should be seen to fast flash + LED = !LED; // Toggle green LED on board, should be seen to fast flash if (WatchDogEnable) { WatchDog--; if (WatchDog < 1) { // Deal with WatchDog timer timeout here WatchDog = 0; - setVI (0.0, 0.0); // set motor volts and amps to zero -// com2.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD - pc.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this + setVI_both (0.0, 0.0); // set motor volts and amps to zero + pc.printf ("TIMEOUT %c\r\n", user_settings.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this } // End of dealing with WatchDog timer timeout } - Update_Current_Scaler (); eighth_sec_count++; if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts eighth_sec_count = 0; ESC_Error.report_any (false); // retain = false - reset error after reporting once - /* if (temp_sensor_exists) { - double tmprt = (double) last_temp_count; - tmprt /= 16.0; - tmprt -= 50.0; - pc.printf ("Temp %.2f\r\n", tmprt); - }*/ } } // End of if(flag_8Hz) } // End of main programme loop
--- a/mbed.bld Sat Nov 30 18:40:30 2019 +0000 +++ b/mbed.bld Tue Jun 09 09:20:19 2020 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file