From 31171ba5f649f1236d68a7c327cde16e722d3f16 Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 8 Nov 2010 11:55:49 -0500 Subject: [PATCH 01/61] * Turning off PSU toggle on lack of toolhead reset confirmation! --- v2/src/Motherboard/Main.cc | 7 +------ v2/src/Motherboard/boards/rrmbv12/PSU.cc | 1 + 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/v2/src/Motherboard/Main.cc b/v2/src/Motherboard/Main.cc index e52cc2b..96f30ea 100644 --- a/v2/src/Motherboard/Main.cc +++ b/v2/src/Motherboard/Main.cc @@ -46,12 +46,7 @@ void reset(bool hard_reset) { } if (!tool::reset()) { - // The tool didn't acknowledge our reset! Force it off by toggling the PSU. - board.getPSU().turnOn(false); - Timeout t; - t.start(1000L*300L); // turn off for 300 ms - while (!t.hasElapsed()); - board.getPSU().turnOn(true); + // Fail, but let it go; toggling the PSU is dangerous. } } } diff --git a/v2/src/Motherboard/boards/rrmbv12/PSU.cc b/v2/src/Motherboard/boards/rrmbv12/PSU.cc index be2de8b..ba29748 100644 --- a/v2/src/Motherboard/boards/rrmbv12/PSU.cc +++ b/v2/src/Motherboard/boards/rrmbv12/PSU.cc @@ -23,6 +23,7 @@ void PSU::init() { #if defined(HAS_PSU) && HAS_PSU == 1 + PSU_PIN.setValue(false); PSU_PIN.setDirection(true); turnOn(true); #endif From 20e602da689716724873488eb35bde7caa471c4b Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 8 Nov 2010 18:04:24 -0500 Subject: [PATCH 02/61] * Multitool support --- v2/src/Extruder/Host.cc | 11 +++++------ v2/src/Extruder/SConscript | 4 ++-- v2/src/Motherboard/Command.cc | 4 ++-- v2/src/Motherboard/Host.cc | 2 +- v2/src/Motherboard/Tool.cc | 6 ++++-- v2/src/Motherboard/Tool.hh | 1 + 6 files changed, 15 insertions(+), 13 deletions(-) diff --git a/v2/src/Extruder/Host.cc b/v2/src/Extruder/Host.cc index b19b68d..6df559f 100644 --- a/v2/src/Extruder/Host.cc +++ b/v2/src/Extruder/Host.cc @@ -212,16 +212,15 @@ void runHostSlice() { } if (in.isFinished()) { out.reset(); - uint8_t slave_id = eeprom::getEeprom8(eeprom::SLAVE_ID, 0); - uint8_t target = in.read8(0); + const uint8_t slave_id = eeprom::getEeprom8(eeprom::SLAVE_ID, 0); + const uint8_t target = in.read8(0); packet_in_timeout.abort(); // SPECIAL CASE: we always process debug packets! if (processDebugPacket(in,out)) { // okay, processed - } else if (target == slave_id || target == 255) { - if (processDebugPacket(in, out)) { - // okay, processed - } else if (processQueryPacket(in, out)) { + } else if ( (target == slave_id) || (target == 255) ) { + // only process packets for us + if (processQueryPacket(in, out)) { // okay, processed } else { // Unrecognized command diff --git a/v2/src/Extruder/SConscript b/v2/src/Extruder/SConscript index 33675a4..ee74be9 100644 --- a/v2/src/Extruder/SConscript +++ b/v2/src/Extruder/SConscript @@ -50,8 +50,8 @@ if (platform == 'ecv22'): has_queue = 0 has_psu = 0 elif (platform == 'ecv34'): - default_baud = '57600' - mcu='atmega328p' + default_baud = '19200' + mcu='atmega168' has_queue = 0 has_psu = 0 else: diff --git a/v2/src/Motherboard/Command.cc b/v2/src/Motherboard/Command.cc index 1c764de..86a5bd8 100644 --- a/v2/src/Motherboard/Command.cc +++ b/v2/src/Motherboard/Command.cc @@ -137,7 +137,7 @@ void runCommandSlice() { OutPacket& out = tool::getOutPacket(); InPacket& in = tool::getInPacket(); out.reset(); - out.append8(0); // TODO: TOOL INDEX + out.append8(tool::tool_index); out.append8(SLAVE_CMD_IS_TOOL_READY); tool::startTransaction(); // WHILE: bounded by timeout in runToolSlice @@ -170,7 +170,7 @@ void runCommandSlice() { } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { command_buffer.pop(); // remove the command code - uint8_t tool_index = command_buffer.pop(); + tool::tool_index = command_buffer.pop(); } } else if (command == HOST_CMD_ENABLE_AXES) { if (command_buffer.getLength() >= 2) { diff --git a/v2/src/Motherboard/Host.cc b/v2/src/Motherboard/Host.cc index d84f8b4..69d68fa 100644 --- a/v2/src/Motherboard/Host.cc +++ b/v2/src/Motherboard/Host.cc @@ -230,7 +230,7 @@ void doToolPause(OutPacket& to_host) { OutPacket& out = tool::getOutPacket(); InPacket& in = tool::getInPacket(); out.reset(); - out.append8(0); // TODO: current tool + out.append8(tool::tool_index); out.append8(SLAVE_CMD_PAUSE_UNPAUSE); // Timeouts are handled inside the toolslice code; there's no need // to check for timeouts on this loop. diff --git a/v2/src/Motherboard/Tool.cc b/v2/src/Motherboard/Tool.cc index 070cf38..bb28d33 100644 --- a/v2/src/Motherboard/Tool.cc +++ b/v2/src/Motherboard/Tool.cc @@ -41,6 +41,8 @@ uint8_t retries = RETRIES; Timeout timeout; +uint8_t tool_index = 0; + bool reset() { // This code is very lightly modified from handleToolQuery in Host.cc. // We don't give up if we fail to get a lock; we force it instead. @@ -57,7 +59,7 @@ bool reset() { OutPacket& out = getOutPacket(); InPacket& in = getInPacket(); out.reset(); - out.append8(0); // TODO: tool index + out.append8(255); // Reset all tools out.append8(SLAVE_CMD_INIT); startTransaction(); // override standard timeout @@ -65,7 +67,7 @@ bool reset() { releaseLock(); // WHILE: bounded by tool timeout while (!isTransactionDone()) { - runToolSlice(); + runToolSlice(); // This will most likely time out if there's multiple toolheads. } return Motherboard::getBoard().getSlaveUART().in.isFinished(); } diff --git a/v2/src/Motherboard/Tool.hh b/v2/src/Motherboard/Tool.hh index e9b54f7..29e4aac 100644 --- a/v2/src/Motherboard/Tool.hh +++ b/v2/src/Motherboard/Tool.hh @@ -78,6 +78,7 @@ InPacket& getInPacket(); */ bool reset(); +extern uint8_t tool_index; } #endif // TOOL_HH_ From aeeba97b59d69e48aeb1bde653db35964b7173eb Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Wed, 10 Nov 2010 16:57:05 -0500 Subject: [PATCH 03/61] bugfixes: uninitialized index variable --- v2/src/Extruder/Host.cc | 2 +- v2/src/Motherboard/Host.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/v2/src/Extruder/Host.cc b/v2/src/Extruder/Host.cc index 6df559f..9c5f936 100644 --- a/v2/src/Extruder/Host.cc +++ b/v2/src/Extruder/Host.cc @@ -88,7 +88,7 @@ bool processQueryPacket(const InPacket& from_host, OutPacket& to_host) { case SLAVE_CMD_GET_BUILD_NAME: to_host.append8(RC_OK); { - for (uint8_t idx; idx < 31; idx++) { + for (uint8_t idx = 0; idx < 31; idx++) { to_host.append8(build_name[idx]); if (build_name[idx] == '\0') { break; } } diff --git a/v2/src/Motherboard/Host.cc b/v2/src/Motherboard/Host.cc index 69d68fa..25d5403 100644 --- a/v2/src/Motherboard/Host.cc +++ b/v2/src/Motherboard/Host.cc @@ -142,7 +142,7 @@ inline void handleVersion(const InPacket& from_host, OutPacket& to_host) { inline void handleGetBuildName(const InPacket& from_host, OutPacket& to_host) { to_host.append8(RC_OK); - for (uint8_t idx; idx < 31; idx++) { + for (uint8_t idx = 0; idx < 31; idx++) { to_host.append8(build_name[idx]); if (build_name[idx] == '\0') { break; } } From 0b8bcc549262d4739881484a9342b7ca405a2708 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Tue, 16 Nov 2010 21:28:20 -0600 Subject: [PATCH 04/61] Fix for external stepper extruder not stopping on reset or stop. --- v2/src/Extruder/boards/ecv22/ExtruderBoard.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc index 2048bb6..79646f6 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc @@ -115,7 +115,6 @@ void ExtruderBoard::reset() { platform_thermistor.init(); extruder_heater.reset(); platform_heater.reset(); - setMotorSpeed(0); getHostUART().enable(true); getHostUART().in.reset(); @@ -162,6 +161,10 @@ void ExtruderBoard::reset() { #else setUsingRelays(false); #endif + + // init after we know what kind of motor we're using + setMotorSpeed(0); + setMotorSpeedRPM(0); } void ExtruderBoard::setMotorSpeed(int16_t speed) { From 922a9e1ed57cadc4bb022d453947b1abb5f7c93a Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 8 Nov 2010 18:56:19 -0500 Subject: [PATCH 05/61] * Bumping version no., adding bootloader to dist dir --- dist/ECv3.6/ATmegaBOOT_168_ec3x.hex | 125 ++++++++++++++++++++++++++++ v2/src/Extruder/Version.hh | 2 +- 2 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 dist/ECv3.6/ATmegaBOOT_168_ec3x.hex diff --git a/dist/ECv3.6/ATmegaBOOT_168_ec3x.hex b/dist/ECv3.6/ATmegaBOOT_168_ec3x.hex new file mode 100644 index 0000000..0724c8e --- /dev/null +++ b/dist/ECv3.6/ATmegaBOOT_168_ec3x.hex @@ -0,0 +1,125 @@ +:103800000C94341C0C94511C0C94511C0C94511CA1 +:103810000C94511C0C94511C0C94511C0C94511C74 +:103820000C94511C0C94511C0C94511C0C94511C64 +:103830000C94511C0C94511C0C94511C0C94511C54 +:103840000C94511C0C94511C0C94511C0C94511C44 +:103850000C94511C0C94511C0C94511C0C94511C34 +:103860000C94511C0C94511C11241FBECFEFD4E0BA +:10387000DEBFCDBF11E0A0E0B1E0EAE9FFE302C0A6 +:1038800005900D92A230B107D9F712E0A2E0B1E0A5 +:1038900001C01D92AD30B107E1F70E942D1D0C94BF +:1038A000CB1F0C94001C982F959595959595959503 +:1038B000905D8F708A307CF0282F295A8091C0004B +:1038C00085FFFCCF9093C6008091C00085FFFCCFA0 +:1038D0002093C6000895282F205DF0CF982F809167 +:1038E000C00085FFFCCF9093C6000895EF92FF9231 +:1038F0000F931F93EE24FF2487018091C00087FD62 +:1039000017C00894E11CF11C011D111D81E4E8168B +:1039100082E4F8068FE0080780E0180770F3E09172 +:103920000401F091050109958091C00087FFE9CF5E +:103930008091C6001F910F91FF90EF9008950E9413 +:10394000761C982F8091C00085FFFCCF9093C60015 +:1039500091362CF490330CF09053892F089597559D +:10396000892F08951F930E949F1C182F0E949F1C4F +:103970001295107F810F1F9108951F93182F882390 +:1039800021F00E94761C1150E1F71F9108951F93BA +:10399000182F0E94761C803249F0809103018F5FBE +:1039A000809303018530C1F01F9108958091C0007C +:1039B00085FFFCCF84E18093C6008091C00085FF25 +:1039C000FCCF1093C6008091C00085FFFCCF80E142 +:1039D0008093C6001F910895E0910401F0910501C4 +:1039E00009951F9108950E94761C803241F08091C4 +:1039F00003018F5F80930301853081F008958091EA +:103A0000C00085FFFCCF84E18093C6008091C00098 +:103A100085FFFCCF80E18093C6000895E09104010A +:103A2000F09105010995089540E951E08823A1F03E +:103A30002D9A28EE33E0FA013197F1F7215030400A +:103A4000D1F72D9828EE33E0FA013197F1F72150A4 +:103A50003040D1F7815061F708953F924F925F92C5 +:103A60006F927F928F929F92AF92BF92CF92DF928E +:103A7000EF92FF920F931F93CF93DF93000085B1D6 +:103A8000897F85B984B1866084B98BB18F798BB910 +:103A90008AB180668AB983E38093C4001092C5001E +:103AA00088E18093C10086E08093C20050985898C6 +:103AB000259A82E00E94141D24E1F22E9EE1E92E57 +:103AC00084E9D82E06E0C02E10E1B12EAA24A394DA +:103AD000B1E49B2EA6E58A2EF2E57F2EE0E26E2E63 +:103AE00079E4572E63E5462E50E5352E0E94761C6C +:103AF0008033B1F18133B9F1803409F46FC081347E +:103B000009F476C0823409F485C0853409F488C08C +:103B1000803531F1823521F1813511F1853509F496 +:103B200085C0863509F48DC0843609F496C0843783 +:103B300009F403C1853709F471C1863709F466C0F9 +:103B4000809103018F5F80930301853079F6E091C6 +:103B50000401F091050109950E94761C803351F60D +:103B60000E94F31CC3CF0E94761C803249F78091DB +:103B7000C00085FFFCCFF092C6008091C00085FF99 +:103B8000FCCF9092C6008091C00085FFFCCF809250 +:103B9000C6008091C00085FFFCCF7092C600809166 +:103BA000C00085FFFCCF6092C6008091C00085FFF9 +:103BB000FCCF5092C6008091C00085FFFCCF4092A0 +:103BC000C6008091C00085FFFCCF3092C600809176 +:103BD000C00085FFFCCFB092C60088CF0E94761C43 +:103BE000863808F4BDCF0E94761C0E94F31C7ECF5D +:103BF0000E94761C803809F49CC0813809F40AC1FF +:103C0000823809F42FC1883909F48FC080E00E94FE +:103C1000C71C6CCF84E10E94BD1C0E94F31C66CFC0 +:103C200085E00E94BD1C0E94F31C60CF0E94761CA0 +:103C3000809306010E94761C809307010E94F31C6A +:103C400055CF0E94761C803309F410C183E00E9496 +:103C5000BD1C80E00E94C71C49CF0E94761C809347 +:103C600009020E94761C8093080280910C028E7FCC +:103C700080930C020E94761C853409F408C180915F +:103C8000080290910902892B89F000E010E00E945F +:103C9000761CF801E85FFE4F80830F5F1F4F809115 +:103CA0000802909109020817190788F30E94761CF0 +:103CB000803209F045CF80910C0280FFF4C0609102 +:103CC000060170910701660F771F709307016093DB +:103CD0000601A0910802B09109021097C9F0E8E02E +:103CE000F1E09B01AD014E0F5F1FF999FECF32BD90 +:103CF00021BD819180BDFA9AF99A2F5F3F4FE41759 +:103D0000F50799F76A0F7B1F70930701609306010F +:103D10008091C00085FFFCCFF092C6008091C0006A +:103D200085FFFCCFB092C600E1CE83E00E94C71CA5 +:103D3000DDCE82E00E94C71CD9CE0E94761C809303 +:103D400009020E94761C80930802809106019091DE +:103D50000701880F991F90930701809306010E9425 +:103D6000761C853409F499C080910C028E7F809373 +:103D70000C020E94761C803209F0B8CE8091C000FF +:103D800085FFFCCFF092C600A0910802B091090215 +:103D90001097B9F180910C02182F1170082F027042 +:103DA000E0910601F0910701AF014F5F5F4FBA014B +:103DB00020E030E01123B1F4002339F49491809194 +:103DC000C00085FFFCCF9093C6002F5F3F4FCB0113 +:103DD0000196FA012A173B0780F4BC014F5F5F4F41 +:103DE000112351F3F999FECFF2BDE1BDF89A90B5D8 +:103DF0008091C00085FFFCCFE6CF709307016093F0 +:103E000006018091C00085FDE6CE8091C00085FF4F +:103E1000F8CFE1CE81E00E94C71C68CE0E94761CDC +:103E2000803209F08DCE8091C00085FFFCCFF092EA +:103E3000C6008091C00085FFFCCFE092C600809153 +:103E4000C00085FFFCCFD092C6008091C00085FFE6 +:103E5000FCCFC092C6008091C00085FFFCCFB0921D +:103E6000C60044CE80E10E94C71C40CE0E94761C52 +:103E70000E94761C182F0E94761C112309F483C01F +:103E8000113009F484C086E00E94C71C2FCE8091B7 +:103E90000C02816080930C02F2CE80910C02816052 +:103EA00080930C0266CF809107018823880F880BCE +:103EB0008A2180930B028091060190910701880F5F +:103EC000991F90930701809306018091080280FF5B +:103ED00009C080910802909109020196909309020D +:103EE00080930802F894F999FECF1127E09106011A +:103EF000F0910701C8E0D1E0809108029091090299 +:103F0000103091F40091570001700130D9F303E0B3 +:103F100000935700E8950091570001700130D9F3E4 +:103F200001E100935700E89509901990009157001E +:103F300001700130D9F301E000935700E895139523 +:103F4000103498F011270091570001700130D9F317 +:103F500005E000935700E89500915700017001308B +:103F6000D9F301E100935700E8953296029709F0E2 +:103F7000C7CF103011F00296E5CF11248091C00018 +:103F800085FFC6CEC9CE8EE10E94C71CAFCD84E9A5 +:0A3F90000E94C71CABCDF894FFCFD0 +:023F9A008000A5 +:0400000300003800C1 +:00000001FF diff --git a/v2/src/Extruder/Version.hh b/v2/src/Extruder/Version.hh index bdd93fd..18e95ff 100644 --- a/v2/src/Extruder/Version.hh +++ b/v2/src/Extruder/Version.hh @@ -22,7 +22,7 @@ #ifndef VERSION -const uint16_t firmware_version = 205; +const uint16_t firmware_version = 206; #else const uint16_t firmware_version = VERSION; #endif From 54005180112012a63f03c22bdf1e9f8563e9f2d4 Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 15 Nov 2010 16:29:51 -0500 Subject: [PATCH 06/61] * Commented out LCD hello world --- v2/src/Motherboard/boards/rrmbv24/Motherboard.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/v2/src/Motherboard/boards/rrmbv24/Motherboard.cc b/v2/src/Motherboard/boards/rrmbv24/Motherboard.cc index 8a41856..b3fa320 100644 --- a/v2/src/Motherboard/boards/rrmbv24/Motherboard.cc +++ b/v2/src/Motherboard/boards/rrmbv24/Motherboard.cc @@ -81,14 +81,14 @@ void Motherboard::reset() { TIMSK2 = 0x01; // OVF flag on // Configure the debug pin. DEBUG_PIN.setDirection(true); - lcd.begin(16,4); - lcd.clear(); - lcd.home(); - lcd.write('H'); - lcd.write('e'); - lcd.write('l'); - lcd.write('l'); - lcd.write('o'); +// lcd.begin(16,4); +// lcd.clear(); +// lcd.home(); +// lcd.write('H'); +// lcd.write('e'); +// lcd.write('l'); +// lcd.write('l'); +// lcd.write('o'); } /// Get the number of microseconds that have passed since From c86be066cd033d983bafe7c4a5cad1be7c349b68 Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 15 Nov 2010 16:30:40 -0500 Subject: [PATCH 07/61] * Adding production images for new extruder controller --- dist/ECv3.6/PRODUCTION.hex | 510 +++++++++++++++++++++++++++++++++++++ dist/ECv3.6/burn.sh | 47 ++++ 2 files changed, 557 insertions(+) create mode 100644 dist/ECv3.6/PRODUCTION.hex create mode 100755 dist/ECv3.6/burn.sh diff --git a/dist/ECv3.6/PRODUCTION.hex b/dist/ECv3.6/PRODUCTION.hex new file mode 100644 index 0000000..b11e907 --- /dev/null +++ b/dist/ECv3.6/PRODUCTION.hex @@ -0,0 +1,510 @@ +:200000000C9484000C94AC000C94AC000C94AC000C94AC000C94AC000C94AC000C940A0E3C +:200020000C94AC000C94AC000C94AC000C94AC000C94AC000C94AC000C94AC000C94AC0060 +:200040000C94AC000C94AC000C949A110C94AC000C9447110C94A90D0C94AC000C94AC008B +:200060000C94AC000C94AC001B093B093F094609B6093C0A3C0A3C0AC1093C0ACE093C0A2B +:20008000E809F509240A3C0A3C0A3C0A3C0A3C0A3C0A3C0A010AAF093C0A4E0974093C0A36 +:2000A0003C0A3C0A0C0A150A270A2E0A2309010049033600FF006B00D100A000B800D500FF +:2000C000A6000A0199003F018E0074018400A9017C00DE01740013026C00480265007D02E7 +:2000E0005D00B2025600E7024E001C03460051033D0086033200BB032200F0030300E808EB +:20010000500C4B0FED11F21111241FBECFEFD4E0DEBFCDBF12E0A0E0B1E0EAE5FAE202C00B +:2001200005900D92AA32B107D9F713E0AAE2B2E001C01D92A935B107E1F710E0C8E0D1E0EF +:2001400004C02297FE010E94F614CE3FD107C9F70E94FB0A0C942B150C940000A0E0B0E09B +:20016000E4EBF0E00C94BD14DC012B01FA019C91923008F439C1EB018881823008F433C1EF +:20018000943069F4843009F02FC111969C9111978981981709F428C1A9E0B1E025C18430D2 +:2001A00009F421C18230A9F4923009F01DC19A01AD0188E0EA010990AE01E90109929E0170 +:2001C0008150C1F7E201898111969C918923818308C1923009F407C112962D903C901397FA +:2001E000EB018A819B811496AD90BD90CD90DC901797EC80FD800E811F819101281B390B10 +:20020000B90137FF04C066277727621B730B603271050CF061C0121613066CF537014801BC +:20022000062E04C096948794779467940A94D2F721E030E040E050E004C0220F331F441F09 +:20024000551F6A95D2F721503040404050402E213F2140235123211531054105510521F0D2 +:2002600021E030E040E050E079018A01E628F728082919293CC0232BD1F1260E371E3501AD +:200280004601062E04C096948794779467940A94D2F721E030E040E050E004C0220F331FC5 +:2002A000441F551F6A95D2F721503040404050402A213B214C215D21211531054105510514 +:2002C00021F021E030E040E050E059016A01A628B728C828D9280BC0821593052CF01C0116 +:2002E000AA24BB24650103C0EE24FF24870111969C91D20111968C91981709F445C0992393 +:2003000039F0A80197012A193B094C095D0906C0A60195012E193F09400B510B57FD08C0DC +:2003200011823382228224833583468357831DC081E081833382228288279927DC01821B4B +:20034000930BA40BB50B84839583A683B7830DC0220F331F441F551F2483358346835783E5 +:20036000828193810197938382832481358146815781DA01C9010197A109B1098F5F9F4F3C +:20038000AF4FBF4328F30BC0918333822282EA0CFB1C0C1D1D1DE482F5820683178383E037 +:2003A0008083248135814681578157FF1AC0C901AA2797FDA095BA2F81709070A070B070A2 +:2003C0005695479537952795822B932BA42BB52B84839583A683B78382819381019693836E +:2003E0008283DF0101C0D201CD01CDB7DEB7E2E10C94D914A0E2B0E0E0E0F2E00C94C9142C +:2004000069837A838B839C832D833E834F835887E9E0EE2EF12CEC0EFD1ECE010196B7016F +:200420000E94D1058E010F5E1F4FCE010596B8010E94D1058A8991E089278A8BC701B80175 +:20044000AE01475E5F4F0E94AE000E94FC04A096E6E00C94E514A0E2B0E0E1E3F2E00C94CB +:20046000C91469837A838B839C832D833E834F835887F9E0EF2EF12CEC0EFD1ECE010196D9 +:20048000B7010E94D1058E010F5E1F4FCE010596B8010E94D105C701B801AE01475E5F4FA4 +:2004A0000E94AE000E94FC04A096E6E00C94E514A0E2B0E0EEE5F2E00C94BD1469837A83A4 +:2004C0008B839C832D833E834F835887CE010196BE01675F7F4F0E94D105CE010596BE0173 +:2004E0006F5E7F4F0E94D1059985923088F089898230C8F0943019F4823051F404C08430D5 +:2005000029F4923081F489E091E0C6C0923049F420E09A858A89981321E02A87CE0109962B +:20052000BBC0823049F420E09A858A89981321E02A8BCE014196B0C02D843E844F84588882 +:200540006D887E888F88988CEE24FF248701AA24BB24650140E050E060E070E0E0E0F0E025 +:20056000C10181709070892BE9F0E60CF71C081D191D9A01AB012A0D3B1D4C1D5D1D80E0C2 +:2005800090E0A0E0B0E0E614F7040805190520F481E090E0A0E0B0E0BA01A901480F591F92 +:2005A0006A1F7B1FAA0CBB1CCC1CDD1C97FE08C081E090E0A0E0B0E0A82AB92ACA2ADB2ABE +:2005C0003196E032F10549F0660C771C881C991C5694479437942794C3CFFA85EA892B8932 +:2005E0003C898B859C85280F391F2E5F3F4F17C0CA0181709070892B61F016950795F794F1 +:20060000E79480E090E0A0E0B0E8E82AF92A0A2B1B2B76956795579547952F5F3F4F77FD68 +:20062000E7CF0CC0440F551F661F771F17FD4160EE0CFF1C001F111F21503040403090E07C +:20064000590790E0690790E4790760F32B8F3C8FDB01CA018F779070A070B0708034910567 +:20066000A105B10561F447FD0AC0E114F1040105110529F0405C5F4F6F4F7F4F40781A8E66 +:20068000FE1711F081E08A8F4D8F5E8F6F8F78A383E0898FCE0149960E94FC04A096E2E124 +:2006A0000C94D914A8E1B0E0E8E5F3E00C94C91469837A838B839C832D833E834F8358873F +:2006C00089E0E82EF12CEC0EFD1ECE010196B7010E94D1058E010F5E1F4FCE010596B80146 +:2006E0000E94D1058985823040F08989823028F0C701B8010E94490601C081E06896E6E05E +:200700000C94E514A8E0B0E0E8E8F3E00C94C6149B01AC0183E08983DA01C9018827B7FD4B +:2007200083959927AA27BB27B82E211531054105510519F482E089833AC08823A9F0203037 +:2007400080E0380780E0480780E8580729F460E070E080E09FEC30C0EE24FF248701E21A43 +:20076000F30A040B150B02C079018A018EE1C82ED12CDC82CB82ED82FE820F831887C80190 +:20078000B7010E94AD0401971816190684F4082E04C0EE0CFF1C001F111F0A94D2F7ED82BD +:2007A000FE820F831887C81AD90ADC82CB82BA82CE0101960E94FC042896E9E00C94E214B7 +:2007C000ACE0B0E0E6EEF3E00C94CD1469837A838B839C83CE010196BE016B5F7F4F0E9460 +:2007E000D1058D81823061F1823050F1843021F48E81882351F12EC02F81388537FD20C0EA +:200800006E812F3131051CF06623F9F023C08EE190E0821B930B29853A854B855C8504C0F6 +:2008200056954795379527958A95D2F76623B1F050954095309521953F4F4F4F5F4F0EC07A +:2008400020E030E040E050E009C02FEF3FEF4FEF5FE704C020E030E040E050E8B901CA01EE +:200860002C96E2E00C94E914A8E0B0E0EAE3F4E00C94C5147B018C01611571058105910514 +:2008800019F482E0898360C083E089838EE1C82ED12CDC82CB82ED82FE820F831887C80158 +:2008A000B7010E94AD04FC013197F7FF3BC0222733272E1B3F0B57016801022E04C0D69422 +:2008C000C794B794A7940A94D2F740E050E060E070E081E090E0A0E0B0E004C0880F991FFC +:2008E000AA1FBB1F2A95D2F70197A109B1098E219F21A023B1230097A105B10521F041E0A6 +:2009000050E060E070E04A295B296C297D294D835E836F8378878EE190E08E1B9F0B9C83F2 +:200920008B8312C0309781F00E2E04C0EE0CFF1C001F111F0A94D2F7ED82FE820F831887B4 +:20094000CE1ADF0ADC82CB821A82CE0101960E94FC042896EAE00C94E114EF92FF920F93A6 +:200960001F937B018C0180E0E81680E0F80681E0080780E0180788F48FEFE816F10401051E +:20098000110531F028F088E090E0A0E0B0E017C080E090E0A0E0B0E012C080E0E81680E0D9 +:2009A000F80680E0080781E0180728F088E190E0A0E0B0E004C080E190E0A0E0B0E020E2A2 +:2009C00030E040E050E0281B390B4A0B5B0B04C016950795F794E7948A95D2F7F701EF5E37 +:2009E000FE4F8081281B310941095109C9011F910F91FF90EF900895DF92EF92FF920F939E +:200A00001F93FC01E480F58006811781D1808081823048F480E090E0A0E1B0E0E82AF92AD9 +:200A20000A2B1B2BA5C0843009F49FC0823021F4EE24FF24870105C0E114F1040105110577 +:200A400019F4E0E0F0E096C0628173819FEF623879070CF05BC022E83FEF261B370B2A31F7 +:200A600031052CF020E030E040E050E02AC0B801A701022E04C076956795579547950A9418 +:200A8000D2F781E090E0A0E0B0E004C0880F991FAA1FBB1F2A95D2F70197A109B1098E21C3 +:200AA0009F21A023B1230097A105B10521F081E090E0A0E0B0E09A01AB01282B392B4A2B87 +:200AC0005B2BDA01C9018F779070A070B07080349105A105B10539F427FF09C0205C3F4FE9 +:200AE0004F4F5F4F04C0215C3F4F4F4F5F4FE0E0F0E02030A0E03A07A0E04A07A0E45A0738 +:200B000010F0E1E0F0E079018A0127C06038710564F5FB01E158FF4FD801C7018F779070C7 +:200B2000A070B07080349105A105B10539F4E7FE0DC080E490E0A0E0B0E004C08FE390E076 +:200B4000A0E0B0E0E80EF91E0A1F1B1F17FF05C016950795F794E794319687E01695079578 +:200B6000F794E7948A95D1F705C0EE24FF248701EFEFF0E06E2F679566276795902F9F7761 +:200B8000D794DD24D7948E2F8695492F462B582F5D29B701CA011F910F91FF90EF90DF9060 +:200BA0000895FC01DB01408151812281622F6F7770E0221F2227221F9381892F880F822BE7 +:200BC000282F30E0991F9927991F11969C93119721153105A9F5411551056105710511F469 +:200BE00082E037C082E89FEF13969C938E9312979A01AB0167E0220F331F441F551F6A951B +:200C0000D1F783E08C930DC0220F331F441F551F12968D919C911397019713969C938E9335 +:200C20001297203080E0380780E0480780E4580758F314962D933D934D935C93179708950B +:200C40002F3F310579F4411551056105710519F484E08C93089564FF03C081E08C9312C056 +:200C60001C9210C02F57304013963C932E93129783E08C9387E0440F551F661F771F8A9534 +:200C8000D1F7706414964D935D936D937C93179708951F93DC01FB019C91923008F447C067 +:200CA0008081823008F443C0943051F411961C91843099F58181682F70E0611B71093FC005 +:200CC000843021F0923031F48230B9F18181882389F12DC011961C9111978230F1F0818167 +:200CE0001817D9F412962D913C911397828193818217930794F028173907BCF014968D915A +:200D00009D910D90BC91A02D2481358146815781281739074A075B0718F4112341F00AC087 +:200D200082179307A407B50740F4112319F061E070E005C06FEF7FEF02C060E070E0CB0168 +:200D40001F910895CF92DF92EF92FF920F931F93CF93DF938C017B01FC018081882351F146 +:200D60006801C80160E00E94B813982F8077803709F59037B1F4C7010E94D713C1E0D0E010 +:200D800009C0C6016C2F0E94B813682FC7010E9425142196F801808190E0C817D9078CF322 +:200DA00007C0913739F0923729F080E0933719F481E001C080E0DF91CF911F910F91FF9031 +:200DC000EF90DF90CF900895EF92FF920F931F93DF93CF9300D0CDB7DEB79C01F42E052F13 +:200DE000162FE72ECE010196B90142E050E00E94031569816F3F19F48A818F3FE9F070E0C6 +:200E000080E090E00E9434047B018C016A8170E080E090E00E94340420E030E040E85BE3C4 +:200E20000E9458029B01AC01C801B7010E942B02F62E072F182FE92E2F2D302F412F5E2DAF +:200E4000B901CA010F900F90CF91DF911F910F91FF90EF9008950F931F93DF93CF9300D00C +:200E6000CDB7DEB79C018B01CE010196B90142E050E00E94031589819A818F5F9F4F11F4FE +:200E80001A83098389819A810F900F90CF91DF911F910F9108951F93DF93CF930F92CDB75E +:200EA000DEB79C01162FCE010196B90141E050E00E94031589818F3F09F4198389810F9076 +:200EC000CF91DF911F910895DF93CF9300D0CDB7DEB7CE01019660E070E042E050E00E944E +:200EE00003153A8184E6389FC00111242981820F911D8E3C9105A1F032503D3F28F082E096 +:200F000090E06BE10E941D1586E0898382E08A83CE01019660E070E042E050E00E9413154E +:200F20000F900F90CF91DF910895FC0176AF65AF0895FC01448D558D25AD36ADC90102976B +:200F40004817590714F480E0089580E02E5F3F4F241735070CF081E00895FC0185AD96AD6F +:200F60000895FC010190F081E02D22813381C90108950F931F93CF93DF93EC018C01025F07 +:200F80001F4FC8010E943D0D882391F0E881F981A081B1812D913C91CF01F901099588232E +:200FA000E1F1488D598D6A8D7B8DC8010E94620D8E010C5F1F4FC8010E943D0D882369F1A9 +:200FC000C80140E251EA67E070E00E94620DE881F981228133812C8F3D8FCE018096B901E3 +:200FE0000E943112BC0197FF03C060E070E006C08F3F910519F014F06FEF70E08DAD9EADFC +:20100000892B11F460E070E0EA81FB81A081B1812D913C91CF01F9010995DF91CF911F91DA +:201020000F9108956F927F928F929F92AF92BF92CF92DF92EF92FF920F931F93CF93DF9371 +:20104000EC011D8E1C8E8E8D9F8D40E050E060EE70E40E94E4063B014C018E8D9F8D029621 +:2010600046E656E666EA7EE30E94E4065B016C018E8D9F8D049640E050E060E172E40E9498 +:20108000E4067B018C01CE0180960E94FC11C401B30120E030E040E050E00E94520388234E +:2010A00091F5C601B50120E030E040E050E00E945203882341F5C801B70120E030E040E044 +:2010C00050E00E9452038823F1F40F2EF0E06F2EF0E07F2EF0EE8F2EF0E49F2EF02D0F2E9C +:2010E000F6E6AF2EF6E6BF2EF6EACF2EFEE3DF2EF02D0F2EF0E0EF2EF0E0FF2EF0E10F2F50 +:20110000F2E41F2FF02D68A279A28AA29BA2ACA2BDA2CEA2DFA2E8A6F9A60AA71BA71EAEF6 +:201120001DAECE01049640E251EA67E070E00E94620D488D598D6A8D7B8DCE010E960E94A7 +:20114000620DDF91CF911F910F91FF90EF90DF90CF90BF90AF909F908F907F906F9008959D +:20116000AF92BF92CF92DF92EF92FF920F931F93CF93DF93EC0158016901798368835B835C +:201180004A8304960E94360DCE010E960E94360DA88EB98ECA8EDB8EFF8EEE8ECE0180967F +:2011A0000E94FC118E8D9F8D892B21F48CE090E09F8F8E8FCE010E941208DF91CF911F913E +:2011C0000F91FF90EF90DF90CF90BF90AF9008958AE292E00E94360D08957F928F929F92A5 +:2011E000AF92BF92CF92DF92EF92FF920F931F93DF93CF93CDB7DEB760970FB6F894DEBF53 +:201200000FBECDBFFC016B018081882309F434C17F01CF0161E00E94B813E82FF0E0E33274 +:20122000F10508F029C1EC5CFF4FEE0FFF1F0590F491E02D0994C60161E00E942514C601B7 +:201240006EEC70E015C1C60161E00E9425140FC00E2D10E000501F4FC601F80160810E9430 +:201260002514F8018081882309F404C1E394FEE1FE1570F7FFC081E08093340274C0C6019A +:2012800061E00E94251484E892E0CCC0C70162E00E94BD13BC0184E892E0D3C0C70162E014 +:2012A0000E94BD135C01C70164E00E94B813813148F5E82EFF248E010F5F1F4FC801B501D4 +:2012C000A7010E940315C60161E00E942514E00EF11E06C0C601F80161918F010E942514E9 +:2012E0000E151F05B9F7C6C0C70162E00E94BD13882E792EC70164E00E94B813982EF0E188 +:20130000F81728F4C60162E00E942514B3C08E010F5F1F4FC801682D772D492D50E00E9496 +:201320000315580115E0092D0B5F08C0C701612F0E94B813F50181935F011F5F1017B1F763 +:20134000CE010196682D772D492D50E00E941315C60161E00E942514C601692DD5CF85E332 +:2013600092E00E940A0BC60161E0CECFC70162E00E94B813682F85E392E070E00E94060BB4 +:20138000F2CFC70162E00E94B81360E0813009F461E085E392E00E94100BE5CFC70162E091 +:2013A0000E94B813682F70E076956795617085E392E00E94100BC70162E00E94B813682F5C +:2013C00070E06170707085E392E00E94130BCBCFC70162E00E94B813682F70E061707070C9 +:2013E00085E692E00E94DD0DBECFC70162E00E94B813682F70E06170707085E692E00E9469 +:20140000EB0DC60161E00E94251484E892E00E949907682FC60178CFC60161E00E94251449 +:2014200083EC92E00E94B10721C085E692E061E00E94050EC70162E00E94BD13BC0183EC15 +:2014400092E00E9495078FCFC60165E05DCFC60161E00E94251484E892E006C0C60161E0B7 +:201460000E94251483EC92E00E94AD07BC01C6010E94411481E001C080E060960FB6F89416 +:20148000DEBF0FBECDBFCF91DF911F910F91FF90EF90DF90CF90BF90AF909F908F907F906F +:2014A00008950F931F938091530390915403892B09F489C08091530390915403049709F0EF +:2014C00088C081C0109234020E94EC0A80912D0390912E03892B21F180912D0390912E0327 +:2014E0000497F1F080912A02882349F48AE292E040E25EE460E070E00E94620D11C08AE22B +:2015000092E00E943D0D882359F08AE093E00E941D1384E080932C0389E093E00E94301165 +:2015200080912C03882361F08AE292E00E943A0D8AE093E00E942B1389E093E00E9430112C +:2015400080912D0390912E03049709F042C080E393E00E94D7138AE190E060E00E944B07F1 +:20156000082F8AE093E060E00E94B813182F8AE292E00E943A0D8AE093E060E373E00E9487 +:20158000A2068823B9F4101711F01F3F71F48AE093E060E373E00E94ED08882359F480E3FB +:2015A00093E065E00E94251405C08AE093E00E942B130FC08AE093E00E942B1389E093E0AE +:2015C0000E947E1106C080913402882309F47ECF79CF1F910F910895F8940E94780C0E944F +:2015E000640785E692E00E94670E85E392E00E94330C789408950E94EC0A0E94510A0E94F1 +:20160000550C85E392E00E94600BF7CFFC01738362830895FC01848191E089278483089580 +:20162000FC01608308951F93CF93DF93EC01162F8981662349F58823F1F188818823D9F199 +:201640008D818823C1F1CE0142960E943D0D882391F181E090E09F838E8385E692E060E03E +:2016600070E00E94630E6C8D7D8D8E8D9F8D28EE33E040E050E00E947714AB01BC01CE01DF +:2016800008960E94620D17C0882399F48881882381F068A579A58AA59BA528EE33E040E084 +:2016A00050E00E947714AB01BC01CE0142960E94620D1F821E821983DF91CF911F910895B2 +:2016C0000F931F93CF93DF93EC018D81882309F471C08E819F81892B09F46CC08E01085F0C +:2016E0001F4FC8010E943D0D882309F47BC08E819F818230910539F18330910524F401974A +:2017000009F070C007C083309105A9F1049709F069C047C082E090E09F838E8368A179A10A +:201720008AA19BA128EE33E040E050E00E947714AB01BC01C8010E94620D6A817B8170956D +:2017400061957F4F4BC083E090E09F838E836C8D7D8D8E8D9F8D28EE33E040E050E00E944F +:201760007714AB01BC01C8010E94620D85E692E060E070E035C084E090E09F838E836CA125 +:201780007DA18EA19FA128EE33E040E050E00E947714AB01BC01C8010E94620D6A817B81EC +:2017A0001DC085E692E060E070E00E94630E1F821E8218C08C81882379F48981882361F088 +:2017C00088812A813B81882311F0B90107C066277727621B730B02C060E070E085E692E017 +:2017E0000E94630EDF91CF911F910F9108950F931F938C01FC01158284E090E065E070E03B +:201800000E942B07A0E0B0E0F801848F958FA68FB78F86E090E064EF71E00E942B07A0E06B +:20182000B0E0F80180A391A3A2A3B3A388E090E06CE271E00E942B07A0E0B0E0F80184A3B2 +:2018400095A3A6A3B7A38AE090E06CE271E00E942B07A0E0B0E0F80180A791A7A2A7B3A755 +:201860001F910F910895FC0181E08083148211821382128217821682CF010E94F70B089586 +:201880000F931F938C0108960E94360DC80142960E94360DC8010E94330C1F910F910895C7 +:2018A00085E392E00E94400C089584E892E00E94B90780910203882321F083EC92E00E942E +:2018C000B9070895DF93CF930F92CDB7DEB7BC01CE01019641E050E00E94031590E0898175 +:2018E0008F3F09F091E0892F0F90CF91DF91089580E091E00E94620C8093610280E891E0BC +:201900000E94620C809362020895DF93CF9300D000D0CDB7DEB7982FE62FFF27E7FDF095AB +:20192000EF59FD4F80818823C1F0662319F060E871E002C060E071E0892F992787FD909517 +:20194000880F991F880F991F680F791FCE01019644E050E00E94031510C0692F772767FDFC +:201960007095660F771F660F771F62557F4FCE01019644E050E00E94FA1469817A818B816C +:201980009C810F900F900F900F90CF91DF910895AF92BF92CF92DF92EF92FF920F931F9377 +:2019A000CF93DF937C01A62E00E033E1B32E19E0812F6A2D0E94850C412F552747FD5095A5 +:2019C000E616F70664F4802F992787FD9095840F951F62E070E00E949614B12E0BC08B2D17 +:2019E000992787FD9095840F951F62E070E00E949614012F162F0617DCF2802F6A2D0E9411 +:201A0000850C6C01EB018B2D6A2D0E94850C9C01002319F4EC16FD06DCF083E1B81619F47D +:201A20006E157F05ACF0EC1AFD0A2C193D096C1B7D0BE29EC001E39E900DF29E900D11249B +:201A40000E9496146C0D7D1D6F3F710519F014F06FEF70E0CB01DF91CF911F910F91FF90CD +:201A6000EF90DF90CF90BF90AF900895FC01108211820895FC0110820895CF93DF93EC0142 +:201A800088818823D9F089818823C1F485E692E00E94D00D2A813B814C815D81621B730BF6 +:201AA000840B950B2E813F8148855985621773078407950718F0188281E089838981DF913A +:201AC000CF910895CF92DF92EF92FF920F931F938C016A017B0181E0F8018083118285E602 +:201AE00092E00E94D00DF8016283738384839583C682D782E086F1861F910F91FF90EF9026 +:201B0000DF90CF90089597B18095982397B998B1982398B9EAE7F0E080818F688083089564 +:201B2000282F80917A0086FF02C080E008959FB7F89470936402609363022F702064209306 +:201B40007C0080917A00806480937A009FBF81E008951F920F920FB60F9211242F933F9330 +:201B60004F938F939F93EF93FF932091780040917900E0916302F0916402942F80E030E0B8 +:201B8000822B932B91838083FF91EF919F918F914F913F912F910F900FBE0F901F9018958C +:201BA000FC018FB7F894E256FF4F20813181428153818FBFB901CA01089580915603E82FF5 +:201BC000F0E09281662311F084E001C080E09B7F892B82830895FC012FB7F894E356FF4FAD +:201BE0001082809180008F738093800080915603E82FF0E09281662311F082E001C080E0BC +:201C00009D7F892B82832FBF0895FC01E356FF4F608308951F920F920FB60F9211248F9351 +:201C20009F93AF93BF938091030390910403A0910503B0910603805C9F4FAF4FBF4F809333 +:201C4000030390930403A0930503B0930603BF91AF919F918F910F900FBE0F901F90189586 +:201C60002FB7F894862F81508E3F88F084B58F7384BD80915803E82FF0E092816F3F11F492 +:201C800080E401C080E09F7B892B828304C067BD84B5806884BD2FBF08952FB7F894809194 +:201CA00080008F738093800080915603E82FF0E09281662311F082E001C080E09D7F892BCE +:201CC00082832FBF0895CB010E946E0F08950F931F938C010E94500FE0915603F0E082816D +:201CE0008B7F8283E0915603F0E0818184608183E0915603F0E082818D7F8283E0915603D9 +:201D0000F0E0818182608183E0915803F0E082818F7B8283E0915803F0E0818180648183D7 +:201D200092E09093B00083E08093B10080E28093B3009093700081E0809380008BE080930A +:201D40008100109289001092880010928B0010928A0010926F00C8010E942810C8010A9637 +:201D60000E94F70FC8014F960E941208C801825A9F4F0E94120880E090E00E946E0F09E02A +:201D800013E0C80161E00E943811C80101960E942B131F910F910895AF92BF92CF92DF92CA +:201DA000EF92FF920F931F93DF93CF9300D000D000D0CDB7DEB76C018091580389838A8170 +:201DC000807F84608A83909156039B838C81807F85608C839D838E81807F84608E83C60191 +:201DE000BE016F5F7F4FAE014D5F5F4F9E012B5F3F4F0E940410AAE0AA2EB12CAC0CBD1C42 +:201E0000C50166E041E00E94BB0F8BE192E0F601948F838F85E192E0968F858FA601455EC4 +:201E20005F4FC6014F96B60100E211EA27E030E0FCE0EF2EF12C0E94B008C601825A9F4F9C +:201E4000B5012DE130E0C20ED31EA60100E513EC20E030E0E2E1EE2EF12C0E94B00880E894 +:201E600090E0C80ED91E81E0F6018193108211821282138226960FB6F894DEBF0FBECDBF68 +:201E8000CF91DF911F910F91FF90EF90DF90CF90BF90AF90089585E692E00E94CC0E089525 +:201EA000109208031092070383E084BD86E085BD18BC10926E00E0915803F0E08181806219 +:201EC0008183E0915803F0E082818F7D8283E0915603F0E081818160818308959C01809182 +:201EE0000703909108032817390709F443C0309308032093070360E037FF04C061E0309562 +:201F000021953F4FA9012F3F310519F014F04FEF50E03FB7F8944115510519F04F3F510599 +:201F200091F484B58F7C84BD80915803E82FF0E022814F3F510511F480E201C080E02F7D89 +:201F4000822B828309C0E0915803F0E082818062828384B5806284BD80915603E82FF0E0D3 +:201F600082818E7F90E0662309F491E0892B828348BD3FBF0895FC0181E292E091838083A8 +:201F800064831786408B1086178212861186148613861686158608950F931F93CF93DF9365 +:201FA000EC018FB7F8940D811E818FBFBE016B5F7F4F8C810E94900D8823A1F08F8590E084 +:201FC000FC01EE0FFF1FEC0FFD1F108707830196837090708F87C80168890E94C80C9B83C3 +:201FE0008A8381E0DF91CF911F910F91089521E030E0FC01048002C0220F331F0A94E2F768 +:20200000822F0E94830D0895FC01DB0187E292E0918380836C9111969C919F708581807F90 +:20202000892B85836483DA016C9111969C919F708781807F892B87836683D9014C91119636 +:202040009C919F708185807F892B818740870895DC011496EC911497F0E0318115968C9126 +:2020600015978F7041E050E0BA0102C0660F771F8A95E2F7262F20952323262B21831696F3 +:20208000EC911697F0E0318117968C9117978F70BA0102C0660F771F8A95E2F7262F209593 +:2020A0002323262B21831896EC911897F0E0918119968C918F7002C0440F551F8A95E2F7DD +:2020C00040954923418308950F931F93CF93DF93DC011496EC911497F0E0328115962C919C +:2020E00015972F7081E090E002C0880F991F2A95E2F780958323828300000000000000005B +:2021000000001696EC911697F0E0328117962C9117972F7081E090E002C0880F991F2A950E +:20212000E2F78095832382830000000000000000000060E070E04FEF5FEFC1E0D0E01696ED +:20214000EC911697F0E0328117968C9117978F708E0102C0000F111F8A95E2F7202F2095CF +:202160002323202B2283000000000000000000004A30510598F4660F771F1896EC911897E8 +:20218000F0E0808190E019962C9119972F7002C0959587952A95E2F780FD61601696EC913C +:2021A0001697F0E0228117968C9117978F708E0102C0000F111F8A95E2F7C801809582237D +:2021C0008283000000000000000000004F5F5F4F4F30510509F0B3CF1496EC911497F0E0AC +:2021E000328115962C9115972F7081E090E002C0880F991F2A95E2F7282F20952323282B5A +:202200002283000000000000000000001696EC911697F0E0328117962C9117972F7081E0A8 +:2022200090E002C0880F991F2A95E2F780958323828313967C936E93129781E0DF91CF9132 +:202240001F910F910895E0915703F0E082818E7F8283E0915703F0E082818D7F8283089595 +:202260001F931FB7F8940E9423111FBF1F910895FC0160838081882321F08091C100806CEE +:2022800003C08091C1008F738093C10008951F920F920FB60F9211242F933F934F935F93E1 +:2022A0006F937F938F939F93AF93BF93EF93FF938091530390915403892B69F080915303BB +:2022C00090915403049739F080E393E00E94DF138093C60002C00E942311FF91EF91BF9187 +:2022E000AF919F918F917F916F915F914F913F912F910F900FBE0F901F9018951F93FC01C8 +:2023000080818823A9F01FB7F894CF0187960E94DF13E0915703F0E0928191609283E09170 +:202320005703F0E09281926092838093C6001FBF1F9108951F920F920FB60F9211242F93A6 +:202340003F934F935F936F937F938F939F93AF93BF93EF93FF936091C6008AE093E00E9491 +:202360003213FF91EF91BF91AF919F918F917F916F915F914F913F912F910F900FBE0F90AD +:202380001F9018950F931F938C01FC011192CF010E942413C80187960E94CF131092C500E6 +:2023A00089E18093C40080E48093C00088E18093C10086E08093C200E0915703F0E0818190 +:2023C00081608183E0915703F0E08181826081830E9423111F910F91089589E093E00E9454 +:2023E000C211089583E28093560386E28093570389E2809358030895CF93DF93EC011C8EE6 +:202400001B8E1A8E198E1E8E1D8E80E090E0A0E0B0E088879987AA87BB878C839D83AE838B +:20242000BF8388839983AA83BB83188E0BC0E82FF0E03696EE0FFF1FEC0FFD1F118210824D +:202440008F5F888F888D843090F3188E80E090E0A0E0B0E08C8B9D8BAE8BBF8BDF91CF9113 +:2024600008952F923F924F925F926F927F928F929F92AF92BF92CF92DF92EF92FF920F93DF +:202480001F93DF93CF93CDB7DEB728970FB6F894DEBF0FBECDBF98878F83FC01058CF68D55 +:2024A000E02DE61BF70BFA83E983FC01838D948D29813A81820F931F948F838F8150914076 +:2024C00024F080E091E0948F838FEF81F885838D948D80509F4F34F480E09FEFEF81F88503 +:2024E000948F838F29813A81B901882777FD8095982F0E948203EF81F8852081318142815F +:2025000053810E9458026B837C838D839E83EF81F885638D748D882777FD8095982F0E944E +:202520008203EF81F88524813581468157810E9458021B012C01EF81F885818D928DC98087 +:20254000DA80C81AD90A008D10E00A5F1F4F3801660C771C6E0E7F1EF3016081718188273B +:2025600077FD8095982F0E9482039B01AC01EF81F88564897589868997890E94FA014B01AB +:202580005C01EF81F885648B758B868B978BF301D182C082EE24D7FCE094FE2CC701B60144 +:2025A0000E9482039B01AC01C501B4010E942B02EF81F885648B758B868B978B05501040AD +:2025C00003701070008F20853185428553850E9458027B018C0129813A81EF81F885328F67 +:2025E000218F6B817C818D819E81A20191010E942B02A80197010E942B020E94E0039B01E0 +:20260000AC01220F331FC90128960FB6F894DEBF0FBECDBFCF91DF911F910F91FF90EF908D +:20262000DF90CF90BF90AF909F908F907F906F905F904F903F902F900895FC0111821082C7 +:2026400012A214A213A20895FC011182108212A214A213A20895FC011182108212A214A254 +:2026600013A20895DC0193968D919C919497892B91F4653D19F481E090E066C011961C9258 +:2026800011971C9292961C92929794961C921E92939781E020C093968D919C9194970197BB +:2026A000E9F4603260F495966C93959795968C919597882309F438C082E090E045C011966F +:2026C0001C9211971C9292961C92929794961C921E92939782E092968C93089593968D91B8 +:2026E0009C919497029719F58C918032B8F411968C911197862728E09CE880FB86950EF423 +:2027000089272A95D1F711968C9311978C91FD01E80FF11D62838C918F5F8C939C9195968D +:202720008C919597981718F183E090E00DC093968D919C9194970397D1F411968C9111978E +:20274000861739F484E090E094969C938E939397089511961C9211971C9292961C929297F0 +:2027600094961C921E92939783E092968C930895FC01E60FF11D82810895E62FF0E0DC01FE +:20278000AE0FBF1F12962C913196E80FF91F4281942F80E030E0282B392BC9010895FC0152 +:2027A0001182108212A214A213A215A20895FC011182108212A214A213A215A20895DC010F +:2027C00093968D919C919497892B41F481E090E094969C938E93939725ED35C093968D91AE +:2027E0009C919497019739F42C918C918823B9F082E090E024C093968D919C9194970297A0 +:2028000089F495968C919597FD01E80FF11D22818F5F95968C9395979C918917A0F083E0FD +:2028200090E00DC093968D919C919497039711F020E009C011962C91119784E090E094964E +:202840009C938E939397822F0895DC018C918032B8F411968C911197862728E09CE880FB38 +:2028600086950EF489272A95D1F711968C9311978C91FD01E80FF11D62838C918F5F8C9367 +:202880000895DC018C918032B8F411968C911197862728E09CE880FB86950EF489272A9597 +:2028A000D1F711968C9311978C91FD01E80FF11D62838C918F5F8C938C918032B8F4119691 +:2028C0008C911197872728E09CE880FB86950EF489272A95D1F711968C9311978C91FD01A1 +:2028E000E80FF11D72838C918F5F8C930895629FD001739FF001829FE00DF11D649FE00D36 +:20290000F11D929FF00D839FF00D749FF00D659FF00D9927729FB00DE11DF91F639FB00DE8 +:20292000E11DF91FBD01CF011124089597FB092E07260AD077FD04D00CD006D000201AF429 +:20294000709561957F4F0895F6F7909581959F4F0895AA1BBB1B51E107C0AA1FBB1FA6176A +:20296000B70710F0A61BB70B881F991F5A95A9F780959095BC01CD0108952F923F924F9253 +:202980005F926F927F928F929F92AF92BF92CF92DF92EF92FF920F931F93CF93DF93CDB7C0 +:2029A000DEB7CA1BDB0B0FB6F894DEBF0FBECDBF09942A88398848885F846E847D848C84AA +:2029C0009B84AA84B984C884DF80EE80FD800C811B81AA81B981CE0FD11D0FB6F894DEBF90 +:2029E0000FBECDBFED010895EE0FFF1F0590F491E02D0994FB01DC0102C005900D924150B4 +:202A00005040D8F70895DC01CB01FC01F999FECF06C0F2BDE1BDF89A319600B40D92415065 +:202A20005040B8F70895DC01CB0103C02D910E941E1541505040D0F70895262FF999FECF82 +:202A40001FBA92BD81BD20BD0FB6F894FA9AF99A0FBE01960895F894FFCF457874727564E4 +:202A6000657200000000000000000000010202030303030404040404040404050505050535 +:202A8000050505050505050505050506060606060606060606060606060606060606060681 +:202AA000060606060606060606060607070707070707070707070707070707070707070741 +:202AC000070707070707070707070707070707070707070707070707070707070707070716 +:202AE0000707070707070707070707080808080808080808080808080808080808080808E1 +:202B00000808080808080808080808080808080808080808080808080808080808080808B5 +:202B2000080808080808080808080808080808080808080808080808080808080808080895 +:202B4000080808080808080808080808080808080808080808080808080808080808080875 +:202B60000808080808080808080808000000004D0E00000000300E00000000CC0F00000089 +:202B800000641000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDD +:202BA000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF35 +:202BC000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF15 +:202BE000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5 +:202C0000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD4 +:202C2000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB4 +:202C4000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF94 +:202C6000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF74 +:202C8000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF54 +:202CA000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF34 +:202CC000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF14 +:202CE000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4 +:202D0000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD3 +:202D2000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB3 +:202D4000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF93 +:202D6000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF73 +:202D8000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF53 +:202DA000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF33 +:202DC000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF13 +:202DE000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3 +:202E0000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD2 +:202E2000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB2 +:202E4000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF92 +:202E6000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF72 +:202E8000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF52 +:202EA000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF32 +:202EC000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF12 +:202EE000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2 +:202F0000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD1 +:202F2000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB1 +:202F4000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF91 +:202F6000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF71 +:202F8000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF51 +:202FA000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF31 +:202FC000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF11 +:202FE000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1 +:20300000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD0 +:20302000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB0 +:20304000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF90 +:20306000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF70 +:20308000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF50 +:2030A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF30 +:2030C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF10 +:2030E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0 +:20310000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCF +:20312000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAF +:20314000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8F +:20316000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6F +:20318000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4F +:2031A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2F +:2031C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0F +:2031E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEF +:20320000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCE +:20322000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAE +:20324000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8E +:20326000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6E +:20328000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4E +:2032A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2E +:2032C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0E +:2032E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEE +:20330000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCD +:20332000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAD +:20334000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8D +:20336000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6D +:20338000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4D +:2033A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2D +:2033C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0D +:2033E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFED +:20340000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCC +:20342000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAC +:20344000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8C +:20346000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6C +:20348000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4C +:2034A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2C +:2034C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0C +:2034E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEC +:20350000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCB +:20352000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAB +:20354000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8B +:20356000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6B +:20358000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4B +:2035A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2B +:2035C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0B +:2035E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEB +:20360000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFCA +:20362000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFAA +:20364000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF8A +:20366000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF6A +:20368000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF4A +:2036A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF2A +:2036C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF0A +:2036E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEA +:20370000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFC9 +:20372000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFA9 +:20374000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF89 +:20376000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF69 +:20378000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF49 +:2037A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF29 +:2037C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF09 +:2037E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFE9 +:203800000C94341C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C5D +:203820000C94511C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C20 +:203840000C94511C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C0C94511C00 +:203860000C94511C0C94511C11241FBECFEFD4E0DEBFCDBF11E0A0E0B1E0EAE9FFE302C008 +:2038800005900D92A230B107D9F712E0A2E0B1E001C01D92AD30B107E1F70E942D1D0C942C +:2038A000CB1F0C94001C982F9595959595959595905D8F708A307CF0282F295A8091C00036 +:2038C00085FFFCCF9093C6008091C00085FFFCCF2093C6000895282F205DF0CF982F80910F +:2038E000C00085FFFCCF9093C6000895EF92FF920F931F93EE24FF2487018091C00087FDBB +:2039000017C00894E11CF11C011D111D81E4E81682E4F8068FE0080780E0180770F3E09146 +:203920000401F091050109958091C00087FFE9CF8091C6001F910F91FF90EF9008950E94DA +:20394000761C982F8091C00085FFFCCF9093C60091362CF490330CF09053892F089597553B +:20396000892F08951F930E949F1C182F0E949F1C1295107F810F1F9108951F93182F882388 +:2039800021F00E94761C1150E1F71F9108951F93182F0E94761C803249F0809103018F5F41 +:2039A000809303018530C1F01F9108958091C00085FFFCCF84E18093C6008091C00085FF8A +:2039C000FCCF1093C6008091C00085FFFCCF80E18093C6001F910895E0910401F09105010F +:2039E00009951F9108950E94761C803241F0809103018F5F80930301853081F008958091D7 +:203A0000C00085FFFCCF84E18093C6008091C00085FFFCCF80E18093C6000895E0910401EC +:203A2000F09105010995089540E951E08823A1F02D9A28EE33E0FA013197F1F721503040B2 +:203A4000D1F72D9828EE33E0FA013197F1F721503040D1F7815061F708953F924F925F92F3 +:203A60006F927F928F929F92AF92BF92CF92DF92EF92FF920F931F93CF93DF93000085B10E +:203A8000897F85B984B1866084B98BB18F798BB98AB180668AB983E38093C4001092C500F8 +:203AA00088E18093C10086E08093C20050985898259A82E00E94141D24E1F22E9EE1E92E07 +:203AC00084E9D82E06E0C02E10E1B12EAA24A394B1E49B2EA6E58A2EF2E57F2EE0E26E2E47 +:203AE00079E4572E63E5462E50E5352E0E94761C8033B1F18133B9F1803409F46FC0813414 +:203B000009F476C0823409F485C0853409F488C0803531F1823521F1813511F1853509F46D +:203B200085C0863509F48DC0843609F496C0843709F403C1853709F471C1863709F466C0E7 +:203B4000809103018F5F80930301853079F6E0910401F091050109950E94761C803351F65E +:203B60000E94F31CC3CF0E94761C803249F78091C00085FFFCCFF092C6008091C00085FF1F +:203B8000FCCF9092C6008091C00085FFFCCF8092C6008091C00085FFFCCF7092C600809181 +:203BA000C00085FFFCCF6092C6008091C00085FFFCCF5092C6008091C00085FFFCCF409284 +:203BC000C6008091C00085FFFCCF3092C6008091C00085FFFCCFB092C60088CF0E94761CC4 +:203BE000863808F4BDCF0E94761C0E94F31C7ECF0E94761C803809F49CC0813809F40AC187 +:203C0000823809F42FC1883909F48FC080E00E94C71C6CCF84E10E94BD1C0E94F31C66CF0A +:203C200085E00E94BD1C0E94F31C60CF0E94761C809306010E94761C809307010E94F31C76 +:203C400055CF0E94761C803309F410C183E00E94BD1C80E00E94C71C49CF0E94761C809369 +:203C600009020E94761C8093080280910C028E7F80930C020E94761C853409F408C18091D7 +:203C8000080290910902892B89F000E010E00E94761CF801E85FFE4F80830F5F1F4F809140 +:203CA0000802909109020817190788F30E94761C803209F045CF80910C0280FFF4C06091DE +:203CC000060170910701660F771F7093070160930601A0910802B09109021097C9F0E8E015 +:203CE000F1E09B01AD014E0F5F1FF999FECF32BD21BD819180BDFA9AF99A2F5F3F4FE41715 +:203D0000F50799F76A0F7B1F70930701609306018091C00085FFFCCFF092C6008091C000C6 +:203D200085FFFCCFB092C600E1CE83E00E94C71CDDCE82E00E94C71CD9CE0E94761C809315 +:203D400009020E94761C809308028091060190910701880F991F90930701809306010E9490 +:203D6000761C853409F499C080910C028E7F80930C020E94761C803209F0B8CE8091C0001F +:203D800085FFFCCFF092C600A0910802B09109021097B9F180910C02182F1170082F027024 +:203DA000E0910601F0910701AF014F5F5F4FBA0120E030E01123B1F4002339F494918091CC +:203DC000C00085FFFCCF9093C6002F5F3F4FCB010196FA012A173B0780F4BC014F5F5F4F61 +:203DE000112351F3F999FECFF2BDE1BDF89A90B58091C00085FFFCCFE6CF709307016093F5 +:203E000006018091C00085FDE6CE8091C00085FFF8CFE1CE81E00E94C71C68CE0E94761C79 +:203E2000803209F08DCE8091C00085FFFCCFF092C6008091C00085FFFCCFE092C6008091AB +:203E4000C00085FFFCCFD092C6008091C00085FFFCCFC092C6008091C00085FFFCCFB09291 +:203E6000C60044CE80E10E94C71C40CE0E94761C0E94761C182F0E94761C112309F483C01F +:203E8000113009F484C086E00E94C71C2FCE80910C02816080930C02F2CE80910C028160D7 +:203EA00080930C0266CF809107018823880F880B8A2180930B028091060190910701880F1B +:203EC000991F90930701809306018091080280FF09C0809108029091090201969093090276 +:203EE00080930802F894F999FECF1127E0910601F0910701C8E0D1E08091080290910902E1 +:203F0000103091F40091570001700130D9F303E000935700E8950091570001700130D9F3E6 +:203F200001E100935700E895099019900091570001700130D9F301E000935700E8951395B0 +:203F4000103498F011270091570001700130D9F305E000935700E895009157000170013031 +:203F6000D9F301E100935700E8953296029709F0C7CF103011F00296E5CF11248091C000A9 +:1C3F800085FFC6CEC9CE8EE10E94C71CAFCD84E90E94C71CABCDF894FFCF8000C2 +:00000001FF diff --git a/dist/ECv3.6/burn.sh b/dist/ECv3.6/burn.sh new file mode 100755 index 0000000..e816b80 --- /dev/null +++ b/dist/ECv3.6/burn.sh @@ -0,0 +1,47 @@ +#!/bin/bash + +MAC_TOOLS_HOME=/Applications/Arduino.app/Contents/Resources/Java/hardware/tools/avr + +# Look for avrdude +if [ ! $AVRDUDE]; then + if [ `which avrdude` ]; then + echo "Using default avrdude installed on system." + AVRDUDE=`which avrdude` + elif [ -a ${MAC_TOOLS_HOME}/bin/avrdude ]; then + echo "Found an avrdude installation in the default Mac Arduino location." + AVRDUDE=${MAC_TOOLS_HOME}/bin/avrdude + AD_CONF=${MAC_TOOLS_HOME}/etc/avrdude.conf + else + echo "Couldn't find a valid AVRDUDE installation. Try setting the" + echo "AVRDUDE environment variable to the location of your AVRDUDE" + echo "installation. You may also need to set the AD_CONF variable" + echo "to the location of your avrdude.conf file, if your installation" + echo "of AVRDUDE doesn't support USBTinyISP out of the box." + exit 1 + fi +fi + +FIRMWARE=PRODUCTION +FWDIR=`dirname $0` + +while true; do + echo "Press ENTER to upload $FIRMWARE" + read + if [ $AD_CONF ]; then + CONF_FLAGS="-C $AD_CONF " + fi + # Burn lock bits and fuses + $AVRDUDE $CONF_FLAGS -v -pm168 -cusbtiny -e -Ulock:w:0x3F:m -Uefuse:w:0x00:m -Uhfuse:w:0xdd:m -Ulfuse:w:0xee:m + # Burn firmware + $AVRDUDE $CONF_FLAGS -v -pm168 -cusbtiny -Uflash:w:${FWDIR}/${FIRMWARE}.hex:i -Ulock:w:0x0F:m +done + +#!/bin/bash + +while true; do + echo "Press ENTER to upload" + read + # Burn lock bits and fuses +done + + From d8a04b35a1a9443bb3fe95b33549e7143238912a Mon Sep 17 00:00:00 2001 From: phooky Date: Mon, 15 Nov 2010 16:59:58 -0500 Subject: [PATCH 08/61] * Added batch files for production --- dist/burn-ECv3.6.bat | 20 ++++++++++++++++++++ dist/burn-MBv2.4.bat | 18 ++++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 dist/burn-ECv3.6.bat create mode 100644 dist/burn-MBv2.4.bat diff --git a/dist/burn-ECv3.6.bat b/dist/burn-ECv3.6.bat new file mode 100644 index 0000000..c4f4101 --- /dev/null +++ b/dist/burn-ECv3.6.bat @@ -0,0 +1,20 @@ +@ECHO OFF +:startBurn +echo Ready to burn firmware for Motherboard v1.2. +pause + +tools-win\avrdude -v -pm168 -cusbtiny -e -Ulock:w:0x3F:m -Uefuse:w:0x00:m -Uhfuse:w:0xDD:m -Ulfuse:w:0xEE:m + +tools-win\avrdude -v -pm168 -cusbtiny -e -Uflash:w:ECv3.6/PRODUCTION.hex + +if errorlevel 1 ( +echo *** FAILURE *** Failed to verify program. Try again. +goto startBurn +) + +if errorlevel 1 ( +echo *** FAILURE *** Failed to verify program. Try again. +goto startBurn +) + +goto startBurn \ No newline at end of file diff --git a/dist/burn-MBv2.4.bat b/dist/burn-MBv2.4.bat new file mode 100644 index 0000000..eca145c --- /dev/null +++ b/dist/burn-MBv2.4.bat @@ -0,0 +1,18 @@ +@ECHO OFF +:startBurn +echo Ready to burn firmware for Motherboard v1.2. +pause + +tools-win\avrdude -cstk500v1 -b57600 -D -v -D -pm1280 -PCOM1 -Uflash:w:MBv2.4/PRODUCTION.hex:i + +if errorlevel 1 ( +echo *** FAILURE *** Failed to verify program. Try again. +goto startBurn +) + +if errorlevel 1 ( +echo *** FAILURE *** Failed to verify program. Try again. +goto startBurn +) + +goto startBurn \ No newline at end of file From 2a9d787e6d5b3d245e2bbf914683ebd8f0e59dbc Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Mon, 15 Nov 2010 18:44:56 -0500 Subject: [PATCH 09/61] Let the HOST_CMD_ENABLE_AXES support up to 5 axes --- v2/src/Motherboard/Command.cc | 2 +- v2/src/Motherboard/Steppers.cc | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/v2/src/Motherboard/Command.cc b/v2/src/Motherboard/Command.cc index 86a5bd8..9166fc5 100644 --- a/v2/src/Motherboard/Command.cc +++ b/v2/src/Motherboard/Command.cc @@ -177,7 +177,7 @@ void runCommandSlice() { command_buffer.pop(); // remove the command code uint8_t axes = command_buffer.pop(); bool enable = (axes & 0x80) != 0; - for (int i = 0; i < 3; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { if ((axes & _BV(i)) != 0) { steppers::enableAxis(i, enable); } diff --git a/v2/src/Motherboard/Steppers.cc b/v2/src/Motherboard/Steppers.cc index eedc1da..3d1493c 100644 --- a/v2/src/Motherboard/Steppers.cc +++ b/v2/src/Motherboard/Steppers.cc @@ -214,7 +214,9 @@ void startHoming(const bool maximums, const uint8_t axes_enabled, const uint32_t /// Enable/disable the given axis. void enableAxis(uint8_t which, bool enable) { - axes[which].enableStepper(enable); + if (which < STEPPER_COUNT) { + axes[which].enableStepper(enable); + } } bool doInterrupt() { From c8e9b42d716717f6193d5aafcfcfe60e81eb71a8 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 17 Nov 2010 08:08:23 -0600 Subject: [PATCH 10/61] Support optiboot and bootloader-less operation. Added the RX pullup to the UART config that was getting inherited by the bootloader. This will fix when using a different/wrong bootloader, or no bootloader. I tested with a modified optiboot and it works. --- v2/src/Extruder/boards/ecv22/UART.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/v2/src/Extruder/boards/ecv22/UART.cc b/v2/src/Extruder/boards/ecv22/UART.cc index ffff8ee..58699e0 100644 --- a/v2/src/Extruder/boards/ecv22/UART.cc +++ b/v2/src/Extruder/boards/ecv22/UART.cc @@ -63,6 +63,11 @@ UART::UART() : enabled(false) { /* defaults to 8-bit, no parity, 1 stop bit */ TX_ENABLE_PIN.setDirection(true); RX_ENABLE_PIN.setDirection(true); + + // pulup on RX + DDRD &= ~_BV(PIND0); + PORTD |= _BV(PIND0); + listen(); } From 86ad2b4c856501606c92fe63d8cf3b316d68d414 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 17 Nov 2010 08:33:03 -0600 Subject: [PATCH 11/61] And, fix the fix. --- v2/src/Extruder/boards/ecv22/ExtruderBoard.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc index 79646f6..ea87ee1 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc @@ -164,7 +164,7 @@ void ExtruderBoard::reset() { // init after we know what kind of motor we're using setMotorSpeed(0); - setMotorSpeedRPM(0); + setMotorSpeedRPM(0, true); } void ExtruderBoard::setMotorSpeed(int16_t speed) { From eefe3ebf42ced7b114ca823d20b8ddac250ed40e Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Tue, 16 Nov 2010 21:28:20 -0600 Subject: [PATCH 12/61] Fix for external stepper extruder not stopping on reset or stop. --- v2/src/Extruder/boards/ecv22/ExtruderBoard.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc index ea87ee1..79646f6 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc @@ -164,7 +164,7 @@ void ExtruderBoard::reset() { // init after we know what kind of motor we're using setMotorSpeed(0); - setMotorSpeedRPM(0, true); + setMotorSpeedRPM(0); } void ExtruderBoard::setMotorSpeed(int16_t speed) { From 43434832dece02c53bee86de6065e72d3413866f Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 17 Nov 2010 08:33:03 -0600 Subject: [PATCH 13/61] And, fix the fix. --- v2/src/Extruder/boards/ecv22/ExtruderBoard.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc index 79646f6..ea87ee1 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc @@ -164,7 +164,7 @@ void ExtruderBoard::reset() { // init after we know what kind of motor we're using setMotorSpeed(0); - setMotorSpeedRPM(0); + setMotorSpeedRPM(0, true); } void ExtruderBoard::setMotorSpeed(int16_t speed) { From 289aed19b39d42cc9e009bc7eb06d057364adbfb Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Wed, 17 Nov 2010 17:25:43 -0500 Subject: [PATCH 14/61] Added support for specifying VERSION and BUILD_NAME when building firmwares --- v2/src/Extruder/SConscript | 6 ++++++ v2/src/Motherboard/SConscript | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/v2/src/Extruder/SConscript b/v2/src/Extruder/SConscript index ee74be9..2634a2f 100644 --- a/v2/src/Extruder/SConscript +++ b/v2/src/Extruder/SConscript @@ -78,6 +78,12 @@ flags=[ '-ffunction-sections', '-fdata-sections'] +if (os.environ.has_key('VERSION')): + flags.append('-DVERSION=' + os.environ['VERSION']) + +if (os.environ.has_key('BUILD_NAME')): + flags.append('-DBUILD_NAME=' + os.environ['BUILD_NAME']) + if (stepper == 'true'): flags.append('-DDEFAULT_STEPPER=1') diff --git a/v2/src/Motherboard/SConscript b/v2/src/Motherboard/SConscript index 7759fa5..2ab6cf8 100644 --- a/v2/src/Motherboard/SConscript +++ b/v2/src/Motherboard/SConscript @@ -95,6 +95,12 @@ flags=[ '-ffunction-sections', '-fdata-sections'] +if (os.environ.has_key('VERSION')): + flags.append('-DVERSION=' + os.environ['VERSION']) + +if (os.environ.has_key('BUILD_NAME')): + flags.append('-DBUILD_NAME=' + os.environ['BUILD_NAME']) + if (os.environ.has_key('AVR_TOOLS_PATH')): avr_tools_path = os.environ['AVR_TOOLS_PATH'] else: From 7a91517f99931bbbc25ef7a3658a159cdd03a1b9 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Wed, 17 Nov 2010 17:26:13 -0500 Subject: [PATCH 15/61] Implemented SLAVE_CMD_IS_PLATFORM_READY and HOST_CMD_WAIT_FOR_PLATFORM --- v2/src/Extruder/Host.cc | 4 ++++ v2/src/Motherboard/Command.cc | 33 ++++++++++++++++++++++++++++++++- v2/src/shared/Commands.hh | 2 ++ 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/v2/src/Extruder/Host.cc b/v2/src/Extruder/Host.cc index 9c5f936..0e14cba 100644 --- a/v2/src/Extruder/Host.cc +++ b/v2/src/Extruder/Host.cc @@ -175,6 +175,10 @@ bool processQueryPacket(const InPacket& from_host, OutPacket& to_host) { to_host.append8(RC_OK); to_host.append16(board.getPlatformHeater().get_set_temperature()); return true; + case SLAVE_CMD_IS_PLATFORM_READY: + to_host.append8(RC_OK); + to_host.append8(board.getPlatformHeater().hasReachedTargetTemperature()?1:0); + return true; } } return false; diff --git a/v2/src/Motherboard/Command.cc b/v2/src/Motherboard/Command.cc index 9166fc5..a2686e3 100644 --- a/v2/src/Motherboard/Command.cc +++ b/v2/src/Motherboard/Command.cc @@ -96,7 +96,8 @@ enum { MOVING, DELAY, HOMING, - WAIT_ON_TOOL + WAIT_ON_TOOL, + WAIT_ON_PLATFORM } mode = READY; Timeout delay_timeout; @@ -152,6 +153,27 @@ void runCommandSlice() { tool::releaseLock(); } } + if (mode == WAIT_ON_PLATFORM) { + // FIXME: Duplicates most code from WAIT_ON_TOOL + if (tool::getLock()) { + OutPacket& out = tool::getOutPacket(); + InPacket& in = tool::getInPacket(); + out.reset(); + out.append8(tool::tool_index); + out.append8(SLAVE_CMD_IS_PLATFORM_READY); + tool::startTransaction(); + // WHILE: bounded by timeout in runToolSlice + while (!tool::isTransactionDone()) { + tool::runToolSlice(); + } + if (!in.hasError()) { + if (in.read8(1) != 0) { + mode = READY; + } + } + tool::releaseLock(); + } + } if (mode == READY) { // process next command on the queue. if (command_buffer.getLength() > 0) { @@ -222,6 +244,15 @@ void runCommandSlice() { uint16_t toolPingDelay = (uint16_t)pop16(); uint16_t toolTimeout = (uint16_t)pop16(); } + } else if (command == HOST_CMD_WAIT_FOR_PLATFORM) { + // FIXME: Almost equivalent to WAIT_FOR_TOOL + if (command_buffer.getLength() >= 6) { + mode = WAIT_ON_PLATFORM; + command_buffer.pop(); + uint8_t currentToolIndex = command_buffer.pop(); + uint16_t toolPingDelay = (uint16_t)pop16(); + uint16_t toolTimeout = (uint16_t)pop16(); + } } else if (command == HOST_CMD_TOOL_COMMAND) { if (command_buffer.getLength() >= 4) { // needs a payload uint8_t payload_length = command_buffer[3]; diff --git a/v2/src/shared/Commands.hh b/v2/src/shared/Commands.hh index 65790d4..d0f1c61 100644 --- a/v2/src/shared/Commands.hh +++ b/v2/src/shared/Commands.hh @@ -70,6 +70,7 @@ #define HOST_CMD_WAIT_FOR_TOOL 135 #define HOST_CMD_TOOL_COMMAND 136 #define HOST_CMD_ENABLE_AXES 137 +#define HOST_CMD_WAIT_FOR_PLATFORM 141 #define HOST_CMD_DEBUG_ECHO 0x70 @@ -108,5 +109,6 @@ #define SLAVE_CMD_GET_PLATFORM_SP 33 // Retrieve the string representing this build #define SLAVE_CMD_GET_BUILD_NAME 34 +#define SLAVE_CMD_IS_PLATFORM_READY 35 #endif // SHARED_COMMANDS_H_ From 3e3c7edbf7e780264cda8fbfcd316ae470d7bd29 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Wed, 17 Nov 2010 17:26:50 -0500 Subject: [PATCH 16/61] Convenience scripts for creating date-tagged beta releases --- v2/extruder_upload_daily.sh | 2 ++ v2/motherboard_upload_daily.sh | 2 ++ 2 files changed, 4 insertions(+) create mode 100755 v2/extruder_upload_daily.sh create mode 100755 v2/motherboard_upload_daily.sh diff --git a/v2/extruder_upload_daily.sh b/v2/extruder_upload_daily.sh new file mode 100755 index 0000000..605c016 --- /dev/null +++ b/v2/extruder_upload_daily.sh @@ -0,0 +1,2 @@ +scons -c -f SConstruct.extruder +VERSION=206 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct.extruder port=$1 upload diff --git a/v2/motherboard_upload_daily.sh b/v2/motherboard_upload_daily.sh new file mode 100755 index 0000000..85ce424 --- /dev/null +++ b/v2/motherboard_upload_daily.sh @@ -0,0 +1,2 @@ +scons -c -f SConstruct +VERSION=204 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct port=$1 upload From 5e3d99c650355c085b968ef385951a4f17169c57 Mon Sep 17 00:00:00 2001 From: phooky Date: Thu, 18 Nov 2010 15:43:37 -0500 Subject: [PATCH 17/61] * Added support for wait-for-tool timeouts --- v2/src/Motherboard/Command.cc | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/v2/src/Motherboard/Command.cc b/v2/src/Motherboard/Command.cc index a2686e3..3cb6a98 100644 --- a/v2/src/Motherboard/Command.cc +++ b/v2/src/Motherboard/Command.cc @@ -102,6 +102,7 @@ enum { Timeout delay_timeout; Timeout homing_timeout; +Timeout tool_wait_timeout; void reset() { command_buffer.reset(); @@ -134,7 +135,9 @@ void runCommandSlice() { } } if (mode == WAIT_ON_TOOL) { - if (tool::getLock()) { + if (tool_wait_timeout.hasElapsed()) { + mode = READY; + } else if (tool::getLock()) { OutPacket& out = tool::getOutPacket(); InPacket& in = tool::getInPacket(); out.reset(); @@ -155,7 +158,9 @@ void runCommandSlice() { } if (mode == WAIT_ON_PLATFORM) { // FIXME: Duplicates most code from WAIT_ON_TOOL - if (tool::getLock()) { + if (tool_wait_timeout.hasElapsed()) { + mode = READY; + } else if (tool::getLock()) { OutPacket& out = tool::getOutPacket(); InPacket& in = tool::getInPacket(); out.reset(); @@ -243,6 +248,7 @@ void runCommandSlice() { uint8_t currentToolIndex = command_buffer.pop(); uint16_t toolPingDelay = (uint16_t)pop16(); uint16_t toolTimeout = (uint16_t)pop16(); + tool_wait_timeout.start(toolTimeout*1000L); } } else if (command == HOST_CMD_WAIT_FOR_PLATFORM) { // FIXME: Almost equivalent to WAIT_FOR_TOOL @@ -252,6 +258,7 @@ void runCommandSlice() { uint8_t currentToolIndex = command_buffer.pop(); uint16_t toolPingDelay = (uint16_t)pop16(); uint16_t toolTimeout = (uint16_t)pop16(); + tool_wait_timeout.start(toolTimeout*1000L); } } else if (command == HOST_CMD_TOOL_COMMAND) { if (command_buffer.getLength() >= 4) { // needs a payload From 4ff795fbe86f9c98c33dc4645ae890770f01ec13 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Thu, 18 Nov 2010 16:20:54 -0500 Subject: [PATCH 18/61] bugfix: Timeout expects microsecs, not millisecs --- v2/src/Motherboard/Command.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/v2/src/Motherboard/Command.cc b/v2/src/Motherboard/Command.cc index 3cb6a98..debfaa9 100644 --- a/v2/src/Motherboard/Command.cc +++ b/v2/src/Motherboard/Command.cc @@ -248,7 +248,7 @@ void runCommandSlice() { uint8_t currentToolIndex = command_buffer.pop(); uint16_t toolPingDelay = (uint16_t)pop16(); uint16_t toolTimeout = (uint16_t)pop16(); - tool_wait_timeout.start(toolTimeout*1000L); + tool_wait_timeout.start(toolTimeout*1000000L); } } else if (command == HOST_CMD_WAIT_FOR_PLATFORM) { // FIXME: Almost equivalent to WAIT_FOR_TOOL @@ -258,7 +258,7 @@ void runCommandSlice() { uint8_t currentToolIndex = command_buffer.pop(); uint16_t toolPingDelay = (uint16_t)pop16(); uint16_t toolTimeout = (uint16_t)pop16(); - tool_wait_timeout.start(toolTimeout*1000L); + tool_wait_timeout.start(toolTimeout*1000000L); } } else if (command == HOST_CMD_TOOL_COMMAND) { if (command_buffer.getLength() >= 4) { // needs a payload From 82f11bcfa073e184f0ecf225e4cdfb052a0838ef Mon Sep 17 00:00:00 2001 From: phooky Date: Thu, 18 Nov 2010 18:30:05 -0500 Subject: [PATCH 19/61] * update 3.4 board to handle 2.3 stepper hooks (unimplemented stubs) * extended upload scripts --- v2/extruder_upload_daily.sh | 9 ++++++++- v2/motherboard_upload_daily.sh | 9 ++++++++- v2/src/Extruder/boards/ecv34/ExtruderBoard.hh | 1 + v2/src/Extruder/boards/ecv34/ExtruderMotor.cc | 5 +++++ v2/src/Extruder/boards/ecv34/ExtruderMotor.hh | 3 +++ 5 files changed, 25 insertions(+), 2 deletions(-) diff --git a/v2/extruder_upload_daily.sh b/v2/extruder_upload_daily.sh index 605c016..62cbdaf 100755 --- a/v2/extruder_upload_daily.sh +++ b/v2/extruder_upload_daily.sh @@ -1,2 +1,9 @@ +#!/bin/bash +if [ $# -lt 1 ] +then + echo "Usage: `basename $0` port=PORTNAME [platform=PLATFORM]" + echo "(Valid platforms are ecv22, ecv34)" + exit 1 +fi scons -c -f SConstruct.extruder -VERSION=206 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct.extruder port=$1 upload +VERSION=206 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct.extruder $@ upload diff --git a/v2/motherboard_upload_daily.sh b/v2/motherboard_upload_daily.sh index 85ce424..5851b09 100755 --- a/v2/motherboard_upload_daily.sh +++ b/v2/motherboard_upload_daily.sh @@ -1,2 +1,9 @@ +#!/bin/bash +if [ $# -lt 1 ] +then + echo "Usage: `basename $0` port=PORTNAME [platform=PLATFORM]" + echo "(Valid platforms are rrmbv12, rrmbv24)" + exit 1 +fi scons -c -f SConstruct -VERSION=204 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct port=$1 upload +VERSION=204 BUILD_NAME=\\\"Beta\ `date '+%Y.%m.%d'`\\\" scons -f SConstruct $@ upload diff --git a/v2/src/Extruder/boards/ecv34/ExtruderBoard.hh b/v2/src/Extruder/boards/ecv34/ExtruderBoard.hh index 3e1dc53..34d4bc8 100644 --- a/v2/src/Extruder/boards/ecv34/ExtruderBoard.hh +++ b/v2/src/Extruder/boards/ecv34/ExtruderBoard.hh @@ -44,6 +44,7 @@ public: Heater& getExtruderHeater() { return extruder_heater; } Heater& getPlatformHeater() { return platform_heater; } void setMotorSpeed(int16_t speed); + void setMotorSpeedRPM(uint32_t speed, bool direction) {} // Unsupported on 3.4 void setFan(bool on); void setValve(bool on); UART& getHostUART() { return UART::getHostUART(); } diff --git a/v2/src/Extruder/boards/ecv34/ExtruderMotor.cc b/v2/src/Extruder/boards/ecv34/ExtruderMotor.cc index 629ab0d..9621d06 100644 --- a/v2/src/Extruder/boards/ecv34/ExtruderMotor.cc +++ b/v2/src/Extruder/boards/ecv34/ExtruderMotor.cc @@ -34,8 +34,13 @@ void initExtruderMotor() { MOTOR_ENABLE_PIN.setDirection(true); MOTOR_ENABLE_PIN.setValue(false); MOTOR_DIR_PIN.setDirection(true); + } + +void setStepperMode(bool mode, bool external/* = false*/) { + // New boards do not drive their own extruder stepper. +} void setExtruderMotor(int16_t speed) { if (speed == last_extruder_speed) return; last_extruder_speed = speed; diff --git a/v2/src/Extruder/boards/ecv34/ExtruderMotor.hh b/v2/src/Extruder/boards/ecv34/ExtruderMotor.hh index 0192606..f1a326a 100644 --- a/v2/src/Extruder/boards/ecv34/ExtruderMotor.hh +++ b/v2/src/Extruder/boards/ecv34/ExtruderMotor.hh @@ -22,10 +22,13 @@ void initExtruderMotor(); +void setStepperMode(bool mode, bool external = false); + // 0 = stop // + = forward direction // - = negative direction // Valid range: -255 through 255 void setExtruderMotor(int16_t speed); +void setExtruderMotorRPM(uint32_t micros, bool direction); #endif // BOARDS_ECV22_EXTRUDER_MOTOR_HH_ From 2b4d456ee2c15a1c3c818d8aa971053aadee4723 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Mon, 22 Nov 2010 14:01:25 -0500 Subject: [PATCH 20/61] cosmetics --- v2/src/Extruder/SConscript | 8 ++++---- v2/src/Motherboard/SConscript | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/v2/src/Extruder/SConscript b/v2/src/Extruder/SConscript index 2634a2f..4b601f1 100644 --- a/v2/src/Extruder/SConscript +++ b/v2/src/Extruder/SConscript @@ -1,12 +1,12 @@ # # HOW TO USE THIS BUILD SCRIPT # -# By default, this script will build the firmware for an atmega644p-based motherboard. +# By default, this script will build the firmware for an atmega168-based extruder controller. # The firmware will be built, but not uploaded. # # To build for another platform, pass an explicit platform parameter. For example, -# $ scons platform=RRMBv12 -# $ scons platform=MBMBv20 +# $ scons platform=ecv22 +# $ scons platform=ecv34 # # To default to stepper rather than DC drive, add "stepper=true". # To default to PWM heater rather than relays, add "relays=false". @@ -116,7 +116,7 @@ env.Elf(elf_name, objs) env.Hex(hex_name, elf_name) avrdude = avr_tools_path+"/avrdude" -avrdude_flags = "-V -F -p"+mcu # +mcu.replace("atmega","m") +avrdude_flags = "-V -p"+mcu # +mcu.replace("atmega","m") avrdude_flags = avrdude_flags + " -P "+upload_port avrdude_flags = avrdude_flags + " -c "+upload_prog avrdude_flags = avrdude_flags + " -b "+upload_baud diff --git a/v2/src/Motherboard/SConscript b/v2/src/Motherboard/SConscript index 2ab6cf8..c939f06 100644 --- a/v2/src/Motherboard/SConscript +++ b/v2/src/Motherboard/SConscript @@ -125,7 +125,7 @@ env.Elf(elf_name, objs) env.Hex(hex_name, elf_name) avrdude = avr_tools_path+"/avrdude" -avrdude_flags = "-V -F -p "+mcu.replace("atmega","m") +avrdude_flags = "-V -p "+mcu.replace("atmega","m") avrdude_flags = avrdude_flags + " -P "+upload_port avrdude_flags = avrdude_flags + " -c "+upload_prog avrdude_flags = avrdude_flags + " -b "+upload_baud From fcf60d9718cb8462ba1e4829d23fc11372565832 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Tue, 23 Nov 2010 15:53:37 -0500 Subject: [PATCH 21/61] Fix for external steppers so they hold torque on M103 commands. To release torque, issue a speed of 0 RPM --- v2/src/Extruder/MotorController.cc | 9 ++++++++- v2/src/Extruder/SConscript | 2 +- v2/src/Extruder/Version.hh | 4 ++++ v2/src/Extruder/boards/ecv22/ExtruderBoard.cc | 8 ++++++++ v2/src/Extruder/boards/ecv22/ExtruderBoard.hh | 5 +++++ v2/src/Extruder/boards/ecv22/ExtruderMotor.cc | 16 +++++++++++++++- v2/src/Extruder/boards/ecv22/ExtruderMotor.hh | 3 +++ 7 files changed, 44 insertions(+), 3 deletions(-) diff --git a/v2/src/Extruder/MotorController.cc b/v2/src/Extruder/MotorController.cc index 97ccddc..7ac68ea 100644 --- a/v2/src/Extruder/MotorController.cc +++ b/v2/src/Extruder/MotorController.cc @@ -79,7 +79,12 @@ void MotorController::update() { int new_speed = (!paused&&on)?(direction?speed:-speed):0; board.setMotorSpeed(new_speed); } else { +#ifdef DEFAULT_EXTERNAL_STEPPER + board.setMotorSpeedRPM(rpm, direction); + board.setMotorOn(!paused && on); +#else board.setMotorSpeedRPM((!paused&&on) ? rpm : 0, direction); +#endif } } @@ -108,7 +113,9 @@ void MotorController::setOn(bool on_in) { ExtruderBoard& board = ExtruderBoard::getBoard(); if (!on_in && on && direction && backoff_enabled && forward_trigger_timeout.hasElapsed()) { backoff_state = BO_HALT_1; - board.setMotorSpeed(0); + // Commented out since this is handled in MotorController::update(), + // kept around for future reference + // board.setMotorSpeed(0); current_operation_timeout.start(halt_ms*1000L); } else if (on_in) { if (!on && direction) { diff --git a/v2/src/Extruder/SConscript b/v2/src/Extruder/SConscript index 4b601f1..1961e8c 100644 --- a/v2/src/Extruder/SConscript +++ b/v2/src/Extruder/SConscript @@ -26,7 +26,7 @@ from os.path import dirname platform = ARGUMENTS.get('platform','ecv22') stepper = ARGUMENTS.get('stepper','false') relays = ARGUMENTS.get('relays','true') -extstepper = ARGUMENTS.get('extstepper','false') +extstepper = ARGUMENTS.get('extstepper','true') f_cpu='16000000L' default_baud = '19200' diff --git a/v2/src/Extruder/Version.hh b/v2/src/Extruder/Version.hh index 18e95ff..afda3d8 100644 --- a/v2/src/Extruder/Version.hh +++ b/v2/src/Extruder/Version.hh @@ -28,7 +28,11 @@ const uint16_t firmware_version = VERSION; #endif #ifndef BUILD_NAME +#ifdef DEFAULT_EXTERNAL_STEPPER +const char* const build_name = "Ext. Stepper"; +#else const char* const build_name = "Extruder"; +#endif #else const char* const build_name = BUILD_NAME; #endif diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc index ea87ee1..3392e99 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.cc @@ -290,3 +290,11 @@ ISR(TIMER2_OVF_vect) { ISR(TIMER2_COMPB_vect) { CHANNEL_A.setValue(false); } + +#ifdef DEFAULT_EXTERNAL_STEPPER +void ExtruderBoard::setMotorOn(bool on) +{ + setExtruderMotorOn(on); +} +#endif + diff --git a/v2/src/Extruder/boards/ecv22/ExtruderBoard.hh b/v2/src/Extruder/boards/ecv22/ExtruderBoard.hh index efbaf48..162cd11 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderBoard.hh +++ b/v2/src/Extruder/boards/ecv22/ExtruderBoard.hh @@ -44,6 +44,11 @@ public: Heater& getPlatformHeater() { return platform_heater; } void setMotorSpeed(int16_t speed); void setMotorSpeedRPM(uint32_t speed, bool direction); +#ifdef DEFAULT_EXTERNAL_STEPPER + // Hack to decouple holding torque from RPM speed + // Stops/starts the motor while holding torque + void setMotorOn(bool on); +#endif void setFan(bool on); void setValve(bool on); UART& getHostUART() { return UART::getHostUART(); } diff --git a/v2/src/Extruder/boards/ecv22/ExtruderMotor.cc b/v2/src/Extruder/boards/ecv22/ExtruderMotor.cc index adbb8be..495d20f 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderMotor.cc +++ b/v2/src/Extruder/boards/ecv22/ExtruderMotor.cc @@ -152,7 +152,8 @@ void setExtruderMotorRPM(uint32_t micros, bool direction) { //ext_stepper_counter = 0; // Timer/Counter 0 Output Compare A Match Interrupt On - TIMSK0 = _BV(OCIE1A); + // This is now done in setExtruderMotorOn() + // TIMSK0 = _BV(OCIE1A); external_dir_pin.setValue(direction); // true = forward external_enable_pin.setValue(false); // true = disabled @@ -169,6 +170,19 @@ void setExtruderMotorRPM(uint32_t micros, bool direction) { } +#ifdef DEFAULT_EXTERNAL_STEPPER +void setExtruderMotorOn(bool on) +{ + if (!external_stepper_motor_mode) return; + // Disable stepping but hold torque by disabling interrupt + if (on) { + TIMSK0 = _BV(OCIE1A); + } else { + TIMSK0 = 0; + } +} +#endif + // ## H-Bridge Stepper Driving using Timer0 Overflow ## const uint8_t hb1_en_pattern = 0xdd; diff --git a/v2/src/Extruder/boards/ecv22/ExtruderMotor.hh b/v2/src/Extruder/boards/ecv22/ExtruderMotor.hh index f1a326a..8b6f41b 100644 --- a/v2/src/Extruder/boards/ecv22/ExtruderMotor.hh +++ b/v2/src/Extruder/boards/ecv22/ExtruderMotor.hh @@ -30,5 +30,8 @@ void setStepperMode(bool mode, bool external = false); // Valid range: -255 through 255 void setExtruderMotor(int16_t speed); void setExtruderMotorRPM(uint32_t micros, bool direction); +#ifdef DEFAULT_EXTERNAL_STEPPER +void setExtruderMotorOn(bool on); +#endif #endif // BOARDS_ECV22_EXTRUDER_MOTOR_HH_ From b424a9c6e8fc0fdfca9518d7e898170b6ebf290f Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Tue, 23 Nov 2010 15:57:39 -0500 Subject: [PATCH 22/61] Set back default to be DC extruder --- v2/src/Extruder/SConscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/v2/src/Extruder/SConscript b/v2/src/Extruder/SConscript index 1961e8c..4b601f1 100644 --- a/v2/src/Extruder/SConscript +++ b/v2/src/Extruder/SConscript @@ -26,7 +26,7 @@ from os.path import dirname platform = ARGUMENTS.get('platform','ecv22') stepper = ARGUMENTS.get('stepper','false') relays = ARGUMENTS.get('relays','true') -extstepper = ARGUMENTS.get('extstepper','true') +extstepper = ARGUMENTS.get('extstepper','false') f_cpu='16000000L' default_baud = '19200' From a394851049b5d0c6fcc214569efd74cefbe4ba14 Mon Sep 17 00:00:00 2001 From: Marius Kintel Date: Tue, 23 Nov 2010 20:35:40 -0500 Subject: [PATCH 23/61] Upped speed of Gen 4 to 115200 bps --- v2/src/Motherboard/boards/rrmbv24/UART.cc | 20 +++++++++++--------- v2/src/Motherboard/boards/rrmbv24/UART.hh | 1 - 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/v2/src/Motherboard/boards/rrmbv24/UART.cc b/v2/src/Motherboard/boards/rrmbv24/UART.cc index 6b14e9a..3bc59db 100644 --- a/v2/src/Motherboard/boards/rrmbv24/UART.cc +++ b/v2/src/Motherboard/boards/rrmbv24/UART.cc @@ -24,25 +24,27 @@ // MEGA644P_DOUBLE_SPEED_MODE is 1 if USXn is 1. #ifndef MEGA644P_DOUBLE_SPEED_MODE -#define MEGA644P_DOUBLE_SPEED_MODE 0 +#define MEGA644P_DOUBLE_SPEED_MODE 1 #endif #if MEGA644P_DOUBLE_SPEED_MODE -#define UBRR_VALUE 51 -#define UBRRA_VALUE _BV(U2X##uart_) +#define UBRR0_VALUE 16 // 115200 baud +#define UBRR1_VALUE 51 // 38400 baud +#define UCSRA_VALUE(uart_) _BV(U2X##uart_) #else -#define UBRR_VALUE 25 -#define UBRRA_VALUE 0 +#define UBRR0_VALUE 8 // 115200 +#define UBRR1_VALUE 25 // 38400 baud +#define UCSRA_VALUE(uart_) 0 #endif -/// Adapted from ancient arduino/wiring rabbit hole +// Adapted from ancient arduino/wiring rabbit hole #define INIT_SERIAL(uart_) \ { \ - UBRR##uart_##H = UBRR_VALUE >> 8; \ - UBRR##uart_##L = UBRR_VALUE & 0xff; \ + UBRR##uart_##H = UBRR##uart_##_VALUE >> 8; \ + UBRR##uart_##L = UBRR##uart_##_VALUE & 0xff; \ \ /* set config for uart_ */ \ - UCSR##uart_##A = UBRRA_VALUE; \ + UCSR##uart_##A = UCSRA_VALUE(uart_); \ UCSR##uart_##B = _BV(RXEN##uart_) | _BV(TXEN##uart_); \ UCSR##uart_##C = _BV(UCSZ##uart_##1)|_BV(UCSZ##uart_##0); \ /* defaults to 8-bit, no parity, 1 stop bit */ \ diff --git a/v2/src/Motherboard/boards/rrmbv24/UART.hh b/v2/src/Motherboard/boards/rrmbv24/UART.hh index b9cfbb4..8d055c5 100644 --- a/v2/src/Motherboard/boards/rrmbv24/UART.hh +++ b/v2/src/Motherboard/boards/rrmbv24/UART.hh @@ -27,7 +27,6 @@ * call is made. beginSend() calls will send completed * packets. * - * All MB UARTs are presumed to run at 38400bps. */ class UART { private: From af6507ffce5e3b092dd7c4f6a3bb860b2f45a13c Mon Sep 17 00:00:00 2001 From: phooky Date: Fri, 19 Nov 2010 15:22:46 -0500 Subject: [PATCH 24/61] * Changed to our Pin() idiom. --- v2/src/Extruder/boards/ecv22/UART.cc | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/v2/src/Extruder/boards/ecv22/UART.cc b/v2/src/Extruder/boards/ecv22/UART.cc index 58699e0..c382877 100644 --- a/v2/src/Extruder/boards/ecv22/UART.cc +++ b/v2/src/Extruder/boards/ecv22/UART.cc @@ -61,14 +61,15 @@ UART::UART() : enabled(false) { UCSR0B = _BV(RXEN0) | _BV(TXEN0); UCSR0C = _BV(UCSZ01)|_BV(UCSZ00); /* defaults to 8-bit, no parity, 1 stop bit */ - TX_ENABLE_PIN.setDirection(true); - RX_ENABLE_PIN.setDirection(true); - - // pulup on RX - DDRD &= ~_BV(PIND0); - PORTD |= _BV(PIND0); - - listen(); + TX_ENABLE_PIN.setDirection(true); + RX_ENABLE_PIN.setDirection(true); + + // pulup on RX + Pin rxPin(PortD,0); + rxPin.setDirection(false); + rxPin.setValue(true); + + listen(); } // Reset the UART to a listening state. This is important for From b8992eeb0d09bca44ac57528a42a08b54cdb0d02 Mon Sep 17 00:00:00 2001 From: phooky Date: Tue, 30 Nov 2010 12:14:12 -0500 Subject: [PATCH 25/61] * Added default axis inversions to support the ordinary Thingomatic configuration. --- v2/src/Motherboard/boards/rrmbv24/Configuration.hh | 7 +++++++ v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc | 4 ++++ 2 files changed, 11 insertions(+) diff --git a/v2/src/Motherboard/boards/rrmbv24/Configuration.hh b/v2/src/Motherboard/boards/rrmbv24/Configuration.hh index 23cbb2b..df412ea 100644 --- a/v2/src/Motherboard/boards/rrmbv24/Configuration.hh +++ b/v2/src/Motherboard/boards/rrmbv24/Configuration.hh @@ -136,6 +136,13 @@ // The B stepper enable pin (active low) #define B_ENABLE_PIN Pin(PortH,3) +// Default axis inversion +// On the Thingomatic, the default configuration means that +// the X and Y axes run counter to what they should. By +// default we "invert" these axes. X and Y are axes 0 and 1, +// respectively. +#define DEFAULT_INVERSIONS ((1 << 1) | (1 << 0)) + // --- Debugging configuration --- // The pin which controls the debug LED (active high) #define DEBUG_PIN Pin(PortB,7) diff --git a/v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc b/v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc index 9447ac2..a35927d 100644 --- a/v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc +++ b/v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc @@ -17,6 +17,7 @@ #include "StepperInterface.hh" #include "EepromMap.hh" +#include "Configuration.hh" void StepperInterface::setDirection(bool forward) { if (invert_axis) forward = !forward; @@ -53,6 +54,9 @@ void StepperInterface::init(uint8_t idx) { enable_pin.setDirection(true); // get inversion characteristics uint8_t axes_invert = eeprom::getEeprom8(eeprom::AXIS_INVERSION, 1<<1); +#ifdef DEFAULT_INVERSIONS + axes_invert ^= DEFAULT_INVERSIONS; +#endif uint8_t endstops_invert = eeprom::getEeprom8(eeprom::ENDSTOP_INVERSION, 0); bool endstops_present = (endstops_invert & (1<<7)) != 0; // If endstops are not present, then we consider them inverted, since they will From 1998832165f428f85709ef3752da79e805bc5ff6 Mon Sep 17 00:00:00 2001 From: phooky Date: Tue, 30 Nov 2010 17:36:23 -0500 Subject: [PATCH 26/61] * Remove xy inversions --- v2/src/Motherboard/boards/rrmbv24/Configuration.hh | 7 ------- 1 file changed, 7 deletions(-) diff --git a/v2/src/Motherboard/boards/rrmbv24/Configuration.hh b/v2/src/Motherboard/boards/rrmbv24/Configuration.hh index df412ea..23cbb2b 100644 --- a/v2/src/Motherboard/boards/rrmbv24/Configuration.hh +++ b/v2/src/Motherboard/boards/rrmbv24/Configuration.hh @@ -136,13 +136,6 @@ // The B stepper enable pin (active low) #define B_ENABLE_PIN Pin(PortH,3) -// Default axis inversion -// On the Thingomatic, the default configuration means that -// the X and Y axes run counter to what they should. By -// default we "invert" these axes. X and Y are axes 0 and 1, -// respectively. -#define DEFAULT_INVERSIONS ((1 << 1) | (1 << 0)) - // --- Debugging configuration --- // The pin which controls the debug LED (active high) #define DEBUG_PIN Pin(PortB,7) From a29a9de76132ce81c744a13da1f234d741e9e833 Mon Sep 17 00:00:00 2001 From: phooky Date: Wed, 1 Dec 2010 11:35:52 -0500 Subject: [PATCH 27/61] Simplifying MBMBv2.4 naming (now just mb24) --- v2/src/Motherboard/SConscript | 5 ++++- v2/src/Motherboard/boards/{rrmbv24 => mb24}/Configuration.hh | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/LiquidCrystal.cc | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/LiquidCrystal.hh | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/Motherboard.cc | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/Motherboard.hh | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/PSU.cc | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/PSU.hh | 0 .../Motherboard/boards/{rrmbv24 => mb24}/StepperInterface.cc | 0 .../Motherboard/boards/{rrmbv24 => mb24}/StepperInterface.hh | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/UART.cc | 0 v2/src/Motherboard/boards/{rrmbv24 => mb24}/UART.hh | 0 12 files changed, 4 insertions(+), 1 deletion(-) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/Configuration.hh (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/LiquidCrystal.cc (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/LiquidCrystal.hh (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/Motherboard.cc (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/Motherboard.hh (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/PSU.cc (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/PSU.hh (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/StepperInterface.cc (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/StepperInterface.hh (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/UART.cc (100%) rename v2/src/Motherboard/boards/{rrmbv24 => mb24}/UART.hh (100%) diff --git a/v2/src/Motherboard/SConscript b/v2/src/Motherboard/SConscript index c939f06..9da1caa 100644 --- a/v2/src/Motherboard/SConscript +++ b/v2/src/Motherboard/SConscript @@ -56,12 +56,15 @@ def extract_version(f): version = extract_version(File('#/src/Motherboard/Version.hh')) +if (platform == 'rrmbv24'): + platform = 'mb24' + if (platform == 'rrmbv12'): default_baud = '38400' mcu='atmega644p' has_queue = 1 has_psu = 1 -elif (platform == 'rrmbv24'): +elif (platform == 'mb24'): default_baud = '57600' mcu='atmega1280' has_queue = 1 diff --git a/v2/src/Motherboard/boards/rrmbv24/Configuration.hh b/v2/src/Motherboard/boards/mb24/Configuration.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/Configuration.hh rename to v2/src/Motherboard/boards/mb24/Configuration.hh diff --git a/v2/src/Motherboard/boards/rrmbv24/LiquidCrystal.cc b/v2/src/Motherboard/boards/mb24/LiquidCrystal.cc similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/LiquidCrystal.cc rename to v2/src/Motherboard/boards/mb24/LiquidCrystal.cc diff --git a/v2/src/Motherboard/boards/rrmbv24/LiquidCrystal.hh b/v2/src/Motherboard/boards/mb24/LiquidCrystal.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/LiquidCrystal.hh rename to v2/src/Motherboard/boards/mb24/LiquidCrystal.hh diff --git a/v2/src/Motherboard/boards/rrmbv24/Motherboard.cc b/v2/src/Motherboard/boards/mb24/Motherboard.cc similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/Motherboard.cc rename to v2/src/Motherboard/boards/mb24/Motherboard.cc diff --git a/v2/src/Motherboard/boards/rrmbv24/Motherboard.hh b/v2/src/Motherboard/boards/mb24/Motherboard.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/Motherboard.hh rename to v2/src/Motherboard/boards/mb24/Motherboard.hh diff --git a/v2/src/Motherboard/boards/rrmbv24/PSU.cc b/v2/src/Motherboard/boards/mb24/PSU.cc similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/PSU.cc rename to v2/src/Motherboard/boards/mb24/PSU.cc diff --git a/v2/src/Motherboard/boards/rrmbv24/PSU.hh b/v2/src/Motherboard/boards/mb24/PSU.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/PSU.hh rename to v2/src/Motherboard/boards/mb24/PSU.hh diff --git a/v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc b/v2/src/Motherboard/boards/mb24/StepperInterface.cc similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/StepperInterface.cc rename to v2/src/Motherboard/boards/mb24/StepperInterface.cc diff --git a/v2/src/Motherboard/boards/rrmbv24/StepperInterface.hh b/v2/src/Motherboard/boards/mb24/StepperInterface.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/StepperInterface.hh rename to v2/src/Motherboard/boards/mb24/StepperInterface.hh diff --git a/v2/src/Motherboard/boards/rrmbv24/UART.cc b/v2/src/Motherboard/boards/mb24/UART.cc similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/UART.cc rename to v2/src/Motherboard/boards/mb24/UART.cc diff --git a/v2/src/Motherboard/boards/rrmbv24/UART.hh b/v2/src/Motherboard/boards/mb24/UART.hh similarity index 100% rename from v2/src/Motherboard/boards/rrmbv24/UART.hh rename to v2/src/Motherboard/boards/mb24/UART.hh From f5abb9d2058b498ee7810deaa4ef000d449b4007 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 5 Oct 2011 21:40:06 -0500 Subject: [PATCH 28/61] Fix case where bounce detection would make things worse. --- firmware/src/shared/StepperAxis.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index c69bce1..8095c22 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -58,7 +58,8 @@ bool StepperAxis::checkEndstop(const bool isHoming) { #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) bool hit_endstop = direction ? interface->isAtMaximum() : interface->isAtMinimum(); // We must move at least ENDSTOP_DEBOUNCE from where we hit the endstop before we declare traveling - if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status != ESS_TRAVELING)) { + if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM))) { + hit_endstop = true; // Did we *just* hit the endstop? if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { endstop_play = ENDSTOP_DEFAULT_PLAY; From 59ef0f1e5e20a3344105dc7849f6b055ecc24126 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 9 Nov 2011 16:22:31 -0600 Subject: [PATCH 29/61] Updated SConstruct/SConscript files to handle 5d-shield add-ons better. Also, changed the build_all.sh script. --- firmware/SConstruct | 4 ++++ firmware/build_all.sh | 34 +++++++++++------------------ firmware/src/SConscript.motherboard | 5 ++++- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/firmware/SConstruct b/firmware/SConstruct index 9247733..1c3f1ee 100644 --- a/firmware/SConstruct +++ b/firmware/SConstruct @@ -1,6 +1,10 @@ platform = ARGUMENTS.get('platform','mb24') +fived = ARGUMENTS.get('fived','false') if (platform == 'rrmbv12' or platform == 'mb24' or platform == 'mb24-2560'): + if (fived == 'true'): + platform = platform+'-5d' + SConscript(['src/SConscript.motherboard'], variant_dir='build/'+platform) elif (platform == 'ecv22' or platform == 'ecv34'): diff --git a/firmware/build_all.sh b/firmware/build_all.sh index e50cb15..edb186b 100755 --- a/firmware/build_all.sh +++ b/firmware/build_all.sh @@ -2,33 +2,27 @@ SCONS=SConstruct -PLATFORMS=( rrmbv12 mb24 mb24-2560 ecv22 ecv34 ) +PLATFORMS=( rrmbv12 'rrmbv12 fived=true' mb24 mb24-2560 ecv22 ecv34 ) LOG_FILE=build_all_output function build_firmware { - platform_list_name="$1[*]" - platform_list=(${!platform_list_name}) - scons_file=$2 - - for platform in ${platform_list[@]} + for platform in "${PLATFORMS[@]}" do - echo -n "Building firmware for ${platform}... " - - echo -e "\n\n\n\n" >> ${LOG_FILE} - echo Building firmware for ${platform} >> ${LOG_FILE} - - scons -f ${scons_file} platform=${platform} >> ${LOG_FILE} 2>&1 + echo -n "Building firmware for ${platform}... " - if [ "$?" -ne "0" ]; then - echo Failure - else - echo Success - fi -done + echo -e "\n\n\n\n" >> ${LOG_FILE} + echo Building firmware for ${platform} >> ${LOG_FILE} + scons -f "${SCONS}" platform=${platform} >> ${LOG_FILE} 2>&1 + if [ "$?" -ne "0" ]; then + echo Failure + else + echo Success + fi + done } @@ -51,8 +45,6 @@ function build_documentation { echo Building all firmware echo "Building all firmware" > ${LOG_FILE} -build_firmware PLATFORMS ${SCONS} - - +build_firmware build_documentation diff --git a/firmware/src/SConscript.motherboard b/firmware/src/SConscript.motherboard index 039b084..12ffb55 100644 --- a/firmware/src/SConscript.motherboard +++ b/firmware/src/SConscript.motherboard @@ -61,8 +61,11 @@ vstr = File('#/current_version.txt').get_contents().strip() vstr = ARGUMENTS.get('version',vstr) version = parse_version(vstr) +subplatform = "" +if (fived == 'true'): + subplatform = "-5d" -target_name = "MB-"+platform+"-v"+str(version//100)+"."+str(version%100) +target_name = "MB-"+platform+subplatform+"-v"+str(version//100)+"."+str(version%100) if (platform == 'rrmbv12'): default_baud = '38400' From 6337f88e96233201010e1a99f7bd235437cea4ce Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Tue, 13 Dec 2011 22:55:31 -0600 Subject: [PATCH 30/61] A good start toward acceleration, again. --- firmware/src/Motherboard/Command.cc | 103 ++-- firmware/src/Motherboard/Planner.cc | 557 ++++++++++++++++++ firmware/src/Motherboard/Planner.hh | 193 ++++++ firmware/src/Motherboard/Point.cc | 74 ++- firmware/src/Motherboard/Point.hh | 11 + firmware/src/Motherboard/Steppers.cc | 240 ++++++-- firmware/src/Motherboard/Steppers.hh | 2 + .../Motherboard/boards/mb24/Configuration.hh | 12 + .../Motherboard/boards/mb24/Motherboard.cc | 16 + .../boards/rrmbv12/Configuration.hh | 12 + .../Motherboard/boards/rrmbv12/Motherboard.cc | 16 + firmware/src/shared/StepperAxis.cc | 122 ++-- firmware/src/shared/StepperAxis.hh | 6 + 13 files changed, 1230 insertions(+), 134 deletions(-) create mode 100644 firmware/src/Motherboard/Planner.cc create mode 100644 firmware/src/Motherboard/Planner.hh diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 91a6865..54eefd4 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -17,6 +17,7 @@ #include "Command.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Commands.hh" #include "Tool.hh" #include "Configuration.hh" @@ -111,6 +112,53 @@ void reset() { mode = READY; } +// Handle movement comands -- called from a few places +void handleMovementCommand(uint8_t &command) { + // if we're already moving, check to make sure the buffer isn't full + if (mode == MOVING && planner::block_buffer.isFull()) { + return; // we'll be back! + } + if (command == HOST_CMD_QUEUE_POINT_ABS) { + // check for completion + if (command_buffer.getLength() >= 17) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t dda = pop32(); + planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA + } + } else if (command == HOST_CMD_QUEUE_POINT_EXT) { + // check for completion + if (command_buffer.getLength() >= 25) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t dda = pop32(); + planner::addMoveToBuffer(Point(x,y,z,a,b), dda); + } + } else if (command == HOST_CMD_QUEUE_POINT_NEW) { + // check for completion + if (command_buffer.getLength() >= 26) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t us = pop32(); + uint8_t relative = pop8(); + //steppers::setTargetNew(Point(x,y,z,a,b),us,relative); + } + } +} + // A fast slice for processing commands and refilling the stepper queue, etc. void runCommandSlice() { if (sdcard::isPlaying()) { @@ -128,7 +176,16 @@ void runCommandSlice() { } } if (mode == MOVING) { - if (!steppers::isRunning()) { mode = READY; } + if (!steppers::isRunning()) { + mode = READY; + } else { + if (command_buffer.getLength() > 0) { + uint8_t command = command_buffer[0]; + if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + handleMovementCommand(command); + } + } + } } if (mode == DELAY) { // check timers @@ -185,44 +242,8 @@ void runCommandSlice() { // process next command on the queue. if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; - if (command == HOST_CMD_QUEUE_POINT_ABS) { - // check for completion - if (command_buffer.getLength() >= 17) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t dda = pop32(); - steppers::setTarget(Point(x,y,z),dda); - } - } else if (command == HOST_CMD_QUEUE_POINT_EXT) { - // check for completion - if (command_buffer.getLength() >= 25) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); - int32_t dda = pop32(); - steppers::setTarget(Point(x,y,z,a,b),dda); - } - } else if (command == HOST_CMD_QUEUE_POINT_NEW) { - // check for completion - if (command_buffer.getLength() >= 26) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); - int32_t us = pop32(); - uint8_t relative = pop8(); - steppers::setTargetNew(Point(x,y,z,a,b),us,relative); - } + if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + handleMovementCommand(command); } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { command_buffer.pop(); // remove the command code @@ -246,7 +267,7 @@ void runCommandSlice() { int32_t x = pop32(); int32_t y = pop32(); int32_t z = pop32(); - steppers::definePosition(Point(x,y,z)); + planner::definePosition(Point(x,y,z)); } } else if (command == HOST_CMD_SET_POSITION_EXT) { // check for completion @@ -257,7 +278,7 @@ void runCommandSlice() { int32_t z = pop32(); int32_t a = pop32(); int32_t b = pop32(); - steppers::definePosition(Point(x,y,z,a,b)); + planner::definePosition(Point(x,y,z,a,b)); } } else if (command == HOST_CMD_DELAY) { if (command_buffer.getLength() >= 5) { diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc new file mode 100644 index 0000000..1bf1124 --- /dev/null +++ b/firmware/src/Motherboard/Planner.cc @@ -0,0 +1,557 @@ +/* + * Copyright 2011 by Rob Giseburt http://tinkerin.gs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see + */ + +/* + * This is heavily influenced by the Marlin RepRap firmware + * (https://github.com/ErikZalm/Marlin) which is derived from + * the Grbl firmware (https://github.com/simen/grbl/tree). + */ + + +/* + Reasoning behind the mathematics in this module (in the key of 'Mathematica'): + + s == speed, a == acceleration, t == time, d == distance + + Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + + Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + + Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + + When to start braking (di) to reach a specified destionation speed (s2) after accelerating + from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) +*/ + + +#include "Planner.hh" +#include +#include +#include +#include // for memmove and memcpy + +#include "Steppers.hh" + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 +#define A_AXIS 3 +#define B_AXIS 4 + +#define FORCE_INLINE __attribute__((always_inline)) inline + +/* Setup some utilities */ + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +inline const int32_t& min(const int32_t& a, const int32_t& b) { return (a)<(b)?(a):(b); } +inline const int32_t& max(const int32_t& a, const int32_t& b) { return (a)>(b)?(a):(b); } + +template +inline T abs(T x) { return (x)>0?(x):-(x); } + +// #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) +// #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +// #define radians(deg) ((deg)*DEG_TO_RAD) +// #define degrees(rad) ((rad)*RAD_TO_DEG) +// #define sq(x) ((x)*(x)) + + +namespace planner { + + // this is very similar to the StepperAxis, but geared toward planning + struct PlannerAxis + { + // how many steps does it take to go a mm (RepG should tell us this during init) + float steps_per_mm; + + // how fast can we go, in mm/s (RepG should have already limited this, disabling) + // float max_feedrate; + + // maximum acceleration for this axis in steps/s^2 (should be in EEPROM) + uint32_t max_acceleration; + + // the maximum amount of speed change allowable for this axis + // note that X+Y has it's own setting, and this if for all the rest + float max_axis_jerk; + }; + + PlannerAxis axes[AXIS_COUNT]; + + float acceleration; + Point position; // the current position (planning-wise, not bot/stepper-wise) in steps + float previous_speed[AXIS_COUNT]; // Speed of previous path line segment + float previous_nominal_speed; // Nominal speed of previous path line segment + static float max_xy_jerk; + + Block block_buffer_data[BLOCK_BUFFER_SIZE]; + ReusingCircularBufferTempl block_buffer(BLOCK_BUFFER_SIZE, block_buffer_data); + + + void init() + { + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i] = PlannerAxis(); // redundant, or a reset? + previous_speed[i] = 0.0; + } + + position = Point(0,0,0,0,0); + previous_nominal_speed = 0.0; + } + + + void setMaxAxisJerk(float jerk, uint8_t axis) { + axes[axis].max_axis_jerk = jerk; + } + + void setMaxXYJerk(float jerk) { + max_xy_jerk = jerk; + } + + void setAxisStepsPerMM(float steps_per_mm, uint8_t axis) { + axes[axis].steps_per_mm = steps_per_mm; + } + + void setAcceleration(float new_acceleration) { + acceleration = new_acceleration; + for (int i = 1; i < AXIS_COUNT; i++) { + axes[i].max_acceleration = acceleration * axes[i].steps_per_mm; + } + } + + // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the + // acceleration within the allotted distance. + FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { + return sqrt(target_velocity*target_velocity-2*acceleration*distance); + } + + // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the + // given acceleration: + FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) + { + if (acceleration!=0) { + return((target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration)); + } + else { + return 0.0; // acceleration was 0, set acceleration distance to 0 + } + } + + // This function gives you the point at which you must start braking (at the rate of -acceleration) if + // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after + // a total travel of distance. This can be used to compute the intersection point between acceleration and + // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) + + FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) + { + if (acceleration!=0) { + return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration)); + } + else { + return 0.0; // acceleration was 0, set intersection distance to 0 + } + } + + // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. + void Block::calculate_trapezoid(float exit_factor_speed) { + float entry_factor = entry_speed/nominal_speed; + float exit_factor = exit_factor_speed/nominal_speed; + + initial_rate = ceil(nominal_rate*entry_factor); // (step/min) + final_rate = ceil(nominal_rate*exit_factor); // (step/min) + + // Limit minimal step rate (Otherwise the timer will overflow.) + if(initial_rate <120) + initial_rate=120; + if(final_rate < 120) + final_rate=120; + + int32_t acceleration = acceleration_st; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(initial_rate, nominal_rate, acceleration)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(nominal_rate, final_rate, -acceleration)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(initial_rate, final_rate, acceleration, step_event_count)); + accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off + accelerate_steps = min(accelerate_steps, step_event_count); + plateau_steps = 0; + } + + #ifdef ADVANCE + long initial_advance = advance*entry_factor*entry_factor; + long final_advance = advance*exit_factor*exit_factor; + #endif // ADVANCE + + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section + if(busy == false) { // Don't update variables if block is busy. + accelerate_until = accelerate_steps; + decelerate_after = accelerate_steps+plateau_steps; + // initial_rate = initial_rate; + // final_rate = final_rate; + #ifdef ADVANCE + initial_advance = initial_advance; + final_advance = final_advance; + #endif //ADVANCE + } // So, ummm, what if it IS busy?! + } // ISR state will be automatically restored here + } + + // forward declare, so we can order the code in a slightly more readable fashion + void planner_reverse_pass_kernel(Block *previous, Block *current, Block *next); + void planner_reverse_pass(); + void planner_forward_pass_kernel(Block *previous, Block *current, Block *next); + void planner_forward_pass(); + void planner_recalculate_trapezoids(); + + // Recalculates the motion plan according to the following algorithm: + // + // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. Block.entry_factor) + // so that: + // a. The junction jerk is within the set limit + // b. No speed reduction within one block requires faster deceleration than the one, true constant + // acceleration. + // 2. Go over every block in chronological order and dial down junction speed reduction values if + // a. The speed increase within one block would require faster accelleration than the one, true + // constant acceleration. + // + // When these stages are complete all blocks have an entry_factor that will allow all speed changes to + // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than + // the set limit. Finally it will: + // + // 3. Recalculate trapezoids for all blocks. + + void planner_recalculate() { + planner_reverse_pass(); + planner_forward_pass(); + planner_recalculate_trapezoids(); + } + + // The kernel called by planner_recalculate() when scanning the plan from last to first entry. + void planner_reverse_pass_kernel(Block *previous, Block *current, Block *next) { + if(!current) { return; } + + if (next) { + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + if (current->entry_speed != current->max_entry_speed) { + + // If nominal length true, max junction speed is guaranteed to be reached. Only compute + // for max allowable speed if block is decelerating and nominal length is false. + if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { + current->entry_speed = min( current->max_entry_speed, + max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); + } else { + current->entry_speed = current->max_entry_speed; + } + current->recalculate_flag = true; + + } + } // Skip last block. Already initialized and set for recalculation. + } + + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This + // implements the reverse pass. + void planner_reverse_pass() { + if(block_buffer.getUsedCount() > 3) { + uint8_t block_index = block_buffer.getHeadIndex(); + Block *block[3] = { NULL, NULL, NULL }; + while(block_index != block_buffer.getTailIndex()) { + block_index = block_buffer.getPreviousIndex(block_index); + // block[2] = block[1]; + // block[1] = block[0]; + // Move two blocks worth of ram, from [0] to [1], using the overlap-safe memmove + memmove(block[0], block[1], sizeof(Block)<<1); + block[0] = &block_buffer[block_index]; + planner_reverse_pass_kernel(block[0], block[1], block[2]); + } + } + } + + // The kernel called by planner_recalculate() when scanning the plan from first to last entry. + void planner_forward_pass_kernel(Block *previous, Block *current, Block *next) { + if(!previous) { return; } + + // If the previous block is an acceleration block, but it is not long enough to complete the + // full speed change within the block, we need to adjust the entry speed accordingly. Entry + // speeds have already been reset, maximized, and reverse planned by reverse planner. + // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. + if (!previous->nominal_length_flag) { + if (previous->entry_speed < current->entry_speed) { + double entry_speed = min( current->entry_speed, + max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); + + // Check for junction speed change + if (current->entry_speed != entry_speed) { + current->entry_speed = entry_speed; + current->recalculate_flag = true; + } + } + } + } + + // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This + // implements the forward pass. + void planner_forward_pass() { + uint8_t block_index = block_buffer.getTailIndex(); + Block *block[3] = { NULL, NULL, NULL }; + + while(block_index != block_buffer.getHeadIndex()) { + // block[0] = block[1]; + // block[1] = block[2]; + // Move two blocks worth of ram, from [1] to [0], using the overlap-safe memmove + memmove(block[1], block[0], sizeof(Block)<<1); + block[2] = &block_buffer[block_index]; + planner_forward_pass_kernel(block[0],block[1],block[2]); + block_index = block_buffer.getNextIndex(block_index); + } + planner_forward_pass_kernel(block[1], block[2], NULL); + } + + // Recalculates the trapezoid speed profiles for all blocks in the plan according to the + // entry_factor for each junction. Must be called by planner_recalculate() after + // updating the blocks. + void planner_recalculate_trapezoids() { + int8_t block_index = block_buffer.getTailIndex(); + Block *current; + Block *next = NULL; + + while(block_index != block_buffer.getHeadIndex()) { + current = next; + next = &block_buffer[block_index]; + if (current) { + // Recalculate if current block entry or exit junction speed has changed. + if (current->recalculate_flag || next->recalculate_flag) { + // NOTE: Entry and exit factors always > 0 by all previous logic operations. + current->calculate_trapezoid(next->entry_speed); + current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + } + } + block_index = block_buffer.getNextIndex( block_index ); + } + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + if(next != NULL) { + next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + next->recalculate_flag = false; + } + } + + + // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly + bool addMoveToBuffer(const Point& target, int32_t us_per_step) + { + if (block_buffer.isFull()) + return false; + + Block *block = block_buffer.getHead(); + // Mark block as not busy (Not executed by the stepper interrupt) + block->busy = false; + + // calculate the difference between the current position and the target + Point delta = target - position; + + // set the number of steps and direction for each axis + block->steps = delta; + + // store the absolute number of steps in each direction, without direction + Point steps = delta.abs(); + + block->step_event_count = 0; + uint8_t master_axis = 0; + for (int i = 1; i < AXIS_COUNT; i++) { + if (steps[i] > block->step_event_count) { + block->step_event_count = steps[i]; + master_axis = i; + + // WARNING, Edge case: Two axis got the same number of steps, but have different steps_per_mm values + // No way to tell which one to choose. + // Need to change the call interface to fix this, though. + } + } + if (axes[master_axis].steps_per_mm == 0) { + Motherboard::getBoard().indicateError(4); + } + float feed_rate = (us_per_step * block->step_event_count)/axes[master_axis].steps_per_mm; // mm/second + if (feed_rate == 0) + return true; // we did not care, but yes, we did something with this + + // // Compute direction bits for this block + // block->direction_bits = 0; + // for (int i = 0; i < AXIS_COUNT; i++) { + // if (target[i] < position[i]) { block->direction_bits |= (1<millimeters = 0; + for (int i = 0; i < AXIS_COUNT; i++) { + delta_mm[i] = delta[i]/axes[i].steps_per_mm; + block->millimeters += delta_mm[i] * delta_mm[i]; + } + block->millimeters = sqrt(block->millimeters); + + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + + // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. + float inverse_second = feed_rate * inverse_millimeters; + + block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 + block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 + + // TODO make sure we are going the minimum speed, at least + // if(feed_rate max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + // } + + // TODO fancy frequency checks + + // Compute and limit the acceleration rate for the trapezoid generator. + float steps_per_mm = block->step_event_count/block->millimeters; + block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + // Limit acceleration per axis + for(int i=0; i < AXIS_COUNT; i++) { + if(((float)block->acceleration_st * (float)steps[i] / (float)block->step_event_count) > axes[i].max_acceleration) + block->acceleration_st = axes[i].max_acceleration; + } + block->acceleration = block->acceleration_st / steps_per_mm; + block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? + + // Compute the speed trasitions, or "jerks" + // Start with a safe speed + float vmax_junction = max_xy_jerk/2; + { + float half_max_z_axis_jerk = axes[Z_AXIS].max_axis_jerk/2; + if(abs(current_speed[Z_AXIS]) > half_max_z_axis_jerk) + vmax_junction = half_max_z_axis_jerk; + } + + vmax_junction = min(vmax_junction, block->nominal_speed); + if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + vmax_junction = block->nominal_speed; + } + if (jerk > max_xy_jerk) { + vmax_junction *= (max_xy_jerk/jerk); + } + + // account for Z, A, and B + for(int i=Z_AXIS; i < AXIS_COUNT; i++) { + float axis_jerk = abs(current_speed[i] - previous_speed[i]); + if(axis_jerk > axes[i].max_axis_jerk) { + vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); + } + } + } + block->max_entry_speed = vmax_junction; + + // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. + double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + block->entry_speed = min(vmax_junction, v_allowable); + + // Initialize planner efficiency flags + // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. + // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then + // the current block and next block junction speeds are guaranteed to always be at their maximum + // junction speeds in deceleration and acceleration, respectively. This is due to how the current + // block nominal speed limits both the current and next maximum junction speeds. Hence, in both + // the reverse and forward planners, the corresponding block junction speed will always be at the + // the maximum junction speed and may always be ignored for any speed reduction checks. + if (block->nominal_speed <= v_allowable) + block->nominal_length_flag = true; + else + block->nominal_length_flag = false; + block->recalculate_flag = true; // Always calculate trapezoid for new block + + // Update previous path unit_vector and nominal speed + memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + previous_nominal_speed = block->nominal_speed; + + block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + + // Update position + memcpy(&position, &target, sizeof(target)); // position[] = target[] + + // Move buffer head + block_buffer++; + + planner_recalculate(); + + steppers::getNextMove(); + return true; + } + + void startHoming(const bool maximums, + const uint8_t axes_enabled, + const uint32_t us_per_step) + { + + } + + void definePosition(const Point& new_position) + { + position = new_position; + steppers::definePosition(new_position); + + // reset speed + for (int i = 0; i < AXIS_COUNT; i++) { + previous_speed[i] = 0.0; + } + } + + const Point getPosition() + { + return position; + } +} \ No newline at end of file diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh new file mode 100644 index 0000000..e759011 --- /dev/null +++ b/firmware/src/Motherboard/Planner.hh @@ -0,0 +1,193 @@ +/* + * Copyright 2011 by Rob Giseburt http://tinkerin.gs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see + */ + +/* + * This is heavily influenced by the Marlin RepRap firmware + * (https://github.com/ErikZalm/Marlin) which is derived from + * the Grbl firmware (https://github.com/simen/grbl/tree). + */ + +/* In this implenmentation, the motor control is handled by steppers, but this code does the planning. */ + +#ifndef PLANNER_HH +#define PLANNER_HH + +#include "Configuration.hh" +#include +#include "CircularBuffer.hh" +#include "Point.hh" + +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec) + +namespace planner { + // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in + // the source g-code and may never actually be reached if acceleration management is active. + typedef struct { + // Fields used by the bresenham algorithm for tracing the line + Point steps; // Step count and direction (may be negative) along each axis + uint32_t step_event_count; // The number of step events required to complete this block + int32_t accelerate_until; // The index of the step event on which to stop acceleration + int32_t decelerate_after; // The index of the step event on which to start decelerating + int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + uint8_t direction_bits; // The direction bit set for this block + uint8_t active_extruder; // Selects the active extruder + #ifdef ADVANCE + int32_t advance_rate; + volatile int32_t initial_advance; + volatile int32_t final_advance; + float advance; + #endif + + // Fields used by the motion planner to manage acceleration + // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis + float nominal_speed; // The nominal speed for this block in mm/min + float entry_speed; // Entry speed at previous-current junction in mm/min + float max_entry_speed; // Maximum allowable junction entry speed in mm/min + float millimeters; // The total travel of this block in mm + float acceleration; // acceleration mm/sec^2 + uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction + uint8_t nominal_length_flag; // Planner flag for nominal speed always reached + + // Settings for the trapezoid generator + uint32_t nominal_rate; // The nominal step rate for this block in step_events/sec + uint32_t initial_rate; // The jerk-adjusted step rate at start of block + uint32_t final_rate; // The minimal rate at exit + uint32_t acceleration_st; // acceleration steps/sec^2 + uint8_t busy; + + // functions + void calculate_trapezoid(float exit_factor_speed); + } Block; + + /// Initilaize the planner data structures + void init(); + + /// Buffer a movement to the target point (in step-space), with us_per_step gaps between steps + /// \param[in] target New position to move to, in step-space + /// \param[in] us_per_step Homing speed, in us per step + /// \return If the move was buffered + bool addMoveToBuffer(const Point& target, int32_t us_per_step); + + /// Home one or more axes + /// \param[in] maximums If true, home in the positive direction + /// \param[in] axes_enabled Bitfield specifiying which axes to + /// home + /// \param[in] us_per_step Homing speed, in us per step + void startHoming(const bool maximums, + const uint8_t axes_enabled, + const uint32_t us_per_step); + + /// Reset the current system position to the given point + /// \param[in] position New system position + void definePosition(const Point& position); + + /// Get the current system position + /// \return The current machine position. + const Point getPosition(); + + void setMaxXYJerk(float jerk); + void setMaxAxisJerk(float jerk, uint8_t axis); + + void setAcceleration(float acceleration); + + void setAxisStepsPerMM(float steps_per_mm, uint8_t axis); + + // Super-simple circular buffer, where old nodes are reused + // TODO: Move to a seperate file + // WARNING WARNING WARNING: If the size of this buffer is not in the following list this WILL FAIL BADLY! + // (2, 4, 8, 16, 32, 64, 128) + template + class ReusingCircularBufferTempl + { + public: + typedef T BufDataType; + + private: + volatile int8_t head, tail; + int8_t size; + int8_t size_mask; + BufDataType* const data; /// Pointer to buffer data + + public: + ReusingCircularBufferTempl(int8_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size), size_mask(size-1), data(buffer_in) {}; + + inline BufDataType *getHead() { + return &data[head]; + } + inline int8_t getHeadIndex() { + return head; + } + + inline BufDataType *getTail() { + return &data[tail]; + } + inline int8_t getTailIndex() { + return tail; + } + + inline int8_t getNextIndex(int8_t from) { + return (from + 1) & size_mask; + } + + inline int8_t getPreviousIndex(int8_t from) { + return (from - 1) & size_mask; + } + + inline BufDataType *getNextHead() { + return &data[getNextIndex(head)]; + } + + inline BufDataType &operator[] (int8_t index) { + // adding size should make negative indexes < size work ok + int8_t offset = (index + head + size) & size_mask; + return data[offset]; + } + + // bump the head with buffer++. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void operator++(int) { + head = getNextIndex(head); + } + + // bump the tail with buffer--. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void operator--(int) { + tail = getNextIndex(tail); + } + + inline bool isEmpty() { + return head == tail; + } + + inline bool isFull() { + return getNextIndex(head) == tail; + } + + inline int8_t getUsedCount() { + return ((head-tail+size) & size_mask); + } + }; + + // Create a circular buffer of blocks to walk + extern Block block_buffer_data[]; + extern ReusingCircularBufferTempl block_buffer; +} + +#endif /* end of include guard: PLANNER_HH */ diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 2184103..08c0fc2 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -1,33 +1,83 @@ #include "Point.hh" +/* Setup some utilities -- TODO: Move this to a common file */ + +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +template +inline T abs(T x) { return (x)>0?(x):-(x); } + +/* end utilities */ + Point::Point() { } +Point::Point(const Point &other) +{ + coordinates[0] = other.coordinates[0]; + coordinates[1] = other.coordinates[1]; + coordinates[2] = other.coordinates[2]; +#if AXIS_COUNT > 3 + coordinates[3] = other.coordinates[3]; + coordinates[4] = other.coordinates[4]; +#endif +} + + Point::Point(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b) { - coordinates[0] = x; - coordinates[1] = y; - coordinates[2] = z; + coordinates[0] = x; + coordinates[1] = y; + coordinates[2] = z; #if AXIS_COUNT > 3 - coordinates[3] = a; - coordinates[4] = b; + coordinates[3] = a; + coordinates[4] = b; #endif } Point::Point(int32_t x, int32_t y, int32_t z) { - coordinates[0] = x; - coordinates[1] = y; - coordinates[2] = z; + coordinates[0] = x; + coordinates[1] = y; + coordinates[2] = z; #if AXIS_COUNT > 3 - coordinates[3] = 0; - coordinates[4] = 0; + coordinates[3] = 0; + coordinates[4] = 0; #endif } const int32_t& Point::operator[](unsigned int index) const { - return coordinates[index]; + return coordinates[index]; } int32_t& Point::operator[](unsigned int index) { - return coordinates[index]; + return coordinates[index]; +} + +// const Point &operator-(const Point &a, const Point &b) { +// Point c = Point(a); +// return c -= b; +// } + +/// Subtraction operator, for fast deltas +const Point &Point::operator-(const Point &other) const { + Point c = Point(); + for (uint8_t i = 0; i < 5; i++) { + c.coordinates[i] = coordinates[i] - other.coordinates[i]; + } + return c; } + +Point Point::abs() { + Point absPoint = Point(); + absPoint.coordinates[0] = ::abs(coordinates[0]); + absPoint.coordinates[1] = ::abs(coordinates[1]); + absPoint.coordinates[2] = ::abs(coordinates[2]); +#if AXIS_COUNT > 3 + absPoint.coordinates[3] = ::abs(coordinates[3]); + absPoint.coordinates[4] = ::abs(coordinates[4]); +#endif + return absPoint; +} \ No newline at end of file diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index f20a679..98c3449 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -16,6 +16,9 @@ public: /// Default point constructor Point(); + /// Copy Point constructor + Point(const Point &other); + /// Construct a point with the given cooridnates. Coordinates are in /// stepper steps. /// \param[in] x X axis position @@ -48,6 +51,14 @@ public: /// \return Reference to the variable containing the axis' position. int32_t& operator[](unsigned int index); + /// Subtraction operator, for fast deltas + const Point &operator-(const Point &other) const; + + // friend const Point &operator-(const Point &a, const Point &b); + + + /// Absolute value -- convert all point to positive + Point abs(); } __attribute__ ((__packed__)); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 23707a8..12dd160 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -18,14 +18,22 @@ #define __STDC_LIMIT_MACROS #include "Steppers.hh" #include "StepperAxis.hh" +#include "Planner.hh" #include namespace steppers { +#define INTERVAL_IN_MICROSECONDS_X_16 (INTERVAL_IN_MICROSECONDS<<4) volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; +volatile int32_t timer_counter; +// The ALL_AXIS_COUNT includes the virtual "speed" axis +#define ALL_AXIS_COUNT STEPPER_COUNT+1 + +// STEPRATE_AXIS is the number of the virtual axis that control the steprate +#define STEPRATE_AXIS STEPPER_COUNT StepperAxis axes[STEPPER_COUNT]; volatile bool is_homing; @@ -39,8 +47,10 @@ bool isRunning() { void init(Motherboard& motherboard) { is_running = false; for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i] = StepperAxis(motherboard.getStepperInterface(i)); + axes[i] = StepperAxis(motherboard.getStepperInterface(i)); } + + axes[STEPRATE_AXIS] = StepperAxis(); } void abort() { @@ -68,47 +78,198 @@ void setHoldZ(bool holdZ_in) { holdZ = holdZ_in; } +/* void setTarget(const Point& target, int32_t dda_interval) { - int32_t max_delta = 0; - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].setTarget(target[i], false); - const int32_t delta = axes[i].delta; - // Only shut z axis on inactivity - if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - else if (delta != 0) axes[i].enableStepper(true); - if (delta > max_delta) { - max_delta = delta; + int32_t max_delta = 0; + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i].setTarget(target[i], false); + const int32_t delta = axes[i].delta; + // Only shut z axis on inactivity + if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); + else if (delta != 0) axes[i].enableStepper(true); + if (delta > max_delta) { + max_delta = delta; + } + } + // compute number of intervals for this move + intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); + intervals_remaining = intervals; + const int32_t negative_half_interval = -intervals / 2; + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i].counter = negative_half_interval; + } + is_running = true; +} + +void setTargetNew(const Point& target, int32_t us, uint8_t relative) { + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i].setTarget(target[i], (relative & (1 << i)) != 0); + // Only shut z axis on inactivity + const int32_t delta = axes[i].delta; + if (i == 2 && !holdZ) { + axes[i].enableStepper(delta != 0); + } else if (delta != 0) { + axes[i].enableStepper(true); + } + } + // compute number of intervals for this move + intervals = us / INTERVAL_IN_MICROSECONDS; + intervals_remaining = intervals; + const int32_t negative_half_interval = -intervals / 2; + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i].counter = negative_half_interval; + } + is_running = true; +} +*/ + +planner::Block *current_block = 0; +enum { + ACCELERATING = 1, + IN_PLATEAU, + DECELERATING +} current_phase = DECELERATING; // pretend we are just leaving a block + +// load up the next movment +// WARNING: called from inside the ISR, so get out fast +bool getNextMove() { + uint32_t max_delta = 0; + is_running = false; + +#if 0 + if (current_phase == DECELERATING) { + current_phase == ACCELERATING; + } + else if (current_phase == ACCELERATING) { + current_phase == IN_PLATEAU; + } + else if (current_phase == IN_PLATEAU) { + current_phase == ACCELERATING; + current_block = 0; + } + + // step_rate is steps/sec, step_timing is sec/step + if (!current_block) { +FetchNext: + if (planner::block_buffer.isEmpty()) + return false; + + current_block = planner::block_buffer.getTail(); + // Mark block as busy (being executed by the stepper interrupt) + current_block->busy = true; + + // bump the tail forward + planner::block_buffer--; + + current_phase = ACCELERATING; + if (current_block->accelerate_until > 0) { + // reset steprate axis scale + axes[STEPRATE_AXIS].setScaleShift(0); + + // setup all real axes + for (int i = 0; i < STEPPER_COUNT; i++) { + // rearrange a little to avoid fractional math. + // Should be the same as: (accelerate_until/step_event_count) * steps[i] + axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); + } + + // setup steprate axis + axes[STEPRATE_AXIS].definePosition(INTERVALS_PER_SECOND/current_block->initial_rate); + axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); + + max_delta = current_block->accelerate_until; + } else { + current_phase = IN_PLATEAU; } } - // compute number of intervals for this move - intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); - intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].counter = negative_half_interval; + + if (current_phase == IN_PLATEAU && current_block->accelerate_until != current_block->decelerate_after) { + // reset steprate axis scale + axes[STEPRATE_AXIS].setScaleShift(0); + + // setup all real axes + for (int i = 0; i < STEPPER_COUNT; i++) { + // rearrange a little to avoid fractional math. + // Should be the same as: (decelerate_after/step_event_count) * steps[i] + axes[i].setTarget((current_block->decelerate_after * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); + } + + // setup steprate axis, using the current position properly + axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); + + max_delta = current_block->step_event_count - current_block->accelerate_until - current_block->decelerate_after; + } else { + current_phase = DECELERATING; } - is_running = true; -} + + if (current_phase == DECELERATING && current_block->decelerate_after != current_block->step_event_count) { + // reset steprate axis scale + axes[STEPRATE_AXIS].setScaleShift(0); + + // setup all real axes + for (int i = 0; i < STEPPER_COUNT; i++) { + // rearrange a little to avoid fractional math. + // Should be the same as: (decelerate_after/step_event_count) * steps[i] + axes[i].setTarget((current_block->decelerate_after * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); + } -void setTargetNew(const Point& target, int32_t us, uint8_t relative) { - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].setTarget(target[i], (relative & (1 << i)) != 0); - // Only shut z axis on inactivity + // setup steprate axis, using the current position properly + axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->final_rate, /*relative =*/ false); + } else { + current_phase = ACCELERATING; + // My apologies for a goto, but we're in a hurry! -Rob + goto FetchNext; + } + +#endif + + if (!current_block) { + if (planner::block_buffer.isEmpty()) + return false; + + current_block = planner::block_buffer.getTail(); + // Mark block as busy (being executed by the stepper interrupt) + current_block->busy = true; + + // bump the tail forward + planner::block_buffer--; + } + + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setTarget(current_block->steps[i], /*relative =*/ true); + } + axes[STEPRATE_AXIS].definePosition(INTERVALS_PER_SECOND/current_block->nominal_rate); + axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); + + + // make sure that steprate axis is not the longest axis, by scaling in halves + // int8_t scale_shift = 0; + // while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + // scale_shift++; + // } + // axes[STEPRATE_AXIS].setScaleShift(scale_shift); + + if (axes[STEPRATE_AXIS].delta > max_delta) + max_delta = axes[STEPRATE_AXIS].delta; + + for (int i = 0; i < STEPPER_COUNT; i++) { const int32_t delta = axes[i].delta; - if (i == 2 && !holdZ) { - axes[i].enableStepper(delta != 0); - } else if (delta != 0) { - axes[i].enableStepper(true); - } + // Only shut z axis on inactivity + if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); + // else if (delta != 0) axes[i].enableStepper(true); } + // compute number of intervals for this move - intervals = us / INTERVAL_IN_MICROSECONDS; + intervals = max_delta; intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { + const int32_t negative_half_interval = -intervals >> 1; + for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } + timer_counter = axes[STEPRATE_AXIS].position<setEnabled(true); } if (delta < 0) { delta = -delta; direction = false; } + + full_delta = delta; } void StepperAxis::setHoming(const bool direction_in) { direction = direction_in; - interface->setEnabled(true); + if (interface != 0) + interface->setEnabled(true); delta = 1; + full_delta = 1; } void StepperAxis::definePosition(const int32_t position_in) { @@ -38,7 +42,8 @@ void StepperAxis::definePosition(const int32_t position_in) { } void StepperAxis::enableStepper(bool enable) { - interface->setEnabled(enable); + if (interface != 0) + interface->setEnabled(enable); } void StepperAxis::reset() { @@ -48,67 +53,94 @@ void StepperAxis::reset() { target = 0; counter = 0; delta = 0; + full_delta = 0; + scale_shift = 0; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) endstop_play = ENDSTOP_DEFAULT_PLAY; endstop_status = ESS_UNKNOWN; #endif //SINGLE_SWITCH_ENDSTOPS } +void StepperAxis::setScaleShift(uint8_t new_shift) { + int8_t shift_delta = new_shift - /*old*/ scale_shift; + scale_shift = new_shift; + + // scale these when set + // recover these by remultiplying (shift_delta would be negative) + position >>= shift_delta; + target >>= shift_delta; + + // always go from the source on the delta + delta = full_delta >> scale_shift; + + // should we shift counter too? +} + + bool StepperAxis::checkEndstop(const bool isHoming) { + if (!interface) + return false; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) - bool hit_endstop = direction ? interface->isAtMaximum() : interface->isAtMinimum(); + bool hit_endstop = direction ? interface->isAtMaximum() : interface->isAtMinimum(); // We must move at least ENDSTOP_DEBOUNCE from where we hit the endstop before we declare traveling - if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM))) { - hit_endstop = true; + if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM))) { + hit_endstop = true; // Did we *just* hit the endstop? - if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { - endstop_play = ENDSTOP_DEFAULT_PLAY; - if (isHoming?direction:prev_direction) - endstop_status = ESS_AT_MAXIMUM; - else - endstop_status = ESS_AT_MINIMUM; - + if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { + endstop_play = ENDSTOP_DEFAULT_PLAY; + if (isHoming?direction:prev_direction) + endstop_status = ESS_AT_MAXIMUM; + else + endstop_status = ESS_AT_MINIMUM; + // OR, are we traveling away from the endstop we just hit and still have play... - } else if ((direction && endstop_status != ESS_AT_MAXIMUM) || (!direction && endstop_status != ESS_AT_MINIMUM)) { - if (endstop_play > 0) { - --endstop_play; - hit_endstop = false; // pretend this never happened... - } else { - // we ran out of play, so we must be ramming into the side, switch directions - // endstop_status = !direction ? ESS_AT_MAXIMUM : ESS_AT_MINIMUM; - // endstop_play = ENDSTOP_DEFAULT_PLAY; - } - } + } else if ((direction && endstop_status != ESS_AT_MAXIMUM) || (!direction && endstop_status != ESS_AT_MINIMUM)) { + if (endstop_play > 0) { + --endstop_play; + hit_endstop = false; // pretend this never happened... + } else { + // we ran out of play, so we must be ramming into the side, switch directions + // endstop_status = !direction ? ESS_AT_MAXIMUM : ESS_AT_MINIMUM; + // endstop_play = ENDSTOP_DEFAULT_PLAY; + } + } // otherwise we hit the endstop - + // but if we didn't hit an endstop, clear the status - } else { - endstop_status = ESS_TRAVELING; - if (!isHoming) { - endstop_play = ENDSTOP_DEFAULT_PLAY; - } - } - prev_direction = direction; - return hit_endstop; + } else { + endstop_status = ESS_TRAVELING; + if (!isHoming) { + endstop_play = ENDSTOP_DEFAULT_PLAY; + } + } + prev_direction = direction; + return hit_endstop; #else - return direction ? interface->isAtMaximum() : interface->isAtMinimum(); + return direction ? interface->isAtMaximum() : interface->isAtMinimum(); #endif } void StepperAxis::doInterrupt(const int32_t intervals) { counter += delta; if (counter >= 0) { - interface->setDirection(direction); counter -= intervals; - bool hit_endstop = checkEndstop(false); - if (direction) { - if (!hit_endstop) interface->step(true); - position++; + if (interface != 0) { + interface->setDirection(direction); + bool hit_endstop = checkEndstop(false); + if (direction) { + if (!hit_endstop) interface->step(true); + position++; + } else { + if (!hit_endstop) interface->step(true); + position--; + } + interface->step(false); } else { - if (!hit_endstop) interface->step(true); - position--; + if (direction) + position++; + else + position--; } - interface->step(false); } } @@ -117,8 +149,8 @@ bool StepperAxis::doHoming(const int32_t intervals) { if (delta == 0) return false; counter += delta; if (counter >= 0) { - interface->setDirection(direction); counter -= intervals; + interface->setDirection(direction); bool hit_endstop = checkEndstop(true); if (direction) { if (!hit_endstop) { diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index 6d13ef7..e6d174e 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -20,6 +20,8 @@ public: ///< a step so far passed. When the counter hits ///< zero, a step is taken. volatile int32_t delta; ///< Amount to increment counter per tick + volatile int32_t full_delta; ///< Unscaled version of delta + volatile int32_t scale_shift; ///< Amount to bit-shift position to get the actual value volatile bool direction; ///< True for positive, false for negative #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) volatile bool prev_direction; ///< Record the previous direction for endstop detection @@ -70,6 +72,10 @@ public: /// \param[in] enable If true, enable the axis; otherwise, disable it. void enableStepper(bool enable); + /// Set the scale_shift (and adjust position and delta accordingly) + /// \param[in] new_shift The number of bits to shift + void setScaleShift(uint8_t new_shift); + /// Reset to initial state void reset(); From a374b4e22dbd764a0aebcf8981c24900d4caf424 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Fri, 16 Dec 2011 03:03:29 -0600 Subject: [PATCH 31/61] Thrashing, please ignore. --- firmware/src/Motherboard/Command.cc | 25 ++--- firmware/src/Motherboard/Main.cc | 2 + firmware/src/Motherboard/Planner.cc | 75 +++++++++----- firmware/src/Motherboard/Planner.hh | 9 +- firmware/src/Motherboard/Point.cc | 11 +++ firmware/src/Motherboard/Point.hh | 1 + firmware/src/Motherboard/Steppers.cc | 98 +++++++++++++------ firmware/src/Motherboard/Steppers.hh | 4 +- .../Motherboard/boards/mb24/Configuration.hh | 6 +- .../boards/rrmbv12/Configuration.hh | 14 +-- firmware/src/shared/StepperAxis.cc | 10 +- 11 files changed, 171 insertions(+), 84 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 54eefd4..0ce95d8 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -115,9 +115,9 @@ void reset() { // Handle movement comands -- called from a few places void handleMovementCommand(uint8_t &command) { // if we're already moving, check to make sure the buffer isn't full - if (mode == MOVING && planner::block_buffer.isFull()) { - return; // we'll be back! - } + // if (mode == MOVING && planner::block_buffer.isFull()) { + // return; // we'll be back! + // } if (command == HOST_CMD_QUEUE_POINT_ABS) { // check for completion if (command_buffer.getLength() >= 17) { @@ -140,7 +140,7 @@ void handleMovementCommand(uint8_t &command) { int32_t a = pop32(); int32_t b = pop32(); int32_t dda = pop32(); - planner::addMoveToBuffer(Point(x,y,z,a,b), dda); + steppers::setTarget(Point(x,y,z,a,b), dda); } } else if (command == HOST_CMD_QUEUE_POINT_NEW) { // check for completion @@ -179,12 +179,12 @@ void runCommandSlice() { if (!steppers::isRunning()) { mode = READY; } else { - if (command_buffer.getLength() > 0) { - uint8_t command = command_buffer[0]; - if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { - handleMovementCommand(command); - } - } + // if (command_buffer.getLength() > 0) { + // uint8_t command = command_buffer[0]; + // if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + // handleMovementCommand(command); + // } + // } } } if (mode == DELAY) { @@ -242,12 +242,13 @@ void runCommandSlice() { // process next command on the queue. if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; + if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { handleMovementCommand(command); } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { command_buffer.pop(); // remove the command code - tool::setCurrentToolheadIndex(command_buffer.pop()); + tool::setCurrentToolheadIndex(command_buffer.pop()); } } else if (command == HOST_CMD_ENABLE_AXES) { if (command_buffer.getLength() >= 2) { @@ -357,7 +358,7 @@ void runCommandSlice() { } } - steppers::definePosition(newPoint); + planner::definePosition(newPoint); } } else if (command == HOST_CMD_TOOL_COMMAND) { diff --git a/firmware/src/Motherboard/Main.cc b/firmware/src/Motherboard/Main.cc index 147c157..37d53ff 100644 --- a/firmware/src/Motherboard/Main.cc +++ b/firmware/src/Motherboard/Main.cc @@ -24,6 +24,7 @@ #include #include "Timeout.hh" #include "Steppers.hh" +#include "Planner.hh" #include "Motherboard.hh" #include "SDCard.hh" #include "Eeprom.hh" @@ -58,6 +59,7 @@ int main() { Motherboard& board = Motherboard::getBoard(); steppers::init(Motherboard::getBoard()); + planner::init(); reset(true); sei(); while (1) { diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 1bf1124..46a5c91 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -130,6 +130,12 @@ namespace planner { position = Point(0,0,0,0,0); previous_nominal_speed = 0.0; + + axes[0].max_acceleration = 9000*axes[0].steps_per_mm; + axes[1].max_acceleration = 9000*axes[1].steps_per_mm; + axes[2].max_acceleration = 100*axes[2].steps_per_mm; + axes[3].max_acceleration = 9000*axes[3].steps_per_mm; + axes[4].max_acceleration = 9000*axes[4].steps_per_mm; } @@ -147,9 +153,9 @@ namespace planner { void setAcceleration(float new_acceleration) { acceleration = new_acceleration; - for (int i = 1; i < AXIS_COUNT; i++) { - axes[i].max_acceleration = acceleration * axes[i].steps_per_mm; - } + // for (int i = 0; i < AXIS_COUNT; i++) { + // axes[i].max_acceleration = acceleration * axes[i].steps_per_mm; + // } } // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the @@ -300,10 +306,10 @@ namespace planner { Block *block[3] = { NULL, NULL, NULL }; while(block_index != block_buffer.getTailIndex()) { block_index = block_buffer.getPreviousIndex(block_index); - // block[2] = block[1]; - // block[1] = block[0]; + block[2] = block[1]; + block[1] = block[0]; // Move two blocks worth of ram, from [0] to [1], using the overlap-safe memmove - memmove(block[0], block[1], sizeof(Block)<<1); + //memmove(block[0], block[1], sizeof(Block)<<1); block[0] = &block_buffer[block_index]; planner_reverse_pass_kernel(block[0], block[1], block[2]); } @@ -339,10 +345,10 @@ namespace planner { Block *block[3] = { NULL, NULL, NULL }; while(block_index != block_buffer.getHeadIndex()) { - // block[0] = block[1]; - // block[1] = block[2]; + block[0] = block[1]; + block[1] = block[2]; // Move two blocks worth of ram, from [1] to [0], using the overlap-safe memmove - memmove(block[1], block[0], sizeof(Block)<<1); + //memmove(block[1], block[0], sizeof(Block)<<1); block[2] = &block_buffer[block_index]; planner_forward_pass_kernel(block[0],block[1],block[2]); block_index = block_buffer.getNextIndex(block_index); @@ -382,12 +388,23 @@ namespace planner { // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) { + steppers::setTarget(target, us_per_step); + return true; + if (block_buffer.isFull()) return false; Block *block = block_buffer.getHead(); // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; + + //steppers::setTarget(target, us_per_step); + block->nominal_rate = ceil(1000000/us_per_step); // (step/sec) Always > 0 + + + + + // calculate the difference between the current position and the target Point delta = target - position; @@ -410,12 +427,20 @@ namespace planner { // Need to change the call interface to fix this, though. } } - if (axes[master_axis].steps_per_mm == 0) { - Motherboard::getBoard().indicateError(4); - } - float feed_rate = (us_per_step * block->step_event_count)/axes[master_axis].steps_per_mm; // mm/second - if (feed_rate == 0) - return true; // we did not care, but yes, we did something with this + + + block->steps = target; + + // Update position + position = target; + + block_buffer++; + steppers::startRunning(); + + return true; + + + // // Compute direction bits for this block // block->direction_bits = 0; @@ -426,7 +451,7 @@ namespace planner { float delta_mm[AXIS_COUNT]; block->millimeters = 0; for (int i = 0; i < AXIS_COUNT; i++) { - delta_mm[i] = delta[i]/axes[i].steps_per_mm; + delta_mm[i] = steps[i]/axes[i].steps_per_mm; block->millimeters += delta_mm[i] * delta_mm[i]; } block->millimeters = sqrt(block->millimeters); @@ -434,10 +459,10 @@ namespace planner { float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. - float inverse_second = feed_rate * inverse_millimeters; + float inverse_second = (1000000.0/us_per_step)*block->step_event_count; block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 - block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 + block->nominal_rate = ceil(1000000.0/us_per_step); // (step/sec) Always > 0 // TODO make sure we are going the minimum speed, at least // if(feed_ratecalculate_trapezoid(MINIMUM_PLANNER_SPEED); // Update position - memcpy(&position, &target, sizeof(target)); // position[] = target[] - - // Move buffer head + position = target; + + // Move buffer head -- should this move to after recalulate? block_buffer++; - planner_recalculate(); + //planner_recalculate(); + + block->nominal_rate = us_per_step; - steppers::getNextMove(); + steppers::startRunning(); return true; } @@ -536,7 +563,7 @@ namespace planner { const uint8_t axes_enabled, const uint32_t us_per_step) { - + // STUB } void definePosition(const Point& new_position) diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index e759011..0a02d75 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -126,7 +126,11 @@ namespace planner { BufDataType* const data; /// Pointer to buffer data public: - ReusingCircularBufferTempl(int8_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size), size_mask(size-1), data(buffer_in) {}; + ReusingCircularBufferTempl(int8_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size), size_mask(size-1), data(buffer_in) { + for (int8_t i = 0; i < size; i++) { + data[i] = BufDataType(); + } + }; inline BufDataType *getHead() { return &data[head]; @@ -147,7 +151,7 @@ namespace planner { } inline int8_t getPreviousIndex(int8_t from) { - return (from - 1) & size_mask; + return ((from+size) - 1) & size_mask; } inline BufDataType *getNextHead() { @@ -186,7 +190,6 @@ namespace planner { }; // Create a circular buffer of blocks to walk - extern Block block_buffer_data[]; extern ReusingCircularBufferTempl block_buffer; } diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 08c0fc2..242fb1b 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -14,6 +14,9 @@ inline T abs(T x) { return (x)>0?(x):-(x); } Point::Point() { + for (uint8_t i = 0; i < 5; i++) { + coordinates[i] = 0; + } } Point::Point(const Point &other) @@ -70,6 +73,14 @@ const Point &Point::operator-(const Point &other) const { return c; } +Point &Point::operator=(const Point &other) { + for (uint8_t i = 0; i < 5; i++) { + coordinates[i] = other.coordinates[i]; + } + return *this; +} + + Point Point::abs() { Point absPoint = Point(); absPoint.coordinates[0] = ::abs(coordinates[0]); diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index 98c3449..fdcef9b 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -53,6 +53,7 @@ public: /// Subtraction operator, for fast deltas const Point &operator-(const Point &other) const; + Point &operator=(const Point &other); // friend const Point &operator-(const Point &a, const Point &b); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 12dd160..80caf4a 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -34,7 +34,7 @@ volatile int32_t timer_counter; // STEPRATE_AXIS is the number of the virtual axis that control the steprate #define STEPRATE_AXIS STEPPER_COUNT -StepperAxis axes[STEPPER_COUNT]; +StepperAxis axes[ALL_AXIS_COUNT]; volatile bool is_homing; bool holdZ = false; @@ -78,7 +78,7 @@ void setHoldZ(bool holdZ_in) { holdZ = holdZ_in; } -/* + void setTarget(const Point& target, int32_t dda_interval) { int32_t max_delta = 0; for (int i = 0; i < AXIS_COUNT; i++) { @@ -92,15 +92,35 @@ void setTarget(const Point& target, int32_t dda_interval) { } } // compute number of intervals for this move - intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); + //intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); + + axes[STEPRATE_AXIS].setScaleShift(0); + + axes[STEPRATE_AXIS].definePosition(10000/INTERVAL_IN_MICROSECONDS);//dda_interval/INTERVAL_IN_MICROSECONDS + axes[STEPRATE_AXIS].setTarget(dda_interval/INTERVAL_IN_MICROSECONDS, /*relative =*/ false); + + int8_t scale_shift = 0; + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + scale_shift++; + } + axes[STEPRATE_AXIS].setScaleShift(scale_shift); + + // if (axes[STEPRATE_AXIS].delta > max_delta) { + // max_delta = axes[STEPRATE_AXIS].delta; + // } + + timer_counter = axes[STEPRATE_AXIS].position<busy = true; + current_block = planner::block_buffer.getTail(); + // Mark block as busy (being executed by the stepper interrupt) + current_block->busy = true; - // bump the tail forward - planner::block_buffer--; + // bump the tail forward + planner::block_buffer--; } - + + max_delta = current_block->step_event_count; + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(current_block->steps[i], /*relative =*/ true); } axes[STEPRATE_AXIS].definePosition(INTERVALS_PER_SECOND/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); + current_block = 0; +#endif - // make sure that steprate axis is not the longest axis, by scaling in halves - // int8_t scale_shift = 0; - // while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { - // scale_shift++; - // } - // axes[STEPRATE_AXIS].setScaleShift(scale_shift); - +#if 1 + //make sure that steprate axis is not the longest axis, by scaling in halves + int8_t scale_shift = 0; + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + scale_shift++; + } + axes[STEPRATE_AXIS].setScaleShift(scale_shift); + + //compute number of intervals for this move + intervals = max_delta; +#else if (axes[STEPRATE_AXIS].delta > max_delta) max_delta = axes[STEPRATE_AXIS].delta; - + intervals = ((max_delta * current_block->nominal_rate) / INTERVAL_IN_MICROSECONDS); + intervals = max_delta; +#endif + for (int i = 0; i < STEPPER_COUNT; i++) { const int32_t delta = axes[i].delta; // Only shut z axis on inactivity if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - // else if (delta != 0) axes[i].enableStepper(true); + else if (delta != 0) axes[i].enableStepper(true); } - // compute number of intervals for this move - intervals = max_delta; + intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals >> 1; + const int32_t negative_half_interval = -(intervals >> 1); for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } @@ -295,14 +325,22 @@ void enableAxis(uint8_t index, bool enable) { } } +void startRunning() { + if (is_running) + return; + is_running = true; + //getNextMove(); +} + bool doInterrupt() { if (is_running) { - if (--timer_counter == 0) { + timer_counter -= INTERVAL_IN_MICROSECONDS; + if (timer_counter <= 0) { if (intervals_remaining-- == 0) { - if (!getNextMove()) { + //if (!getNextMove()) { is_running = false; return false; - } + //} } for (int i = 0; i < ALL_AXIS_COUNT; i++) { diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index cc4329e..b144229 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -93,7 +93,9 @@ namespace steppers { /// disabled when not moving. void setHoldZ(bool holdZ); - bool getNextMove(); + // bool getNextMove(); + + void startRunning(); }; #endif // STEPPERS_HH_ diff --git a/firmware/src/Motherboard/boards/mb24/Configuration.hh b/firmware/src/Motherboard/boards/mb24/Configuration.hh index 4a43747..4f3f3e4 100644 --- a/firmware/src/Motherboard/boards/mb24/Configuration.hh +++ b/firmware/src/Motherboard/boards/mb24/Configuration.hh @@ -28,7 +28,7 @@ /// starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 128 // 8MHz / INTERVAL_IN_MICROSECONDS = 125000 -#define INTERVALS_PER_SECOND 62500 +#define INTERVALS_PER_SECOND (1000000 / INTERVAL_IN_MICROSECONDS) // --- Secure Digital Card configuration --- // NOTE: If SD support is enabled, it is implicitly assumed that the @@ -152,8 +152,8 @@ // THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 -#define DEFAULT_ACCELERATION 10.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 10.0 +#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 20.0 #define DEFAULT_MAX_Z_JERK 0.2 #define DEFAULT_MAX_A_JERK 0.2 #define DEFAULT_MAX_B_JERK 0.2 diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 0ee9b44..3288205 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -27,8 +27,8 @@ // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 64 -// 8MHz / INTERVAL_IN_MICROSECONDS = 125000 -#define INTERVALS_PER_SECOND 125000 +// 1000000 / INTERVAL_IN_MICROSECONDS = 125000 +#define INTERVALS_PER_SECOND (1000000 / INTERVAL_IN_MICROSECONDS) // The pin that connects to the /PS_ON pin on the PSU header. This pin switches // on the PSU when pulled low. @@ -158,10 +158,10 @@ // THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 -#define DEFAULT_ACCELERATION 10.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 10.0 -#define DEFAULT_MAX_Z_JERK 0.2 -#define DEFAULT_MAX_A_JERK 0.2 -#define DEFAULT_MAX_B_JERK 0.2 +#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 20.0 +#define DEFAULT_MAX_Z_JERK 0.4 +#define DEFAULT_MAX_A_JERK 0.4 +#define DEFAULT_MAX_B_JERK 0.4 #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index 4a292a0..73a196f 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -2,6 +2,7 @@ StepperAxis::StepperAxis() : interface(0) { + reset(); } StepperAxis::StepperAxis(StepperInterface& stepper_interface) : @@ -11,14 +12,15 @@ interface(&stepper_interface) { void StepperAxis::setTarget(const int32_t target_in, bool relative) { - target = target_in; if (relative) { - delta = target; + delta = target_in; + target = position + target_in; } else { - delta = target - position; + delta = target_in - position; + target = target_in; } direction = true; - if (delta != 0 && interface) { + if (delta != 0 && interface != 0) { interface->setEnabled(true); } if (delta < 0) { From a5ccf3182da22a1d3e2a3e420f604ffc3b716bc5 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 22 Dec 2011 00:09:08 -0600 Subject: [PATCH 32/61] Damn bit-shifts, can't be negative! Found a better way anyway. --- firmware/src/Motherboard/Steppers.cc | 30 ++++++++------ firmware/src/shared/StepperAxis.cc | 59 +++++++++++++++------------- firmware/src/shared/StepperAxis.hh | 34 ++++++++-------- 3 files changed, 67 insertions(+), 56 deletions(-) diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 80caf4a..4f812fd 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -23,8 +23,6 @@ namespace steppers { -#define INTERVAL_IN_MICROSECONDS_X_16 (INTERVAL_IN_MICROSECONDS<<4) - volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; @@ -51,11 +49,13 @@ void init(Motherboard& motherboard) { } axes[STEPRATE_AXIS] = StepperAxis(); + timer_counter = 0; } void abort() { is_running = false; is_homing = false; + timer_counter = 0; } /// Define current position as given point @@ -81,7 +81,7 @@ void setHoldZ(bool holdZ_in) { void setTarget(const Point& target, int32_t dda_interval) { int32_t max_delta = 0; - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], false); const int32_t delta = axes[i].delta; // Only shut z axis on inactivity @@ -95,10 +95,15 @@ void setTarget(const Point& target, int32_t dda_interval) { //intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); axes[STEPRATE_AXIS].setScaleShift(0); + axes[STEPRATE_AXIS].definePosition(dda_interval);//dda_interval/INTERVAL_IN_MICROSECONDS + axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); + + if (max_delta == 0) { + is_running = false; + return; + } - axes[STEPRATE_AXIS].definePosition(10000/INTERVAL_IN_MICROSECONDS);//dda_interval/INTERVAL_IN_MICROSECONDS - axes[STEPRATE_AXIS].setTarget(dda_interval/INTERVAL_IN_MICROSECONDS, /*relative =*/ false); - + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { scale_shift++; @@ -106,14 +111,15 @@ void setTarget(const Point& target, int32_t dda_interval) { axes[STEPRATE_AXIS].setScaleShift(scale_shift); // if (axes[STEPRATE_AXIS].delta > max_delta) { - // max_delta = axes[STEPRATE_AXIS].delta; - // } - - timer_counter = axes[STEPRATE_AXIS].position<>1); // same as -(intervals/2), but faster (?) for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } @@ -347,7 +353,7 @@ bool doInterrupt() { axes[i].doInterrupt(intervals); } - timer_counter = axes[STEPRATE_AXIS].position<setEnabled(true); } - if (delta < 0) { - delta = -delta; + if (unscaled_delta < 0) { + unscaled_delta = -unscaled_delta; direction = false; } - - full_delta = delta; + + if (scale_shift > 0) { + // scale these when set + position = unscaled_position >> scale_shift; + target = unscaled_target >> scale_shift; + delta = unscaled_delta >> scale_shift; + } } void StepperAxis::setHoming(const bool direction_in) { @@ -36,7 +38,7 @@ void StepperAxis::setHoming(const bool direction_in) { if (interface != 0) interface->setEnabled(true); delta = 1; - full_delta = 1; + unscaled_delta = 1; } void StepperAxis::definePosition(const int32_t position_in) { @@ -50,12 +52,14 @@ void StepperAxis::enableStepper(bool enable) { void StepperAxis::reset() { position = 0; + unscaled_position =0; minimum = 0; maximum = 0; target = 0; + unscaled_target = 0; counter = 0; delta = 0; - full_delta = 0; + unscaled_delta = 0; scale_shift = 0; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) endstop_play = ENDSTOP_DEFAULT_PLAY; @@ -64,18 +68,16 @@ void StepperAxis::reset() { } void StepperAxis::setScaleShift(uint8_t new_shift) { - int8_t shift_delta = new_shift - /*old*/ scale_shift; - scale_shift = new_shift; - - // scale these when set - // recover these by remultiplying (shift_delta would be negative) - position >>= shift_delta; - target >>= shift_delta; - - // always go from the source on the delta - delta = full_delta >> scale_shift; + if (scale_shift != new_shift) { + scale_shift = new_shift; + + // scale these when set + position = unscaled_position >> scale_shift; + target = unscaled_target >> scale_shift; + delta = unscaled_delta >> scale_shift; - // should we shift counter too? + // should we shift counter too? + } } @@ -124,6 +126,7 @@ bool StepperAxis::checkEndstop(const bool isHoming) { void StepperAxis::doInterrupt(const int32_t intervals) { counter += delta; + if (counter >= 0) { counter -= intervals; if (interface != 0) { diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index e6d174e..0de9d9a 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -11,23 +11,25 @@ class StepperAxis { public: - StepperInterface* interface; ///< Interface this axis is connected to - volatile int32_t position; ///< Current position of this axis, in steps - int32_t minimum; ///< Minimum position, in steps - int32_t maximum; ///< Maximum position, in steps - volatile int32_t target; ///< Target position, in steps - volatile int32_t counter; ///< Step counter; represents the proportion of - ///< a step so far passed. When the counter hits - ///< zero, a step is taken. - volatile int32_t delta; ///< Amount to increment counter per tick - volatile int32_t full_delta; ///< Unscaled version of delta - volatile int32_t scale_shift; ///< Amount to bit-shift position to get the actual value - volatile bool direction; ///< True for positive, false for negative + StepperInterface* interface; ///< Interface this axis is connected to + volatile int32_t position; ///< Current position of this axis, in steps + volatile int32_t unscaled_position; ///< Current position of this axis, in steps, unscaled by scale_shift + int32_t minimum; ///< Minimum position, in steps + int32_t maximum; ///< Maximum position, in steps + volatile int32_t target; ///< Target position, in steps + volatile int32_t unscaled_target; ///< Target position, in steps, unscaled by scale_shift + volatile int32_t counter; ///< Step counter; represents the proportion of + ///< a step so far passed. When the counter hits + ///< zero, a step is taken. + volatile int32_t delta; ///< Amount to increment counter per tick + volatile int32_t unscaled_delta; ///< Unscaled version of delta + volatile int32_t scale_shift; ///< Amount to bit-shift position to get the actual value + volatile bool direction; ///< True for positive, false for negative #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) - volatile bool prev_direction; ///< Record the previous direction for endstop detection - volatile int32_t endstop_play; ///< Amount to move while endstop triggered, to see which way to move - - enum endstop_status_t { ///< State of the endstop + volatile bool prev_direction; ///< Record the previous direction for endstop detection + volatile int32_t endstop_play; ///< Amount to move while endstop triggered, to see which way to move + + enum endstop_status_t { ///< State of the endstop ESS_UNKNOWN, ESS_TRAVELING, ESS_AT_MAXIMUM, From eb6ca920c905a16f7ca2dd5fc92fc1ff6b494302 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 28 Dec 2011 22:09:23 -0600 Subject: [PATCH 33/61] Got the circular buffer working, now to get the planner ironed out... --- firmware/src/Motherboard/Command.cc | 14 +- firmware/src/Motherboard/Planner.cc | 161 +++++++--- firmware/src/Motherboard/Planner.hh | 85 +---- firmware/src/Motherboard/Steppers.cc | 298 +++++++++--------- firmware/src/Motherboard/Steppers.hh | 6 +- .../boards/rrmbv12/Configuration.hh | 2 +- firmware/src/shared/StepperAxis.cc | 41 +-- firmware/src/shared/StepperAxis.hh | 10 +- 8 files changed, 285 insertions(+), 332 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 0ce95d8..083f99d 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -113,11 +113,11 @@ void reset() { } // Handle movement comands -- called from a few places -void handleMovementCommand(uint8_t &command) { +static void handleMovementCommand(const uint8_t &command) { // if we're already moving, check to make sure the buffer isn't full - // if (mode == MOVING && planner::block_buffer.isFull()) { - // return; // we'll be back! - // } + if (mode == MOVING && planner::isBufferFull()) { + return; // we'll be back! + } if (command == HOST_CMD_QUEUE_POINT_ABS) { // check for completion if (command_buffer.getLength() >= 17) { @@ -127,7 +127,7 @@ void handleMovementCommand(uint8_t &command) { int32_t y = pop32(); int32_t z = pop32(); int32_t dda = pop32(); - planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA + // planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA } } else if (command == HOST_CMD_QUEUE_POINT_EXT) { // check for completion @@ -140,7 +140,7 @@ void handleMovementCommand(uint8_t &command) { int32_t a = pop32(); int32_t b = pop32(); int32_t dda = pop32(); - steppers::setTarget(Point(x,y,z,a,b), dda); + planner::addMoveToBuffer(Point(x,y,z,a,b), dda); } } else if (command == HOST_CMD_QUEUE_POINT_NEW) { // check for completion @@ -243,7 +243,7 @@ void runCommandSlice() { if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; - if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + if (command == HOST_CMD_QUEUE_POINT_EXT) { handleMovementCommand(command); } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 46a5c91..154ad95 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -92,6 +92,87 @@ inline T abs(T x) { return (x)>0?(x):-(x); } namespace planner { + + // Super-simple circular buffer, where old nodes are reused + // TODO: Move to a seperate file + // WARNING WARNING WARNING: If the size of this buffer is not in the following list this WILL FAIL BADLY! + // (2, 4, 8, 16, 32, 64, 128) + template + class ReusingCircularBufferTempl + { + public: + typedef T BufDataType; + + private: + volatile uint16_t head, tail; + uint16_t size; + uint16_t size_mask; + BufDataType* const data; /// Pointer to buffer data + + public: + ReusingCircularBufferTempl(int16_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size_in), size_mask(size_in-1), data(buffer_in) { + for (int16_t i = 0; i < size; i++) { + data[i] = BufDataType(); + } + }; + + inline BufDataType *getHead() { + return &data[head]; + } + inline uint16_t getHeadIndex() { + return head; + } + + inline BufDataType *getTail() { + return &data[tail]; + } + inline uint16_t getTailIndex() { + return tail; + } + + inline int16_t getNextIndex(uint16_t from) { + return ((from + 1) & size_mask); + } + + inline int16_t getPreviousIndex(uint16_t from) { + return (((from+size) - 1) & size_mask); + } + + inline BufDataType *getNextHead() { + return &data[getNextIndex(head)]; + } + + inline BufDataType &operator[] (int16_t index) { + // adding size should make negative indexes < size work ok + int16_t offset = (index + head + size) & size_mask; + return data[offset]; + } + + // bump the head with buffer++. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void bumpHead() { + head = getNextIndex(head); + } + + // bump the tail with buffer--. cannot return anything useful, so it doesn't + // WARNING: no sanity checks! + inline void bumpTail() { + tail = getNextIndex(tail); + } + + inline bool isEmpty() { + return head == tail; + } + + inline bool isFull() { + return getNextIndex(head) == tail; + } + + inline int16_t getUsedCount() { + return ((head-tail+size) & size_mask); + } + }; + // this is very similar to the StepperAxis, but geared toward planning struct PlannerAxis { @@ -200,10 +281,10 @@ namespace planner { final_rate = ceil(nominal_rate*exit_factor); // (step/min) // Limit minimal step rate (Otherwise the timer will overflow.) - if(initial_rate <120) - initial_rate=120; + if(initial_rate < 120) + initial_rate = 120; if(final_rate < 120) - final_rate=120; + final_rate = 120; int32_t acceleration = acceleration_st; int32_t accelerate_steps = @@ -384,13 +465,26 @@ namespace planner { } } + bool isBufferFull() { + // return false; + return block_buffer.isFull(); + } + + bool isBufferEmpty() { + // return false; + return block_buffer.isEmpty(); + } + + Block *getNextBlock() { + Block *block = block_buffer.getTail(); + planner::block_buffer.bumpTail(); + return block; + } + // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) - { - steppers::setTarget(target, us_per_step); - return true; - + { if (block_buffer.isFull()) return false; @@ -398,14 +492,6 @@ namespace planner { // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; - //steppers::setTarget(target, us_per_step); - block->nominal_rate = ceil(1000000/us_per_step); // (step/sec) Always > 0 - - - - - - // calculate the difference between the current position and the target Point delta = target - position; @@ -421,6 +507,7 @@ namespace planner { if (steps[i] > block->step_event_count) { block->step_event_count = steps[i]; master_axis = i; + // steps_per_mm = block->step_event_count/block->millimeters; // WARNING, Edge case: Two axis got the same number of steps, but have different steps_per_mm values // No way to tell which one to choose. @@ -428,21 +515,7 @@ namespace planner { } } - - block->steps = target; - - // Update position - position = target; - - block_buffer++; - steppers::startRunning(); - - return true; - - - - - // // Compute direction bits for this block + // // Compute direction bits for this block -- UNUSED FOR NOW // block->direction_bits = 0; // for (int i = 0; i < AXIS_COUNT; i++) { // if (target[i] < position[i]) { block->direction_bits |= (1<millimeters = sqrt(block->millimeters); - float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides + // Calculate 1 second/(seconds for this movement) + float inverse_second = 1000000/(us_per_step * block->step_event_count); + float steps_per_mm = block->step_event_count * inverse_millimeters; + // we are given microseconds/step, and we need steps/mm, and steps/second + + // Calculate speed in steps/sec + uint32_t steps_per_second = 1000000/us_per_step; + float mm_per_second = block->millimeters * inverse_second; + // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. - float inverse_second = (1000000.0/us_per_step)*block->step_event_count; - - block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 - block->nominal_rate = ceil(1000000.0/us_per_step); // (step/sec) Always > 0 + block->nominal_speed = mm_per_second; // (mm/sec) Always > 0 + block->nominal_rate = steps_per_second; // (step/sec) Always > 0 // TODO make sure we are going the minimum speed, at least // if(feed_ratestep_event_count/block->millimeters; + // Compute and limit the acceleration rate for the trapezoid generator. block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis for(int i=0; i < AXIS_COUNT; i++) { @@ -549,11 +628,9 @@ namespace planner { position = target; // Move buffer head -- should this move to after recalulate? - block_buffer++; + block_buffer.bumpHead(); - //planner_recalculate(); - - block->nominal_rate = us_per_step; + // planner_recalculate(); steppers::startRunning(); return true; @@ -579,6 +656,6 @@ namespace planner { const Point getPosition() { - return position; + return position; } } \ No newline at end of file diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 0a02d75..023bdc3 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -109,88 +109,11 @@ namespace planner { void setAxisStepsPerMM(float steps_per_mm, uint8_t axis); - // Super-simple circular buffer, where old nodes are reused - // TODO: Move to a seperate file - // WARNING WARNING WARNING: If the size of this buffer is not in the following list this WILL FAIL BADLY! - // (2, 4, 8, 16, 32, 64, 128) - template - class ReusingCircularBufferTempl - { - public: - typedef T BufDataType; - - private: - volatile int8_t head, tail; - int8_t size; - int8_t size_mask; - BufDataType* const data; /// Pointer to buffer data + bool isBufferFull(); + bool isBufferEmpty(); - public: - ReusingCircularBufferTempl(int8_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size), size_mask(size-1), data(buffer_in) { - for (int8_t i = 0; i < size; i++) { - data[i] = BufDataType(); - } - }; - - inline BufDataType *getHead() { - return &data[head]; - } - inline int8_t getHeadIndex() { - return head; - } - - inline BufDataType *getTail() { - return &data[tail]; - } - inline int8_t getTailIndex() { - return tail; - } - - inline int8_t getNextIndex(int8_t from) { - return (from + 1) & size_mask; - } - - inline int8_t getPreviousIndex(int8_t from) { - return ((from+size) - 1) & size_mask; - } - - inline BufDataType *getNextHead() { - return &data[getNextIndex(head)]; - } - - inline BufDataType &operator[] (int8_t index) { - // adding size should make negative indexes < size work ok - int8_t offset = (index + head + size) & size_mask; - return data[offset]; - } - - // bump the head with buffer++. cannot return anything useful, so it doesn't - // WARNING: no sanity checks! - inline void operator++(int) { - head = getNextIndex(head); - } - - // bump the tail with buffer--. cannot return anything useful, so it doesn't - // WARNING: no sanity checks! - inline void operator--(int) { - tail = getNextIndex(tail); - } - - inline bool isEmpty() { - return head == tail; - } - - inline bool isFull() { - return getNextIndex(head) == tail; - } - - inline int8_t getUsedCount() { - return ((head-tail+size) & size_mask); - } - }; - - // Create a circular buffer of blocks to walk - extern ReusingCircularBufferTempl block_buffer; + // Fetches the *tail* and bumps the tail + Block *getNextBlock(); } #endif /* end of include guard: PLANNER_HH */ diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 4f812fd..17d49e1 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -1,19 +1,19 @@ /* - * Copyright 2010 by Adam Mayer - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see - */ +* Copyright 2010 by Adam Mayer +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see +*/ #define __STDC_LIMIT_MACROS #include "Steppers.hh" @@ -27,6 +27,7 @@ volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; volatile int32_t timer_counter; +int8_t feedrate_scale_shift; // The ALL_AXIS_COUNT includes the virtual "speed" axis #define ALL_AXIS_COUNT STEPPER_COUNT+1 @@ -47,15 +48,17 @@ void init(Motherboard& motherboard) { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i] = StepperAxis(motherboard.getStepperInterface(i)); } - + axes[STEPRATE_AXIS] = StepperAxis(); timer_counter = 0; + feedrate_scale_shift = 0; } void abort() { is_running = false; is_homing = false; timer_counter = 0; + feedrate_scale_shift = 0; } /// Define current position as given point @@ -80,72 +83,84 @@ void setHoldZ(bool holdZ_in) { void setTarget(const Point& target, int32_t dda_interval) { - int32_t max_delta = 0; - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(target[i], false); - const int32_t delta = axes[i].delta; - // Only shut z axis on inactivity - if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - else if (delta != 0) axes[i].enableStepper(true); - if (delta > max_delta) { - max_delta = delta; - } - } - // compute number of intervals for this move - //intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); - - axes[STEPRATE_AXIS].setScaleShift(0); - axes[STEPRATE_AXIS].definePosition(dda_interval);//dda_interval/INTERVAL_IN_MICROSECONDS - axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); - - if (max_delta == 0) { - is_running = false; - return; + int32_t max_delta = 0; + for (int i = 0; i < AXIS_COUNT; i++) { + axes[i].setTarget(target[i], false); + const int32_t delta = axes[i].delta; + // Only shut z axis on inactivity + if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); + else if (delta != 0) axes[i].enableStepper(true); + if (delta > max_delta) { + max_delta = delta; } + } + // // compute number of intervals for this move + //intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); - // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled - int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { - scale_shift++; - } - axes[STEPRATE_AXIS].setScaleShift(scale_shift); + // reset feedrate_scale_shift and feedrate position + if (feedrate_scale_shift != 0) { + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position << feedrate_scale_shift; + feedrate_scale_shift = 0; + } + if (axes[STEPRATE_AXIS].position == 0) { + axes[STEPRATE_AXIS].definePosition(dda_interval); + } + + axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); + + if (max_delta == 0) { + is_running = false; + return; + } + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled + int8_t scale_shift = 0; + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + scale_shift++; + } + if (scale_shift > 0) { + feedrate_scale_shift = scale_shift; + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; + axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; + axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } + // // if (axes[STEPRATE_AXIS].delta > max_delta) { - // max_delta = axes[STEPRATE_AXIS].delta; + // max_delta = axes[STEPRATE_AXIS].delta; // } - - // We use += here so that the odd rounded-off time from the last move is still waited out - timer_counter += axes[STEPRATE_AXIS].position << axes[STEPRATE_AXIS].scale_shift; - - intervals = max_delta; - intervals_remaining = intervals; - const int32_t negative_half_interval = -(intervals>>1); // same as -(intervals/2), but faster (?) - for (int i = 0; i < ALL_AXIS_COUNT; i++) { - axes[i].counter = negative_half_interval; - } - is_running = true; + + // We use += here so that the odd rounded-off time from the last move is still waited out + timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; + + intervals = max_delta; + intervals_remaining = intervals; + const int32_t negative_half_interval = -intervals / 2; + for (int i = 0; i < ALL_AXIS_COUNT; i++) { + axes[i].counter = negative_half_interval; + } + is_running = true; } /* void setTargetNew(const Point& target, int32_t us, uint8_t relative) { - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].setTarget(target[i], (relative & (1 << i)) != 0); - // Only shut z axis on inactivity - const int32_t delta = axes[i].delta; - if (i == 2 && !holdZ) { - axes[i].enableStepper(delta != 0); - } else if (delta != 0) { - axes[i].enableStepper(true); - } - } - // compute number of intervals for this move - intervals = us / INTERVAL_IN_MICROSECONDS; - intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < AXIS_COUNT; i++) { - axes[i].counter = negative_half_interval; - } - is_running = true; +for (int i = 0; i < AXIS_COUNT; i++) { +axes[i].setTarget(target[i], (relative & (1 << i)) != 0); +// Only shut z axis on inactivity +const int32_t delta = axes[i].delta; +if (i == 2 && !holdZ) { +axes[i].enableStepper(delta != 0); +} else if (delta != 0) { +axes[i].enableStepper(true); +} +} +// compute number of intervals for this move +intervals = us / INTERVAL_IN_MICROSECONDS; +intervals_remaining = intervals; +const int32_t negative_half_interval = -intervals / 2; +for (int i = 0; i < AXIS_COUNT; i++) { +axes[i].counter = negative_half_interval; +} +is_running = true; } */ @@ -162,7 +177,6 @@ bool getNextMove() { uint32_t max_delta = 0; is_running = false; -#if 0 if (current_phase == DECELERATING) { current_phase == ACCELERATING; current_block = 0; @@ -174,45 +188,48 @@ bool getNextMove() { current_phase == DECELERATING; } + // reset steprate axis + axes[STEPRATE_AXIS].position <<= feedrate_scale_shift; + feedrate_scale_shift = 0; + // step_rate is steps/sec, step_timing is sec/step if (!current_block) { -FetchNext: - if (planner::block_buffer.isEmpty()) + FetchNext: + if (planner::isBufferEmpty()) return false; - current_block = planner::block_buffer.getTail(); + current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) current_block->busy = true; - - // bump the tail forward - planner::block_buffer--; - + current_phase = ACCELERATING; if (current_block->accelerate_until > 0) { - // reset steprate axis scale - axes[STEPRATE_AXIS].setScaleShift(0); - // setup all real axes for (int i = 0; i < STEPPER_COUNT; i++) { // rearrange a little to avoid fractional math. // Should be the same as: (accelerate_until/step_event_count) * steps[i] - axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); +// axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); + axes[i].setTarget(current_block->steps[i], /*relative =*/ true); } - + // setup steprate axis - axes[STEPRATE_AXIS].definePosition(INTERVALS_PER_SECOND/current_block->initial_rate); - axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); + axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); max_delta = current_block->accelerate_until; + + // XXX + axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + max_delta = current_block->step_event_count; + current_phase == DECELERATING; + } else { current_phase = IN_PLATEAU; } } - +#if 0 if (current_phase == IN_PLATEAU && current_block->accelerate_until != current_block->decelerate_after) { - // reset steprate axis scale - axes[STEPRATE_AXIS].setScaleShift(0); - // setup all real axes for (int i = 0; i < STEPPER_COUNT; i++) { // rearrange a little to avoid fractional math. @@ -221,17 +238,14 @@ bool getNextMove() { } // setup steprate axis, using the current position properly - axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); - + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + max_delta = current_block->step_event_count - current_block->accelerate_until - current_block->decelerate_after; } else { current_phase = DECELERATING; } - + if (current_phase == DECELERATING && current_block->decelerate_after != current_block->step_event_count) { - // reset steprate axis scale - axes[STEPRATE_AXIS].setScaleShift(0); - // setup all real axes for (int i = 0; i < STEPPER_COUNT; i++) { // rearrange a little to avoid fractional math. @@ -240,70 +254,42 @@ bool getNextMove() { } // setup steprate axis, using the current position properly - axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->final_rate, /*relative =*/ false); + axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); } else { current_phase = ACCELERATING; // My apologies for a goto, but we're in a hurry! -Rob goto FetchNext; } - -#else - - if (!current_block) { - if (planner::block_buffer.isEmpty()) - return false; - - current_block = planner::block_buffer.getTail(); - // Mark block as busy (being executed by the stepper interrupt) - current_block->busy = true; - - // bump the tail forward - planner::block_buffer--; - } - - max_delta = current_block->step_event_count; - +#endif for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(current_block->steps[i], /*relative =*/ true); + const int32_t delta = axes[i].delta; + // Only shut z axis on inactivity + if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); + else if (delta != 0) axes[i].enableStepper(true); } - axes[STEPRATE_AXIS].definePosition(INTERVALS_PER_SECOND/current_block->nominal_rate); - axes[STEPRATE_AXIS].setTarget(INTERVALS_PER_SECOND/current_block->nominal_rate, /*relative =*/ false); - - current_block = 0; -#endif - -#if 1 - //make sure that steprate axis is not the longest axis, by scaling in halves + + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { scale_shift++; } - axes[STEPRATE_AXIS].setScaleShift(scale_shift); + if (scale_shift > 0) { + feedrate_scale_shift = scale_shift; + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; + axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; + axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } - //compute number of intervals for this move - intervals = max_delta; -#else - if (axes[STEPRATE_AXIS].delta > max_delta) - max_delta = axes[STEPRATE_AXIS].delta; - intervals = ((max_delta * current_block->nominal_rate) / INTERVAL_IN_MICROSECONDS); - intervals = max_delta; -#endif + // We use += here so that the odd rounded-off time from the last move is still waited out + timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; - for (int i = 0; i < STEPPER_COUNT; i++) { - const int32_t delta = axes[i].delta; - // Only shut z axis on inactivity - if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - else if (delta != 0) axes[i].enableStepper(true); - } - - + intervals = max_delta; intervals_remaining = intervals; - const int32_t negative_half_interval = -(intervals >> 1); + const int32_t negative_half_interval = -intervals / 2; for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } - timer_counter = axes[STEPRATE_AXIS].position<setEnabled(true); } - if (unscaled_delta < 0) { - unscaled_delta = -unscaled_delta; + if (delta < 0) { + delta = -delta; direction = false; } - - if (scale_shift > 0) { - // scale these when set - position = unscaled_position >> scale_shift; - target = unscaled_target >> scale_shift; - delta = unscaled_delta >> scale_shift; - } } void StepperAxis::setHoming(const bool direction_in) { @@ -38,7 +31,6 @@ void StepperAxis::setHoming(const bool direction_in) { if (interface != 0) interface->setEnabled(true); delta = 1; - unscaled_delta = 1; } void StepperAxis::definePosition(const int32_t position_in) { @@ -52,34 +44,17 @@ void StepperAxis::enableStepper(bool enable) { void StepperAxis::reset() { position = 0; - unscaled_position =0; minimum = 0; maximum = 0; target = 0; - unscaled_target = 0; counter = 0; delta = 0; - unscaled_delta = 0; - scale_shift = 0; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) endstop_play = ENDSTOP_DEFAULT_PLAY; endstop_status = ESS_UNKNOWN; #endif //SINGLE_SWITCH_ENDSTOPS } -void StepperAxis::setScaleShift(uint8_t new_shift) { - if (scale_shift != new_shift) { - scale_shift = new_shift; - - // scale these when set - position = unscaled_position >> scale_shift; - target = unscaled_target >> scale_shift; - delta = unscaled_delta >> scale_shift; - - // should we shift counter too? - } -} - bool StepperAxis::checkEndstop(const bool isHoming) { if (!interface) @@ -175,4 +150,4 @@ bool StepperAxis::doHoming(const int32_t intervals) { interface->step(false); } return true; -} +} \ No newline at end of file diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index 0de9d9a..3252b80 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -13,17 +13,13 @@ class StepperAxis public: StepperInterface* interface; ///< Interface this axis is connected to volatile int32_t position; ///< Current position of this axis, in steps - volatile int32_t unscaled_position; ///< Current position of this axis, in steps, unscaled by scale_shift int32_t minimum; ///< Minimum position, in steps int32_t maximum; ///< Maximum position, in steps volatile int32_t target; ///< Target position, in steps - volatile int32_t unscaled_target; ///< Target position, in steps, unscaled by scale_shift volatile int32_t counter; ///< Step counter; represents the proportion of ///< a step so far passed. When the counter hits ///< zero, a step is taken. volatile int32_t delta; ///< Amount to increment counter per tick - volatile int32_t unscaled_delta; ///< Unscaled version of delta - volatile int32_t scale_shift; ///< Amount to bit-shift position to get the actual value volatile bool direction; ///< True for positive, false for negative #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) volatile bool prev_direction; ///< Record the previous direction for endstop detection @@ -74,10 +70,6 @@ public: /// \param[in] enable If true, enable the axis; otherwise, disable it. void enableStepper(bool enable); - /// Set the scale_shift (and adjust position and delta accordingly) - /// \param[in] new_shift The number of bits to shift - void setScaleShift(uint8_t new_shift); - /// Reset to initial state void reset(); @@ -91,4 +83,4 @@ public: bool doHoming(const int32_t intervals); }; -#endif // STEPPERAXIS_HH +#endif // STEPPERAXIS_HH \ No newline at end of file From 3c8bff0ce15936bac51f449378d56c7797cc8540 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 29 Dec 2011 02:05:11 -0600 Subject: [PATCH 34/61] Really really close... --- firmware/src/Motherboard/Planner.cc | 42 ++++++++++++++++++++-------- firmware/src/Motherboard/Planner.hh | 5 ++-- firmware/src/Motherboard/Point.cc | 36 ++++++++++++------------ firmware/src/Motherboard/Point.hh | 4 +-- firmware/src/Motherboard/Steppers.cc | 29 +++++++++---------- 5 files changed, 69 insertions(+), 47 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 154ad95..a1cda8c 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -61,6 +61,7 @@ #include // for memmove and memcpy #include "Steppers.hh" +#include "Point.hh" #define X_AXIS 0 #define Y_AXIS 1 @@ -477,7 +478,7 @@ namespace planner { Block *getNextBlock() { Block *block = block_buffer.getTail(); - planner::block_buffer.bumpTail(); + block_buffer.bumpTail(); return block; } @@ -492,18 +493,33 @@ namespace planner { // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; - // calculate the difference between the current position and the target - Point delta = target - position; - - // set the number of steps and direction for each axis - block->steps = delta; + // // calculate the difference between the current position and the target + // Point delta = target - position; + // + // // set the number of steps and direction for each axis + // block->steps = target - position; + + block->steps[0] = target[0] - position[0]; + block->steps[1] = target[1] - position[1]; + block->steps[2] = target[2] - position[2]; + block->steps[3] = target[3] - position[3]; + block->steps[4] = target[4] - position[4]; + + block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 +#if 1 // store the absolute number of steps in each direction, without direction - Point steps = delta.abs(); + Point steps = Point( + abs(target[0] - position[0]), + abs(target[1] - position[1]), + abs(target[2] - position[2]), + abs(target[3] - position[3]), + abs(target[4] - position[4]) + ); block->step_event_count = 0; uint8_t master_axis = 0; - for (int i = 1; i < AXIS_COUNT; i++) { + for (int i = 0; i < AXIS_COUNT; i++) { if (steps[i] > block->step_event_count) { block->step_event_count = steps[i]; master_axis = i; @@ -623,14 +639,18 @@ namespace planner { previous_nominal_speed = block->nominal_speed; block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); - +#endif // Update position - position = target; + position[0] = target[0]; + position[1] = target[1]; + position[2] = target[2]; + position[3] = target[3]; + position[4] = target[4]; // Move buffer head -- should this move to after recalulate? block_buffer.bumpHead(); - // planner_recalculate(); + planner_recalculate(); steppers::startRunning(); return true; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 023bdc3..cb5afd1 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -39,7 +39,8 @@ namespace planner { // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // the source g-code and may never actually be reached if acceleration management is active. - typedef struct { + class Block { + public: // Fields used by the bresenham algorithm for tracing the line Point steps; // Step count and direction (may be negative) along each axis uint32_t step_event_count; // The number of step events required to complete this block @@ -74,7 +75,7 @@ namespace planner { // functions void calculate_trapezoid(float exit_factor_speed); - } Block; + }; /// Initilaize the planner data structures void init(); diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 242fb1b..10e40f9 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -14,7 +14,7 @@ inline T abs(T x) { return (x)>0?(x):-(x); } Point::Point() { - for (uint8_t i = 0; i < 5; i++) { + for (uint16_t i = 0; i < 5; i++) { coordinates[i] = 0; } } @@ -31,7 +31,7 @@ Point::Point(const Point &other) } -Point::Point(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b) { +Point::Point(const int32_t x, const int32_t y, const int32_t z, const int32_t a, const int32_t b) { coordinates[0] = x; coordinates[1] = y; coordinates[2] = z; @@ -41,7 +41,7 @@ Point::Point(int32_t x, int32_t y, int32_t z, int32_t a, int32_t b) { #endif } -Point::Point(int32_t x, int32_t y, int32_t z) { +Point::Point(const int32_t x, const int32_t y, const int32_t z) { coordinates[0] = x; coordinates[1] = y; coordinates[2] = z; @@ -51,32 +51,32 @@ Point::Point(int32_t x, int32_t y, int32_t z) { #endif } -const int32_t& Point::operator[](unsigned int index) const { +const int32_t& Point::operator[](const unsigned int index) const { return coordinates[index]; } -int32_t& Point::operator[](unsigned int index) { +int32_t& Point::operator[](const unsigned int index) { return coordinates[index]; } -// const Point &operator-(const Point &a, const Point &b) { -// Point c = Point(a); -// return c -= b; -// } - /// Subtraction operator, for fast deltas -const Point &Point::operator-(const Point &other) const { - Point c = Point(); - for (uint8_t i = 0; i < 5; i++) { - c.coordinates[i] = coordinates[i] - other.coordinates[i]; - } +Point operator- (const Point &a, const Point &b) { + Point c = Point( + a.coordinates[0] - b.coordinates[0], + a.coordinates[1] - b.coordinates[1], + a.coordinates[2] - b.coordinates[2], + a.coordinates[3] - b.coordinates[3], + a.coordinates[4] - b.coordinates[4] + ); return c; } Point &Point::operator=(const Point &other) { - for (uint8_t i = 0; i < 5; i++) { - coordinates[i] = other.coordinates[i]; - } + coordinates[0] = other.coordinates[0]; + coordinates[1] = other.coordinates[1]; + coordinates[2] = other.coordinates[2]; + coordinates[3] = other.coordinates[3]; + coordinates[4] = other.coordinates[4]; return *this; } diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index fdcef9b..d5f5b4e 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -52,8 +52,8 @@ public: int32_t& operator[](unsigned int index); /// Subtraction operator, for fast deltas - const Point &operator-(const Point &other) const; - Point &operator=(const Point &other); + friend Point operator- (const Point &a, const Point &b); + Point & operator= (const Point &other); // friend const Point &operator-(const Point &a, const Point &b); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 17d49e1..7759d01 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -189,11 +189,13 @@ bool getNextMove() { } // reset steprate axis - axes[STEPRATE_AXIS].position <<= feedrate_scale_shift; - feedrate_scale_shift = 0; + if (feedrate_scale_shift != 0) { + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position << feedrate_scale_shift; + feedrate_scale_shift = 0; + } // step_rate is steps/sec, step_timing is sec/step - if (!current_block) { + if (current_block == 0) { FetchNext: if (planner::isBufferEmpty()) return false; @@ -208,27 +210,27 @@ bool getNextMove() { for (int i = 0; i < STEPPER_COUNT; i++) { // rearrange a little to avoid fractional math. // Should be the same as: (accelerate_until/step_event_count) * steps[i] -// axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); - axes[i].setTarget(current_block->steps[i], /*relative =*/ true); + axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); + // axes[i].setTarget(current_block->steps[i], /*relative =*/ true); } // setup steprate axis axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - + // axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); + // axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + // max_delta = current_block->accelerate_until; - - // XXX - axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - max_delta = current_block->step_event_count; - current_phase == DECELERATING; + // max_delta = current_block->step_event_count; + // + // current_phase = DECELERATING; + } else { current_phase = IN_PLATEAU; } } -#if 0 +#if 1 if (current_phase == IN_PLATEAU && current_block->accelerate_until != current_block->decelerate_after) { // setup all real axes for (int i = 0; i < STEPPER_COUNT; i++) { @@ -289,7 +291,6 @@ bool getNextMove() { for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } - is_running = true; return true; } From 8fd076f6e040cbc50f26d9aa92a92e71feff3162 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 4 Jan 2012 23:23:16 -0600 Subject: [PATCH 35/61] A bunch more sorted out, but still a ways to go. --- firmware/src/Motherboard/Host.cc | 3 +- firmware/src/Motherboard/Planner.cc | 145 +++++++++++++----- firmware/src/Motherboard/Planner.hh | 7 + firmware/src/Motherboard/Steppers.cc | 101 ++++++------ firmware/src/Motherboard/Steppers.hh | 2 +- .../boards/rrmbv12/Configuration.hh | 1 + .../Motherboard/boards/rrmbv12/Motherboard.hh | 4 +- 7 files changed, 164 insertions(+), 99 deletions(-) diff --git a/firmware/src/Motherboard/Host.cc b/firmware/src/Motherboard/Host.cc index 6a57fec..158635d 100644 --- a/firmware/src/Motherboard/Host.cc +++ b/firmware/src/Motherboard/Host.cc @@ -20,6 +20,7 @@ #include "Tool.hh" #include "Commands.hh" #include "Steppers.hh" +#include "Planner.hh" #include "DebugPacketProcessor.hh" #include "Timeout.hh" #include "Version.hh" @@ -411,7 +412,7 @@ enum { // bit assignments inline void handleExtendedStop(const InPacket& from_host, OutPacket& to_host) { uint8_t flags = from_host.read8(1); if (flags & _BV(ES_STEPPERS)) { - steppers::abort(); + planner::abort(); } if (flags & _BV(ES_COMMANDS)) { command::reset(); diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index a1cda8c..cbe93a8 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -74,15 +74,28 @@ /* Setup some utilities */ // undefine stdlib's abs if encountered -#ifdef abs -#undef abs +// #ifdef abs +// #undef abs +// #endif +#ifdef min +#undef min +#endif +#ifdef max +#undef max #endif -inline const int32_t& min(const int32_t& a, const int32_t& b) { return (a)<(b)?(a):(b); } -inline const int32_t& max(const int32_t& a, const int32_t& b) { return (a)>(b)?(a):(b); } +template +inline const T& min(const T& a, const T& b) { return (a)<(b)?(a):(b); } +// template inline const int32_t& min(const int32_t&, const int32_t&); +// template inline const uint32_t& min(const uint32_t&, const uint32_t&); template -inline T abs(T x) { return (x)>0?(x):-(x); } +inline const T& max(const T& a, const T& b) { return (a)>(b)?(a):(b); } +// template inline const int32_t& max(const int32_t&, const int32_t&); +// template inline const uint32_t& max(const uint32_t&, const uint32_t&); + +// template +// inline T abs(T x) { return (x)>0?(x):-(x); } // #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) // #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) @@ -172,6 +185,11 @@ namespace planner { inline int16_t getUsedCount() { return ((head-tail+size) & size_mask); } + + inline void clear() { + head = 0; + tail = 0; + } }; // this is very similar to the StepperAxis, but geared toward planning @@ -193,7 +211,7 @@ namespace planner { PlannerAxis axes[AXIS_COUNT]; - float acceleration; + float default_acceleration; Point position; // the current position (planning-wise, not bot/stepper-wise) in steps float previous_speed[AXIS_COUNT]; // Speed of previous path line segment float previous_nominal_speed; // Nominal speed of previous path line segment @@ -206,7 +224,7 @@ namespace planner { void init() { for (int i = 0; i < AXIS_COUNT; i++) { - axes[i] = PlannerAxis(); // redundant, or a reset? + // axes[i] = PlannerAxis(); // redundant, or a reset? previous_speed[i] = 0.0; } @@ -234,9 +252,9 @@ namespace planner { } void setAcceleration(float new_acceleration) { - acceleration = new_acceleration; + default_acceleration = new_acceleration; // for (int i = 0; i < AXIS_COUNT; i++) { - // axes[i].max_acceleration = acceleration * axes[i].steps_per_mm; + // axes[i].max_acceleration = default_acceleration * axes[i].steps_per_mm; // } } @@ -274,12 +292,13 @@ namespace planner { } // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. + // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); void Block::calculate_trapezoid(float exit_factor_speed) { float entry_factor = entry_speed/nominal_speed; float exit_factor = exit_factor_speed/nominal_speed; - initial_rate = ceil(nominal_rate*entry_factor); // (step/min) - final_rate = ceil(nominal_rate*exit_factor); // (step/min) + initial_rate = ceil((float)nominal_rate*entry_factor); // (step/min) + final_rate = ceil((float)nominal_rate*exit_factor); // (step/min) // Limit minimal step rate (Otherwise the timer will overflow.) if(initial_rate < 120) @@ -302,8 +321,8 @@ namespace planner { if (plateau_steps < 0) { accelerate_steps = ceil( intersection_distance(initial_rate, final_rate, acceleration, step_event_count)); - accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off - accelerate_steps = min(accelerate_steps, step_event_count); + accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off + accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); plateau_steps = 0; } @@ -485,7 +504,7 @@ namespace planner { // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) - { + { if (block_buffer.isFull()) return false; @@ -507,30 +526,25 @@ namespace planner { block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 -#if 1 // store the absolute number of steps in each direction, without direction Point steps = Point( - abs(target[0] - position[0]), - abs(target[1] - position[1]), - abs(target[2] - position[2]), - abs(target[3] - position[3]), - abs(target[4] - position[4]) + abs(block->steps[0]), + abs(block->steps[1]), + abs(block->steps[2]), + abs(block->steps[3]), + abs(block->steps[4]) ); block->step_event_count = 0; - uint8_t master_axis = 0; + // uint8_t master_axis = 0; for (int i = 0; i < AXIS_COUNT; i++) { - if (steps[i] > block->step_event_count) { + if (block->step_event_count < steps[i]) { block->step_event_count = steps[i]; - master_axis = i; - // steps_per_mm = block->step_event_count/block->millimeters; - - // WARNING, Edge case: Two axis got the same number of steps, but have different steps_per_mm values - // No way to tell which one to choose. - // Need to change the call interface to fix this, though. + // master_axis = i; } } +#if 0 // // Compute direction bits for this block -- UNUSED FOR NOW // block->direction_bits = 0; // for (int i = 0; i < AXIS_COUNT; i++) { @@ -538,17 +552,17 @@ namespace planner { // } float delta_mm[AXIS_COUNT]; - block->millimeters = 0; + block->millimeters = 0.0; for (int i = 0; i < AXIS_COUNT; i++) { - delta_mm[i] = steps[i]/axes[i].steps_per_mm; + delta_mm[i] = ((float)steps[i])/axes[i].steps_per_mm; block->millimeters += delta_mm[i] * delta_mm[i]; } block->millimeters = sqrt(block->millimeters); float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate 1 second/(seconds for this movement) - float inverse_second = 1000000/(us_per_step * block->step_event_count); - float steps_per_mm = block->step_event_count * inverse_millimeters; + float inverse_second = 1000000.0/(float)(us_per_step * block->step_event_count); + float steps_per_mm = (float)block->step_event_count * inverse_millimeters; // we are given microseconds/step, and we need steps/mm, and steps/second @@ -568,7 +582,7 @@ namespace planner { current_speed[i] = delta_mm[i] * inverse_second; } - // Limit speed per axis (already done in RepG) + // // Limit speed per axis (already done in RepG) // float speed_factor = 1.0; //factor <=1 do decrease speed // for(int i=0; i < AXIS_COUNT; i++) { // if(abs(current_speed[i]) > max_feedrate[i]) @@ -577,21 +591,42 @@ namespace planner { // TODO fancy frequency checks + + // // Correct the speed + // if( speed_factor < 1.0) { + // // Serial.print("speed factor : "); Serial.println(speed_factor); + // for(int i=0; i < 4; i++) { + // if(abs(current_speed[i]) > max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + // /* + // if(speed_factor < 0.1) { + // Serial.print("speed factor : "); Serial.println(speed_factor); + // Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]); + // } + // */ + // } + // for(unsigned char i=0; i < 4; i++) { + // current_speed[i] *= speed_factor; + // } + // block->nominal_speed *= speed_factor; + // block->nominal_rate *= speed_factor; + // } + // Compute and limit the acceleration rate for the trapezoid generator. - block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + block->acceleration_st = ceil(default_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis for(int i=0; i < AXIS_COUNT; i++) { - if(((float)block->acceleration_st * (float)steps[i] / (float)block->step_event_count) > axes[i].max_acceleration) + if((uint32_t)((float)block->acceleration_st * (float)steps[i] / (float)block->step_event_count) > axes[i].max_acceleration) block->acceleration_st = axes[i].max_acceleration; } block->acceleration = block->acceleration_st / steps_per_mm; - block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? + block->acceleration_rate = (int32_t)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? // Compute the speed trasitions, or "jerks" // Start with a safe speed - float vmax_junction = max_xy_jerk/2; + float vmax_junction = max_xy_jerk/2.0; { - float half_max_z_axis_jerk = axes[Z_AXIS].max_axis_jerk/2; + float half_max_z_axis_jerk = axes[Z_AXIS].max_axis_jerk/2.0; if(abs(current_speed[Z_AXIS]) > half_max_z_axis_jerk) vmax_junction = half_max_z_axis_jerk; } @@ -617,7 +652,7 @@ namespace planner { block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + float v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -635,11 +670,26 @@ namespace planner { block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed - memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + // memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + previous_speed[0] = current_speed[0]; + previous_speed[1] = current_speed[1]; + previous_speed[2] = current_speed[2]; + previous_speed[3] = current_speed[3]; + previous_speed[4] = current_speed[4]; + previous_nominal_speed = block->nominal_speed; block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); #endif + + block->accelerate_until = block->step_event_count / 3; + block->decelerate_after = block->step_event_count - block->accelerate_until; + + block->initial_rate = 120; + block->final_rate = 240; + + block->start_position = position; + // Update position position[0] = target[0]; position[1] = target[1]; @@ -650,7 +700,7 @@ namespace planner { // Move buffer head -- should this move to after recalulate? block_buffer.bumpHead(); - planner_recalculate(); + //planner_recalculate(); steppers::startRunning(); return true; @@ -662,7 +712,18 @@ namespace planner { { // STUB } - + + void abort() { + position = steppers::getPosition(); + + // reset speed + for (int i = 0; i < AXIS_COUNT; i++) { + previous_speed[i] = 0.0; + } + + block_buffer.clear(); + } + void definePosition(const Point& new_position) { position = new_position; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index cb5afd1..b108074 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -43,6 +43,7 @@ namespace planner { public: // Fields used by the bresenham algorithm for tracing the line Point steps; // Step count and direction (may be negative) along each axis + Point start_position; // Step count and direction (may be negative) along each axis uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration int32_t decelerate_after; // The index of the step event on which to start decelerating @@ -73,6 +74,8 @@ namespace planner { uint32_t acceleration_st; // acceleration steps/sec^2 uint8_t busy; + Block() : steps(), start_position() {}; + // functions void calculate_trapezoid(float exit_factor_speed); }; @@ -99,6 +102,10 @@ namespace planner { /// \param[in] position New system position void definePosition(const Point& position); + /// Abort the current motion (and all planeed movments) and set the stepper subsystem to + /// the not-running state. + void abort(); + /// Get the current system position /// \return The current machine position. const Point getPosition(); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 7759d01..dec7198 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -19,6 +19,7 @@ #include "Steppers.hh" #include "StepperAxis.hh" #include "Planner.hh" +#include #include namespace steppers { @@ -38,6 +39,14 @@ volatile bool is_homing; bool holdZ = false; +planner::Block *current_block; + +enum { + ACCELERATING = 1, + IN_PLATEAU, + DECELERATING +} current_phase; // pretend we are just leaving a block + bool isRunning() { return is_running || is_homing; } @@ -52,6 +61,8 @@ void init(Motherboard& motherboard) { axes[STEPRATE_AXIS] = StepperAxis(); timer_counter = 0; feedrate_scale_shift = 0; + current_phase = DECELERATING; + current_block = 0; } void abort() { @@ -164,28 +175,22 @@ is_running = true; } */ -planner::Block *current_block = 0; -enum { - ACCELERATING = 1, - IN_PLATEAU, - DECELERATING -} current_phase = DECELERATING; // pretend we are just leaving a block - // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { uint32_t max_delta = 0; + int32_t new_interval_position = 0; is_running = false; - + if (current_phase == DECELERATING) { - current_phase == ACCELERATING; + current_phase = ACCELERATING; current_block = 0; } else if (current_phase == ACCELERATING) { - current_phase == IN_PLATEAU; + current_phase = IN_PLATEAU; } else if (current_phase == IN_PLATEAU) { - current_phase == DECELERATING; + current_phase = DECELERATING; } // reset steprate axis @@ -203,68 +208,58 @@ bool getNextMove() { current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) current_block->busy = true; + + // make sure we start at the starting speed + axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].position = current_block->start_position[i]; + } - current_phase = ACCELERATING; - if (current_block->accelerate_until > 0) { + // if (current_block->accelerate_until > 0) { + // force this + current_phase = ACCELERATING; + // setup all real axes - for (int i = 0; i < STEPPER_COUNT; i++) { - // rearrange a little to avoid fractional math. - // Should be the same as: (accelerate_until/step_event_count) * steps[i] - axes[i].setTarget((current_block->accelerate_until * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); - // axes[i].setTarget(current_block->steps[i], /*relative =*/ true); - } - - // setup steprate axis - axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - // axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); - // axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - // max_delta = current_block->accelerate_until; - // max_delta = current_block->step_event_count; - // - // current_phase = DECELERATING; + new_interval_position = current_block->accelerate_until; - - } else { - current_phase = IN_PLATEAU; - } + // setup steprate axis target + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + // } else { + // current_phase = IN_PLATEAU; + // } } -#if 1 + if (current_phase == IN_PLATEAU && current_block->accelerate_until != current_block->decelerate_after) { // setup all real axes - for (int i = 0; i < STEPPER_COUNT; i++) { - // rearrange a little to avoid fractional math. - // Should be the same as: (decelerate_after/step_event_count) * steps[i] - axes[i].setTarget((current_block->decelerate_after * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); - } + max_delta = current_block->decelerate_after - current_block->accelerate_until; + new_interval_position = current_block->decelerate_after; - // setup steprate axis, using the current position properly + // setup steprate axis target, using the current position properly axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - max_delta = current_block->step_event_count - current_block->accelerate_until - current_block->decelerate_after; } else { current_phase = DECELERATING; } if (current_phase == DECELERATING && current_block->decelerate_after != current_block->step_event_count) { - // setup all real axes - for (int i = 0; i < STEPPER_COUNT; i++) { - // rearrange a little to avoid fractional math. - // Should be the same as: (decelerate_after/step_event_count) * steps[i] - axes[i].setTarget((current_block->decelerate_after * current_block->steps[i]) / current_block->step_event_count, /*relative =*/ true); - } + max_delta = current_block->step_event_count - current_block->decelerate_after; + new_interval_position = current_block->step_event_count; - // setup steprate axis, using the current position properly + // setup steprate axis target, using the current position properly axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); } else { - current_phase = ACCELERATING; // My apologies for a goto, but we're in a hurry! -Rob goto FetchNext; } -#endif + for (int i = 0; i < STEPPER_COUNT; i++) { - const int32_t delta = axes[i].delta; + // rearrange a little to avoid fractional math. + // Should be the same as: (max_delta/step_event_count) * steps[i] + const int32_t delta = floor(((float)new_interval_position * (float)current_block->steps[i]) / (float)current_block->step_event_count); + + axes[i].setTarget(current_block->start_position[i] + delta, /*relative =*/ false); + // Only shut z axis on inactivity if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); else if (delta != 0) axes[i].enableStepper(true); @@ -330,9 +325,9 @@ bool doInterrupt() { timer_counter -= INTERVAL_IN_MICROSECONDS; if (timer_counter <= 0) { if (intervals_remaining-- == 0) { - // if (!getNextMove()) { + if (!getNextMove()) { is_running = false; - // } + } } else { for (int i = 0; i < ALL_AXIS_COUNT; i++) { diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index ee42f2b..397b8dc 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -51,7 +51,7 @@ namespace steppers { /// given position. /// \param[in] target Position to move to /// \param[in] dda_interval Motion speed, in us per step. - void setTarget(const Point& target, int32_t dda_interval); + // void setTarget(const Point& target, int32_t dda_interval); /// Instruct the stepper subsystem to move the machine to the /// given position. diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 0543341..a1251aa 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -158,6 +158,7 @@ // THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 +//#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_MAX_XY_JERK 20.0 #define DEFAULT_MAX_Z_JERK 0.4 diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh index a151486..847d6a9 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.hh @@ -39,8 +39,8 @@ private: /// Microseconds since board initialization volatile micros_t micros; - /// Private constructor; use the singleton - Motherboard(const Pin& psu_pin); + /// Private constructor; use the singleton + Motherboard(const Pin& psu_pin); static Motherboard motherboard; public: From 7a69996b0090b2df9081d34071ff04a2226713b5 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 5 Jan 2012 00:26:40 -0600 Subject: [PATCH 36/61] =?UTF-8?q?Starting=20prep=20for=20new=20style.?= =?UTF-8?q?=E2=80=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- firmware/src/Motherboard/Planner.cc | 6 +- firmware/src/Motherboard/Planner.hh | 4 +- firmware/src/Motherboard/Steppers.cc | 162 ++++++++++++++------------- 3 files changed, 91 insertions(+), 81 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index cbe93a8..e660260 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -682,13 +682,13 @@ namespace planner { block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); #endif - block->accelerate_until = block->step_event_count / 3; - block->decelerate_after = block->step_event_count - block->accelerate_until; + block->accelerate_until = 10; + block->decelerate_after = block->step_event_count - 20; block->initial_rate = 120; block->final_rate = 240; - block->start_position = position; + block->target = target; // Update position position[0] = target[0]; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index b108074..bf4a27d 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -43,7 +43,7 @@ namespace planner { public: // Fields used by the bresenham algorithm for tracing the line Point steps; // Step count and direction (may be negative) along each axis - Point start_position; // Step count and direction (may be negative) along each axis + Point target; // Step count and direction (may be negative) along each axis uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration int32_t decelerate_after; // The index of the step event on which to start decelerating @@ -74,7 +74,7 @@ namespace planner { uint32_t acceleration_st; // acceleration steps/sec^2 uint8_t busy; - Block() : steps(), start_position() {}; + Block() : steps(), target() {}; // functions void calculate_trapezoid(float exit_factor_speed); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index dec7198..7547269 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -27,8 +27,16 @@ namespace steppers { volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; +struct feedrate_element { + int32_t intervals; + int32_t target; +}; +feedrate_element feedrate_elements[3]; +volatile int32_t feedrate_intervals_remaining; +volatile uint8_t current_feedrate_index; + volatile int32_t timer_counter; -int8_t feedrate_scale_shift; +volatile int8_t feedrate_scale_shift; // The ALL_AXIS_COUNT includes the virtual "speed" axis #define ALL_AXIS_COUNT STEPPER_COUNT+1 @@ -63,6 +71,14 @@ void init(Motherboard& motherboard) { feedrate_scale_shift = 0; current_phase = DECELERATING; current_block = 0; + + for (int i = 0; i < 3; i++) { + feedrate_elements[i] = feedrate_element(); + feedrate_elements[i].intervals = 0; + feedrate_elements[i].target = 0; + } + feedrate_intervals_remaining = 0; + } void abort() { @@ -178,96 +194,67 @@ is_running = true; // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { - uint32_t max_delta = 0; int32_t new_interval_position = 0; is_running = false; - - if (current_phase == DECELERATING) { - current_phase = ACCELERATING; - current_block = 0; - } - else if (current_phase == ACCELERATING) { - current_phase = IN_PLATEAU; - } - else if (current_phase == IN_PLATEAU) { - current_phase = DECELERATING; + + if (planner::isBufferEmpty()) + return false; + + current_block = planner::getNextBlock(); + // Mark block as busy (being executed by the stepper interrupt) + current_block->busy = true; + + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setTarget(current_block->target[i], /*relative =*/ false); } + - // reset steprate axis - if (feedrate_scale_shift != 0) { - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position << feedrate_scale_shift; - feedrate_scale_shift = 0; - } + current_feedrate_index = 0; - // step_rate is steps/sec, step_timing is sec/step - if (current_block == 0) { - FetchNext: - if (planner::isBufferEmpty()) - return false; - - current_block = planner::getNextBlock(); - // Mark block as busy (being executed by the stepper interrupt) - current_block->busy = true; - - // make sure we start at the starting speed - axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].position = current_block->start_position[i]; - } + // setup acceleration + feedrate_elements[0].intervals = current_block->accelerate_until; + feedrate_elements[0].target = 1000000/current_block->nominal_rate; - // if (current_block->accelerate_until > 0) { - // force this - current_phase = ACCELERATING; - - // setup all real axes - max_delta = current_block->accelerate_until; - new_interval_position = current_block->accelerate_until; - - // setup steprate axis target - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - // } else { - // current_phase = IN_PLATEAU; - // } - } + // setup plateau + feedrate_elements[1].intervals = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[1].target = 1000000/current_block->nominal_rate; - if (current_phase == IN_PLATEAU && current_block->accelerate_until != current_block->decelerate_after) { - // setup all real axes - max_delta = current_block->decelerate_after - current_block->accelerate_until; - new_interval_position = current_block->decelerate_after; + // setup deceleration + feedrate_elements[2].intervals = current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[2].target = 1000000/current_block->final_rate; - // setup steprate axis target, using the current position properly + if (feedrate_elements[0].intervals > 0) { + // setup the acceleration speed + axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - - } else { - current_phase = DECELERATING; - } - - if (current_phase == DECELERATING && current_block->decelerate_after != current_block->step_event_count) { - max_delta = current_block->step_event_count - current_block->decelerate_after; - new_interval_position = current_block->step_event_count; - - // setup steprate axis target, using the current position properly + } else if (feedrate_elements[1].intervals > 0) { + // setup the acceleration speed + axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + current_feedrate_index = 1; + } else if (feedrate_elements[2].intervals > 0) { + // setup the acceleration speed + axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); - } else { - // My apologies for a goto, but we're in a hurry! -Rob - goto FetchNext; + current_feedrate_index = 2; } + for (int i = 0; i < STEPPER_COUNT; i++) { - // rearrange a little to avoid fractional math. - // Should be the same as: (max_delta/step_event_count) * steps[i] - const int32_t delta = floor(((float)new_interval_position * (float)current_block->steps[i]) / (float)current_block->step_event_count); - - axes[i].setTarget(current_block->start_position[i] + delta, /*relative =*/ false); + axes[i].setTarget(current_block->target[i], /*relative =*/ false); + const int32_t delta = axes[i].delta; // Only shut z axis on inactivity if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); else if (delta != 0) axes[i].enableStepper(true); } - + + feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; + axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals_remaining) { scale_shift++; } if (scale_shift > 0) { @@ -280,7 +267,7 @@ bool getNextMove() { // We use += here so that the odd rounded-off time from the last move is still waited out timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; - intervals = max_delta; + intervals = current_block->step_event_count; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; for (int i = 0; i < ALL_AXIS_COUNT; i++) { @@ -325,15 +312,38 @@ bool doInterrupt() { timer_counter -= INTERVAL_IN_MICROSECONDS; if (timer_counter <= 0) { if (intervals_remaining-- == 0) { - if (!getNextMove()) { - is_running = false; - } + getNextMove(); + // is_running = false; } else { - for (int i = 0; i < ALL_AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } + + if (feedrate_intervals_remaining-- == 0) { + axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; + + current_feedrate_index++; + + axes[STEPRATE_AXIS].setTarget(feedrate_elements[current_feedrate_index].target, /*relative =*/ false); + + feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; + axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); + + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled + int8_t scale_shift = 0; + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals_remaining) { + scale_shift++; + } + if (scale_shift > 0) { + feedrate_scale_shift = scale_shift; + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; + axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; + axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } + } + axes[STEPRATE_AXIS].doInterrupt(feedrate_elements[current_feedrate_index].intervals); timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; } } From 6c64b5c18f361a06adbfae27d3ada17e2f7c0645 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Wed, 11 Jan 2012 18:42:37 -0600 Subject: [PATCH 37/61] Grrr --- firmware/src/Motherboard/Planner.cc | 60 ++++++++++--------- firmware/src/Motherboard/Planner.hh | 10 ++-- firmware/src/Motherboard/Point.cc | 12 ++++ firmware/src/Motherboard/Steppers.cc | 46 +++++++------- .../boards/rrmbv12/Configuration.hh | 2 +- 5 files changed, 76 insertions(+), 54 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index e660260..86aff3a 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -74,9 +74,9 @@ /* Setup some utilities */ // undefine stdlib's abs if encountered -// #ifdef abs -// #undef abs -// #endif +#ifdef abs +#undef abs +#endif #ifdef min #undef min #endif @@ -518,27 +518,26 @@ namespace planner { // // set the number of steps and direction for each axis // block->steps = target - position; - block->steps[0] = target[0] - position[0]; - block->steps[1] = target[1] - position[1]; - block->steps[2] = target[2] - position[2]; - block->steps[3] = target[3] - position[3]; - block->steps[4] = target[4] - position[4]; + // block->steps[0] = target[0] - position[0]; + // block->steps[1] = target[1] - position[1]; + // block->steps[2] = target[2] - position[2]; + // block->steps[3] = target[3] - position[3]; + // block->steps[4] = target[4] - position[4]; block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 // store the absolute number of steps in each direction, without direction - Point steps = Point( - abs(block->steps[0]), - abs(block->steps[1]), - abs(block->steps[2]), - abs(block->steps[3]), - abs(block->steps[4]) - ); + Point steps; + steps[0] = labs(target[0] - position[0]); + steps[1] = labs(target[1] - position[1]); + steps[2] = labs(target[2] - position[2]); + steps[3] = labs(target[3] - position[3]); + steps[4] = labs(target[4] - position[4]); block->step_event_count = 0; // uint8_t master_axis = 0; for (int i = 0; i < AXIS_COUNT; i++) { - if (block->step_event_count < steps[i]) { + if (steps[i] > block->step_event_count) { block->step_event_count = steps[i]; // master_axis = i; } @@ -585,8 +584,8 @@ namespace planner { // // Limit speed per axis (already done in RepG) // float speed_factor = 1.0; //factor <=1 do decrease speed // for(int i=0; i < AXIS_COUNT; i++) { - // if(abs(current_speed[i]) > max_feedrate[i]) - // speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + // if(fabs(current_speed[i]) > max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); // } // TODO fancy frequency checks @@ -596,8 +595,8 @@ namespace planner { // if( speed_factor < 1.0) { // // Serial.print("speed factor : "); Serial.println(speed_factor); // for(int i=0; i < 4; i++) { - // if(abs(current_speed[i]) > max_feedrate[i]) - // speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); + // if(fabs(current_speed[i]) > max_feedrate[i]) + // speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); // /* // if(speed_factor < 0.1) { // Serial.print("speed factor : "); Serial.println(speed_factor); @@ -627,7 +626,7 @@ namespace planner { float vmax_junction = max_xy_jerk/2.0; { float half_max_z_axis_jerk = axes[Z_AXIS].max_axis_jerk/2.0; - if(abs(current_speed[Z_AXIS]) > half_max_z_axis_jerk) + if(fabs(current_speed[Z_AXIS]) > half_max_z_axis_jerk) vmax_junction = half_max_z_axis_jerk; } @@ -643,7 +642,7 @@ namespace planner { // account for Z, A, and B for(int i=Z_AXIS; i < AXIS_COUNT; i++) { - float axis_jerk = abs(current_speed[i] - previous_speed[i]); + float axis_jerk = fabs(current_speed[i] - previous_speed[i]); if(axis_jerk > axes[i].max_axis_jerk) { vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); } @@ -682,13 +681,17 @@ namespace planner { block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); #endif - block->accelerate_until = 10; - block->decelerate_after = block->step_event_count - 20; - - block->initial_rate = 120; + block->accelerate_until = 30; + block->decelerate_after = block->step_event_count - block->accelerate_until; + + block->initial_rate = 240; block->final_rate = 240; - block->target = target; + block->target[0] = target[0]; + block->target[1] = target[1]; + block->target[2] = target[2]; + block->target[3] = target[3]; + block->target[4] = target[4]; // Update position position[0] = target[0]; @@ -714,6 +717,7 @@ namespace planner { } void abort() { + steppers::abort(); position = steppers::getPosition(); // reset speed @@ -721,7 +725,7 @@ namespace planner { previous_speed[i] = 0.0; } - block_buffer.clear(); + // block_buffer.clear(); } void definePosition(const Point& new_position) diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index bf4a27d..8fea4db 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -42,14 +42,14 @@ namespace planner { class Block { public: // Fields used by the bresenham algorithm for tracing the line - Point steps; // Step count and direction (may be negative) along each axis + // Point steps; // Step count and direction (may be negative) along each axis Point target; // Step count and direction (may be negative) along each axis uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t acceleration_rate; // The acceleration rate used for acceleration calculation - uint8_t direction_bits; // The direction bit set for this block - uint8_t active_extruder; // Selects the active extruder + // int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + // uint8_t direction_bits; // The direction bit set for this block + // uint8_t active_extruder; // Selects the active extruder #ifdef ADVANCE int32_t advance_rate; volatile int32_t initial_advance; @@ -74,7 +74,7 @@ namespace planner { uint32_t acceleration_st; // acceleration steps/sec^2 uint8_t busy; - Block() : steps(), target() {}; + Block() : target() {}; // functions void calculate_trapezoid(float exit_factor_speed); diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 10e40f9..fcbe82a 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -1,4 +1,5 @@ #include "Point.hh" +#include // for labs() /* Setup some utilities -- TODO: Move this to a common file */ @@ -7,9 +8,20 @@ #undef abs #endif +#ifdef labs +#undef labs +#endif + template inline T abs(T x) { return (x)>0?(x):-(x); } +template <> +inline int abs(int x) { return __builtin_abs(x); } + +template <> +inline long abs(long x) { return __builtin_labs(x); } + + /* end utilities */ Point::Point() diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 7547269..0f91fc0 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -203,14 +203,24 @@ bool getNextMove() { current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) current_block->busy = true; + + current_feedrate_index = 0; + int32_t max_delta = current_block->step_event_count; for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(current_block->target[i], /*relative =*/ false); - } + axes[i].setTarget(current_block->target[i], false); + const int32_t delta = axes[i].delta; - - current_feedrate_index = 0; - + // Only shut z axis on inactivity + if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); + else if (delta != 0) axes[i].enableStepper(true); + + if (delta > max_delta) { + max_delta = delta; + } + } + +#if 0 // setup acceleration feedrate_elements[0].intervals = current_block->accelerate_until; feedrate_elements[0].target = 1000000/current_block->nominal_rate; @@ -238,17 +248,12 @@ bool getNextMove() { axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); current_feedrate_index = 2; } - - - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(current_block->target[i], /*relative =*/ false); - const int32_t delta = axes[i].delta; +#else + // setup acceleration + feedrate_elements[0].intervals = max_delta; + feedrate_elements[0].target = 1000000/current_block->nominal_rate; +#endif - // Only shut z axis on inactivity - if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - else if (delta != 0) axes[i].enableStepper(true); - } - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); @@ -267,10 +272,10 @@ bool getNextMove() { // We use += here so that the odd rounded-off time from the last move is still waited out timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; - intervals = current_block->step_event_count; + intervals = max_delta; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < ALL_AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; @@ -312,14 +317,14 @@ bool doInterrupt() { timer_counter -= INTERVAL_IN_MICROSECONDS; if (timer_counter <= 0) { if (intervals_remaining-- == 0) { - getNextMove(); - // is_running = false; + // getNextMove(); + is_running = false; } else { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } - + #if 1 if (feedrate_intervals_remaining-- == 0) { axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; @@ -344,6 +349,7 @@ bool doInterrupt() { } axes[STEPRATE_AXIS].doInterrupt(feedrate_elements[current_feedrate_index].intervals); + #endif timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; } } diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index a1251aa..80dbc51 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -156,7 +156,7 @@ // The number of movements we can plan ahead at a time // THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... -#define BLOCK_BUFFER_SIZE 16 +#define BLOCK_BUFFER_SIZE 8 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ From dcb66ef46aa7f9a7162116620f77458d67a75624 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 12 Jan 2012 01:00:23 -0600 Subject: [PATCH 38/61] Moving, again. --- firmware/src/Motherboard/Command.cc | 2 +- firmware/src/Motherboard/Planner.cc | 76 +++++++++++++--------------- firmware/src/Motherboard/Point.cc | 36 ++++++------- firmware/src/Motherboard/Point.hh | 4 +- firmware/src/Motherboard/Steppers.cc | 35 +++++++------ firmware/src/Motherboard/Steppers.hh | 2 +- 6 files changed, 75 insertions(+), 80 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 083f99d..b3c9c04 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -30,7 +30,7 @@ namespace command { -#define COMMAND_BUFFER_SIZE 512 +#define COMMAND_BUFFER_SIZE 256 //512 uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 86aff3a..3d6ef51 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -186,10 +186,10 @@ namespace planner { return ((head-tail+size) & size_mask); } - inline void clear() { - head = 0; - tail = 0; - } + // inline void clear() { + // head = 0; + // tail = 0; + // } }; // this is very similar to the StepperAxis, but geared toward planning @@ -511,6 +511,8 @@ namespace planner { Block *block = block_buffer.getHead(); // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; + + block->target = target; // // calculate the difference between the current position and the target // Point delta = target - position; @@ -526,22 +528,22 @@ namespace planner { block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 - // store the absolute number of steps in each direction, without direction - Point steps; - steps[0] = labs(target[0] - position[0]); - steps[1] = labs(target[1] - position[1]); - steps[2] = labs(target[2] - position[2]); - steps[3] = labs(target[3] - position[3]); - steps[4] = labs(target[4] - position[4]); - - block->step_event_count = 0; - // uint8_t master_axis = 0; - for (int i = 0; i < AXIS_COUNT; i++) { - if (steps[i] > block->step_event_count) { - block->step_event_count = steps[i]; - // master_axis = i; - } - } + // // store the absolute number of steps in each direction, without direction + // Point steps; + // steps[0] = labs(target[0] - position[0]); + // steps[1] = labs(target[1] - position[1]); + // steps[2] = labs(target[2] - position[2]); + // steps[3] = labs(target[3] - position[3]); + // steps[4] = labs(target[4] - position[4]); + // + // block->step_event_count = 0; + // // uint8_t master_axis = 0; + // for (int i = 0; i < AXIS_COUNT; i++) { + // if (steps[i] > block->step_event_count) { + // block->step_event_count = steps[i]; + // // master_axis = i; + // } + // } #if 0 // // Compute direction bits for this block -- UNUSED FOR NOW @@ -681,26 +683,20 @@ namespace planner { block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); #endif - block->accelerate_until = 30; - block->decelerate_after = block->step_event_count - block->accelerate_until; - - block->initial_rate = 240; - block->final_rate = 240; - - block->target[0] = target[0]; - block->target[1] = target[1]; - block->target[2] = target[2]; - block->target[3] = target[3]; - block->target[4] = target[4]; - - // Update position - position[0] = target[0]; - position[1] = target[1]; - position[2] = target[2]; - position[3] = target[3]; - position[4] = target[4]; - - // Move buffer head -- should this move to after recalulate? + // block->accelerate_until = 30; + // block->decelerate_after = block->step_event_count - block->accelerate_until; + // + // block->initial_rate = 240; + // block->final_rate = 240; + // + // // Update position + // position[0] = target[0]; + // position[1] = target[1]; + // position[2] = target[2]; + // position[3] = target[3]; + // position[4] = target[4]; + // + // Move buffer head -- should this move to after recalculate? block_buffer.bumpHead(); //planner_recalculate(); diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index fcbe82a..68a9450 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -31,16 +31,16 @@ Point::Point() } } -Point::Point(const Point &other) -{ - coordinates[0] = other.coordinates[0]; - coordinates[1] = other.coordinates[1]; - coordinates[2] = other.coordinates[2]; -#if AXIS_COUNT > 3 - coordinates[3] = other.coordinates[3]; - coordinates[4] = other.coordinates[4]; -#endif -} +// Point::Point(const Point &other) +// { +// coordinates[0] = other.coordinates[0]; +// coordinates[1] = other.coordinates[1]; +// coordinates[2] = other.coordinates[2]; +// #if AXIS_COUNT > 3 +// coordinates[3] = other.coordinates[3]; +// coordinates[4] = other.coordinates[4]; +// #endif +// } Point::Point(const int32_t x, const int32_t y, const int32_t z, const int32_t a, const int32_t b) { @@ -83,14 +83,14 @@ Point operator- (const Point &a, const Point &b) { return c; } -Point &Point::operator=(const Point &other) { - coordinates[0] = other.coordinates[0]; - coordinates[1] = other.coordinates[1]; - coordinates[2] = other.coordinates[2]; - coordinates[3] = other.coordinates[3]; - coordinates[4] = other.coordinates[4]; - return *this; -} +// Point &Point::operator=(const Point &other) { +// coordinates[0] = other.coordinates[0]; +// coordinates[1] = other.coordinates[1]; +// coordinates[2] = other.coordinates[2]; +// coordinates[3] = other.coordinates[3]; +// coordinates[4] = other.coordinates[4]; +// return *this; +// } Point Point::abs() { diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index d5f5b4e..cb36dd7 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -17,7 +17,7 @@ public: Point(); /// Copy Point constructor - Point(const Point &other); + // Point(const Point &other); /// Construct a point with the given cooridnates. Coordinates are in /// stepper steps. @@ -53,7 +53,7 @@ public: /// Subtraction operator, for fast deltas friend Point operator- (const Point &a, const Point &b); - Point & operator= (const Point &other); + // Point & operator= (const Point &other); // friend const Point &operator-(const Point &a, const Point &b); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 0f91fc0..75724e6 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -151,10 +151,6 @@ void setTarget(const Point& target, int32_t dda_interval) { axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; } - // - // if (axes[STEPRATE_AXIS].delta > max_delta) { - // max_delta = axes[STEPRATE_AXIS].delta; - // } // We use += here so that the odd rounded-off time from the last move is still waited out timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; @@ -193,8 +189,7 @@ is_running = true; // load up the next movment // WARNING: called from inside the ISR, so get out fast -bool getNextMove() { - int32_t new_interval_position = 0; +bool getNextMove() { is_running = false; if (planner::isBufferEmpty()) @@ -203,12 +198,12 @@ bool getNextMove() { current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) current_block->busy = true; + Point &target = current_block->target; + int32_t dda_interval = 1000000/current_block->nominal_rate; - current_feedrate_index = 0; - - int32_t max_delta = current_block->step_event_count; + int32_t max_delta = 0;//current_block->step_event_count; for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(current_block->target[i], false); + axes[i].setTarget(target[i], false); const int32_t delta = axes[i].delta; // Only shut z axis on inactivity @@ -221,6 +216,8 @@ bool getNextMove() { } #if 0 + current_feedrate_index = 0; + // setup acceleration feedrate_elements[0].intervals = current_block->accelerate_until; feedrate_elements[0].target = 1000000/current_block->nominal_rate; @@ -248,18 +245,18 @@ bool getNextMove() { axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); current_feedrate_index = 2; } -#else - // setup acceleration - feedrate_elements[0].intervals = max_delta; - feedrate_elements[0].target = 1000000/current_block->nominal_rate; -#endif feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); +#else + axes[STEPRATE_AXIS].definePosition(dda_interval); + axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); +#endif + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals_remaining) { + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { scale_shift++; } if (scale_shift > 0) { @@ -275,7 +272,7 @@ bool getNextMove() { intervals = max_delta; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < STEPPER_COUNT; i++) { + for (int i = 0; i < ALL_AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; @@ -324,7 +321,7 @@ bool doInterrupt() { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } - #if 1 + #if 0 if (feedrate_intervals_remaining-- == 0) { axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; @@ -349,6 +346,8 @@ bool doInterrupt() { } axes[STEPRATE_AXIS].doInterrupt(feedrate_elements[current_feedrate_index].intervals); + #else + axes[STEPRATE_AXIS].doInterrupt(intervals); #endif timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; } diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index 397b8dc..ee42f2b 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -51,7 +51,7 @@ namespace steppers { /// given position. /// \param[in] target Position to move to /// \param[in] dda_interval Motion speed, in us per step. - // void setTarget(const Point& target, int32_t dda_interval); + void setTarget(const Point& target, int32_t dda_interval); /// Instruct the stepper subsystem to move the machine to the /// given position. From cca5b52baa1a07f6e3a8ba48e2e99eb3ca85de59 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 12 Jan 2012 01:52:18 -0600 Subject: [PATCH 39/61] Adding (stub) support for latest avr-libc changes. --- firmware/src/SConscript.extruder | 1 + firmware/src/SConscript.motherboard | 1 + 2 files changed, 2 insertions(+) diff --git a/firmware/src/SConscript.extruder b/firmware/src/SConscript.extruder index 9df8453..0a8abf3 100644 --- a/firmware/src/SConscript.extruder +++ b/firmware/src/SConscript.extruder @@ -88,6 +88,7 @@ include_paths = ['shared', 'Extruder', 'Extruder/boards/%s' % platform, '.'] flags=[ '-DF_CPU='+str(f_cpu), '-DVERSION='+str(version), + '-D__PROG_TYPES_COMPAT__', # see http://www.nongnu.org/avr-libc//changes-1.8.html '-mmcu='+mcu, '-g', '-Os', diff --git a/firmware/src/SConscript.motherboard b/firmware/src/SConscript.motherboard index 12ffb55..eafc148 100644 --- a/firmware/src/SConscript.motherboard +++ b/firmware/src/SConscript.motherboard @@ -105,6 +105,7 @@ include_paths = ['shared', 'Motherboard','Motherboard/boards/%s' % platform, '.' flags=[ '-DF_CPU='+str(f_cpu), '-DVERSION='+str(version), + '-D__PROG_TYPES_COMPAT__', # see http://www.nongnu.org/avr-libc//changes-1.8.html '-mmcu='+mcu, '-g', '-Os', From 605e9e6e402c38177676dfd6da14c480b16b5e15 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 12 Jan 2012 15:55:04 -0600 Subject: [PATCH 40/61] Five axes, even for four steppers! --- .../src/Motherboard/boards/rrmbv12/Configuration.hh | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 80dbc51..eebefb8 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -72,7 +72,8 @@ #define STEPPER_COUNT 3 #else // Rob G's hacked G3 motherboard supports four steppers. - #define STEPPER_COUNT 4 + // why 5? Because everywhere there are checks for more than 3 that assume 5 + #define STEPPER_COUNT 5 #endif // FOURTH_STEPPER // --- Stepper and endstop configuration --- @@ -142,6 +143,13 @@ #define A_DIR_PIN Pin(PortA,5) // The A stepper enable pin (active low) #define A_ENABLE_PIN Pin(PortA,0) + + // The B stepper step pin (active on rising edge) + #define B_STEP_PIN Pin() + // The B direction pin (forward on logic high) + #define B_DIR_PIN Pin() + // The B stepper enable pin (active low) + #define B_ENABLE_PIN Pin() #endif // FOURTH_STEPPER From 434a9cbb33d93cf4ebf23af1a3e2626e4f5a02a5 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Fri, 13 Jan 2012 15:59:37 -0600 Subject: [PATCH 41/61] Working, except the planner isn't doing lookahead properly. --- firmware/src/Motherboard/Command.cc | 16 ++--- firmware/src/Motherboard/Planner.cc | 72 ++++++------------- firmware/src/Motherboard/Planner.hh | 7 +- firmware/src/Motherboard/Point.cc | 23 +++--- firmware/src/Motherboard/Steppers.cc | 65 ++++++++++------- .../boards/rrmbv12/Configuration.hh | 2 +- 6 files changed, 92 insertions(+), 93 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index b3c9c04..559f4be 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -30,7 +30,7 @@ namespace command { -#define COMMAND_BUFFER_SIZE 256 //512 +#define COMMAND_BUFFER_SIZE 256 uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); @@ -171,7 +171,7 @@ void runCommandSlice() { if (!steppers::isRunning()) { mode = READY; } else if (homing_timeout.hasElapsed()) { - steppers::abort(); + planner::abort(); mode = READY; } } @@ -179,12 +179,12 @@ void runCommandSlice() { if (!steppers::isRunning()) { mode = READY; } else { - // if (command_buffer.getLength() > 0) { - // uint8_t command = command_buffer[0]; - // if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { - // handleMovementCommand(command); - // } - // } + if (command_buffer.getLength() > 0) { + uint8_t command = command_buffer[0]; + if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + handleMovementCommand(command); + } + } } } if (mode == DELAY) { diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 3d6ef51..ba4ee42 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -186,10 +186,10 @@ namespace planner { return ((head-tail+size) & size_mask); } - // inline void clear() { - // head = 0; - // tail = 0; - // } + inline void clear() { + head = 0; + tail = 0; + } }; // this is very similar to the StepperAxis, but geared toward planning @@ -233,7 +233,7 @@ namespace planner { axes[0].max_acceleration = 9000*axes[0].steps_per_mm; axes[1].max_acceleration = 9000*axes[1].steps_per_mm; - axes[2].max_acceleration = 100*axes[2].steps_per_mm; + axes[2].max_acceleration = 50*axes[2].steps_per_mm; axes[3].max_acceleration = 9000*axes[3].steps_per_mm; axes[4].max_acceleration = 9000*axes[4].steps_per_mm; } @@ -497,10 +497,13 @@ namespace planner { Block *getNextBlock() { Block *block = block_buffer.getTail(); - block_buffer.bumpTail(); return block; } + void doneWithNextBlock() { + block_buffer.bumpTail(); + } + // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) @@ -514,38 +517,19 @@ namespace planner { block->target = target; - // // calculate the difference between the current position and the target - // Point delta = target - position; - // - // // set the number of steps and direction for each axis - // block->steps = target - position; - - // block->steps[0] = target[0] - position[0]; - // block->steps[1] = target[1] - position[1]; - // block->steps[2] = target[2] - position[2]; - // block->steps[3] = target[3] - position[3]; - // block->steps[4] = target[4] - position[4]; - block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 // // store the absolute number of steps in each direction, without direction - // Point steps; - // steps[0] = labs(target[0] - position[0]); - // steps[1] = labs(target[1] - position[1]); - // steps[2] = labs(target[2] - position[2]); - // steps[3] = labs(target[3] - position[3]); - // steps[4] = labs(target[4] - position[4]); - // - // block->step_event_count = 0; - // // uint8_t master_axis = 0; - // for (int i = 0; i < AXIS_COUNT; i++) { - // if (steps[i] > block->step_event_count) { - // block->step_event_count = steps[i]; - // // master_axis = i; - // } - // } + Point steps = (target - position).abs(); + + block->step_event_count = 0; + for (int i = 0; i < AXIS_COUNT; i++) { + if (steps[i] > block->step_event_count) { + block->step_event_count = steps[i]; + } + } -#if 0 +#if 1 // // Compute direction bits for this block -- UNUSED FOR NOW // block->direction_bits = 0; // for (int i = 0; i < AXIS_COUNT; i++) { @@ -683,23 +667,13 @@ namespace planner { block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); #endif - // block->accelerate_until = 30; - // block->decelerate_after = block->step_event_count - block->accelerate_until; - // - // block->initial_rate = 240; - // block->final_rate = 240; - // - // // Update position - // position[0] = target[0]; - // position[1] = target[1]; - // position[2] = target[2]; - // position[3] = target[3]; - // position[4] = target[4]; - // + // Update position + position = target; + // Move buffer head -- should this move to after recalculate? block_buffer.bumpHead(); - //planner_recalculate(); + planner_recalculate(); steppers::startRunning(); return true; @@ -721,7 +695,7 @@ namespace planner { previous_speed[i] = 0.0; } - // block_buffer.clear(); + block_buffer.clear(); } void definePosition(const Point& new_position) diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 8fea4db..6bb724d 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -47,7 +47,7 @@ namespace planner { uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration int32_t decelerate_after; // The index of the step event on which to start decelerating - // int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + int32_t acceleration_rate; // The acceleration rate used for acceleration calculation // uint8_t direction_bits; // The direction bit set for this block // uint8_t active_extruder; // Selects the active extruder #ifdef ADVANCE @@ -120,8 +120,11 @@ namespace planner { bool isBufferFull(); bool isBufferEmpty(); - // Fetches the *tail* and bumps the tail + // Fetches the *tail* Block *getNextBlock(); + + // pushes the tail forward, making it available + void doneWithNextBlock(); } #endif /* end of include guard: PLANNER_HH */ diff --git a/firmware/src/Motherboard/Point.cc b/firmware/src/Motherboard/Point.cc index 68a9450..c33a130 100644 --- a/firmware/src/Motherboard/Point.cc +++ b/firmware/src/Motherboard/Point.cc @@ -26,9 +26,11 @@ inline long abs(long x) { return __builtin_labs(x); } Point::Point() { - for (uint16_t i = 0; i < 5; i++) { - coordinates[i] = 0; - } + // coordinates[0] = 0; + // coordinates[1] = 0; + // coordinates[2] = 0; + // coordinates[3] = 0; + // coordinates[4] = 0; } // Point::Point(const Point &other) @@ -77,8 +79,10 @@ Point operator- (const Point &a, const Point &b) { a.coordinates[0] - b.coordinates[0], a.coordinates[1] - b.coordinates[1], a.coordinates[2] - b.coordinates[2], +#if AXIS_COUNT > 3 a.coordinates[3] - b.coordinates[3], a.coordinates[4] - b.coordinates[4] +#endif ); return c; } @@ -94,13 +98,14 @@ Point operator- (const Point &a, const Point &b) { Point Point::abs() { - Point absPoint = Point(); - absPoint.coordinates[0] = ::abs(coordinates[0]); - absPoint.coordinates[1] = ::abs(coordinates[1]); - absPoint.coordinates[2] = ::abs(coordinates[2]); + Point absPoint = Point( + ::abs(coordinates[0]), + ::abs(coordinates[1]), + ::abs(coordinates[2]), #if AXIS_COUNT > 3 - absPoint.coordinates[3] = ::abs(coordinates[3]); - absPoint.coordinates[4] = ::abs(coordinates[4]); + ::abs(coordinates[3]), + ::abs(coordinates[4]) #endif + ); return absPoint; } \ No newline at end of file diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 75724e6..ae446dc 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -27,11 +27,13 @@ namespace steppers { volatile bool is_running; int32_t intervals; volatile int32_t intervals_remaining; + struct feedrate_element { int32_t intervals; int32_t target; }; feedrate_element feedrate_elements[3]; +volatile int32_t feedrate_intervals; volatile int32_t feedrate_intervals_remaining; volatile uint8_t current_feedrate_index; @@ -86,6 +88,8 @@ void abort() { is_homing = false; timer_counter = 0; feedrate_scale_shift = 0; + feedrate_intervals = 0; + feedrate_intervals_remaining = 0; } /// Define current position as given point @@ -190,18 +194,18 @@ is_running = true; // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { - is_running = false; + is_running = false; // this ensures that the interrupt does not .. interrupt us if (planner::isBufferEmpty()) return false; - + current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) current_block->busy = true; + Point &target = current_block->target; - int32_t dda_interval = 1000000/current_block->nominal_rate; - int32_t max_delta = 0;//current_block->step_event_count; + int32_t max_delta = current_block->step_event_count; for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], false); const int32_t delta = axes[i].delta; @@ -210,14 +214,13 @@ bool getNextMove() { if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); else if (delta != 0) axes[i].enableStepper(true); - if (delta > max_delta) { - max_delta = delta; - } + // if (delta > max_delta) { + // max_delta = delta; + // } } -#if 0 current_feedrate_index = 0; - +#if 1 // setup acceleration feedrate_elements[0].intervals = current_block->accelerate_until; feedrate_elements[0].target = 1000000/current_block->nominal_rate; @@ -235,28 +238,30 @@ bool getNextMove() { axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); } else if (feedrate_elements[1].intervals > 0) { - // setup the acceleration speed + // setup the plateau speed axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); current_feedrate_index = 1; } else if (feedrate_elements[2].intervals > 0) { - // setup the acceleration speed + // setup the deceleration speed axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); current_feedrate_index = 2; } - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; - axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); + feedrate_intervals = + feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; + axes[STEPRATE_AXIS].counter = -(feedrate_intervals / 2); #else - axes[STEPRATE_AXIS].definePosition(dda_interval); - axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); + axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); + axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); + axes[STEPRATE_AXIS].counter = -(max_delta / 2); #endif // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { scale_shift++; } if (scale_shift > 0) { @@ -264,6 +269,8 @@ bool getNextMove() { axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } else { + feedrate_scale_shift = 0; } // We use += here so that the odd rounded-off time from the last move is still waited out @@ -272,10 +279,14 @@ bool getNextMove() { intervals = max_delta; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < ALL_AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; + + current_block->busy = false; + planner::doneWithNextBlock(); + return true; } @@ -314,14 +325,14 @@ bool doInterrupt() { timer_counter -= INTERVAL_IN_MICROSECONDS; if (timer_counter <= 0) { if (intervals_remaining-- == 0) { - // getNextMove(); - is_running = false; + getNextMove(); + // is_running = false; } else { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } - #if 0 + #if 1 if (feedrate_intervals_remaining-- == 0) { axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; @@ -329,12 +340,13 @@ bool doInterrupt() { axes[STEPRATE_AXIS].setTarget(feedrate_elements[current_feedrate_index].target, /*relative =*/ false); - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; - axes[STEPRATE_AXIS].counter = -(feedrate_elements[current_feedrate_index].intervals / 2); + feedrate_intervals = + feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; + axes[STEPRATE_AXIS].counter = -(feedrate_intervals / 2); // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals_remaining) { + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { scale_shift++; } if (scale_shift > 0) { @@ -342,10 +354,12 @@ bool doInterrupt() { axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } else { + feedrate_scale_shift = 0; } } - axes[STEPRATE_AXIS].doInterrupt(feedrate_elements[current_feedrate_index].intervals); + axes[STEPRATE_AXIS].doInterrupt(feedrate_intervals); #else axes[STEPRATE_AXIS].doInterrupt(intervals); #endif @@ -359,6 +373,9 @@ bool doInterrupt() { bool still_homing = axes[i].doHoming(intervals); is_homing = still_homing || is_homing; } + // if we're done, force a sync with the planner + if (!is_homing) + planner::abort(); return is_homing; } return false; diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index eebefb8..bf7a7ff 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -164,7 +164,7 @@ // The number of movements we can plan ahead at a time // THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... -#define BLOCK_BUFFER_SIZE 8 +#define BLOCK_BUFFER_SIZE 16 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ From 1ab512d3902373b06b05d7a63835691948663a1f Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 14 Jan 2012 14:04:19 -0600 Subject: [PATCH 42/61] Working. The minimum speed seeps to high. Now automatically will do multiple steps per tick, if needed. --- firmware/src/Motherboard/Planner.cc | 28 +++++++++++++++------------- firmware/src/Motherboard/Planner.hh | 2 +- firmware/src/Motherboard/Steppers.cc | 2 +- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index ba4ee42..06cda94 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -116,16 +116,17 @@ namespace planner { { public: typedef T BufDataType; + typedef uint8_t BufSizeType; private: - volatile uint16_t head, tail; - uint16_t size; - uint16_t size_mask; + volatile BufSizeType head, tail; + BufSizeType size; + BufSizeType size_mask; BufDataType* const data; /// Pointer to buffer data public: - ReusingCircularBufferTempl(int16_t size_in, BufDataType* buffer_in) : head(0), tail(0), size(size_in), size_mask(size_in-1), data(buffer_in) { - for (int16_t i = 0; i < size; i++) { + ReusingCircularBufferTempl(BufSizeType size_in, BufDataType* buffer_in) : head(0), tail(0), size(size_in), size_mask(size_in-1), data(buffer_in) { + for (BufSizeType i = 0; i < size; i++) { data[i] = BufDataType(); } }; @@ -133,22 +134,22 @@ namespace planner { inline BufDataType *getHead() { return &data[head]; } - inline uint16_t getHeadIndex() { + inline BufSizeType getHeadIndex() { return head; } inline BufDataType *getTail() { return &data[tail]; } - inline uint16_t getTailIndex() { + inline BufSizeType getTailIndex() { return tail; } - inline int16_t getNextIndex(uint16_t from) { + inline BufSizeType getNextIndex(BufSizeType from) { return ((from + 1) & size_mask); } - inline int16_t getPreviousIndex(uint16_t from) { + inline BufSizeType getPreviousIndex(BufSizeType from) { return (((from+size) - 1) & size_mask); } @@ -156,10 +157,10 @@ namespace planner { return &data[getNextIndex(head)]; } - inline BufDataType &operator[] (int16_t index) { + inline BufDataType &operator[] (BufSizeType index) { // adding size should make negative indexes < size work ok - int16_t offset = (index + head + size) & size_mask; - return data[offset]; + // int16_t offset = index < 0 ? index : ((index + size) & size_mask); + return data[index]; } // bump the head with buffer++. cannot return anything useful, so it doesn't @@ -182,7 +183,7 @@ namespace planner { return getNextIndex(head) == tail; } - inline int16_t getUsedCount() { + inline BufSizeType getUsedCount() { return ((head-tail+size) & size_mask); } @@ -414,6 +415,7 @@ namespace planner { block[0] = &block_buffer[block_index]; planner_reverse_pass_kernel(block[0], block[1], block[2]); } + planner_reverse_pass_kernel(NULL, block[0], block[1]); } } diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 6bb724d..069fde6 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -34,7 +34,7 @@ // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed // if unwanted behavior is observed on a user's machine when running at very slow speeds. -#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec) +#define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) namespace planner { // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index ae446dc..a06454f 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -323,7 +323,7 @@ void startRunning() { bool doInterrupt() { if (is_running) { timer_counter -= INTERVAL_IN_MICROSECONDS; - if (timer_counter <= 0) { + while (timer_counter <= 0) { if (intervals_remaining-- == 0) { getNextMove(); // is_running = false; From 1b9661aad9e45119e0c02baed4b695918dee1f70 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Fri, 20 Jan 2012 23:37:23 -0600 Subject: [PATCH 43/61] Fixing start/stop speed stupidity. Working on acceleration curve. So, I got the curve wrong because I up the acceleration every _step_ instead of every _tick_. --- firmware/src/Motherboard/Planner.cc | 117 ++++++++++++------ firmware/src/Motherboard/Planner.hh | 28 ++--- firmware/src/Motherboard/Steppers.cc | 107 +++++++--------- .../boards/rrmbv12/Configuration.hh | 17 ++- 4 files changed, 144 insertions(+), 125 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 06cda94..19d994e 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -51,6 +51,20 @@ di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) + + x + vt + (1/2)at^2=X + x = destination + X = position + v = current speed + a = acceleration rate + t = time + See: http://www.wolframalpha.com/input/?i=x+%2B+vt+%2B+%281%2F2%29at%5E2%3DX+for+t + + Solved for time, simplified (with a few multiplications as possible) gives: + (sqrt(v^2-2*a*(x-X)) - v)/a + So, making x-X = D gives: + (sqrt(v*v-2*a*D)-v)/a = time to accelerate from velocity v over D steps with acceleration a + */ @@ -232,11 +246,16 @@ namespace planner { position = Point(0,0,0,0,0); previous_nominal_speed = 0.0; - axes[0].max_acceleration = 9000*axes[0].steps_per_mm; - axes[1].max_acceleration = 9000*axes[1].steps_per_mm; - axes[2].max_acceleration = 50*axes[2].steps_per_mm; - axes[3].max_acceleration = 9000*axes[3].steps_per_mm; - axes[4].max_acceleration = 9000*axes[4].steps_per_mm; + // axes[0].max_acceleration = 9000*axes[0].steps_per_mm; + // axes[1].max_acceleration = 9000*axes[1].steps_per_mm; + // axes[2].max_acceleration = 200*axes[2].steps_per_mm; + // axes[3].max_acceleration = 20000*axes[3].steps_per_mm; + // axes[4].max_acceleration = 20000*axes[4].steps_per_mm; + axes[0].max_acceleration = 1000*axes[0].steps_per_mm; + axes[1].max_acceleration = 1000*axes[1].steps_per_mm; + axes[2].max_acceleration = 200*axes[2].steps_per_mm; + axes[3].max_acceleration = 20000*axes[3].steps_per_mm; + axes[4].max_acceleration = 20000*axes[4].steps_per_mm; } @@ -291,6 +310,20 @@ namespace planner { return 0.0; // acceleration was 0, set intersection distance to 0 } } + + // Calculates the time (not distance) in microseconds (S*1,000,000) it takes to go from initial_rate for distance at acceleration rate + FORCE_INLINE uint32_t estimate_time_to_accelerate(float initial_rate, float acceleration, float distance) { + float abs_acceleration = abs(acceleration); + if (abs_acceleration!=0.0 && initial_rate == 0.0) { + return (sqrt(-2*abs_acceleration*distance)/acceleration) * 1000000; + } else if (abs_acceleration!=0.0) { + // (sqrt(v*v-2*a*D)-v)/a + return ((sqrt(initial_rate*initial_rate-2*abs_acceleration*distance)-initial_rate)/abs_acceleration) * 1000000; + } + else { + return (distance/initial_rate) * 1000000; // no acceleration is just distance/rate + } + } // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); @@ -327,21 +360,14 @@ namespace planner { plateau_steps = 0; } - #ifdef ADVANCE - long initial_advance = advance*entry_factor*entry_factor; - long final_advance = advance*exit_factor*exit_factor; - #endif // ADVANCE - ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section if(busy == false) { // Don't update variables if block is busy. accelerate_until = accelerate_steps; + time_to_accelerate = estimate_time_to_accelerate(initial_rate, acceleration, accelerate_steps); decelerate_after = accelerate_steps+plateau_steps; + time_to_decelerate = estimate_time_to_accelerate(nominal_rate, -acceleration, decelerate_steps); // initial_rate = initial_rate; // final_rate = final_rate; - #ifdef ADVANCE - initial_advance = initial_advance; - final_advance = final_advance; - #endif //ADVANCE } // So, ummm, what if it IS busy?! } // ISR state will be automatically restored here } @@ -480,9 +506,9 @@ namespace planner { } block_index = block_buffer.getNextIndex( block_index ); } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + // Last/newest block in buffer. Exit speed is set with next->stop_speed. Always recalculated. if(next != NULL) { - next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + next->calculate_trapezoid(next->stop_speed); next->recalculate_flag = false; } } @@ -519,31 +545,23 @@ namespace planner { block->target = target; - block->nominal_rate = 1000000/us_per_step; // (step/sec) Always > 0 - // // store the absolute number of steps in each direction, without direction Point steps = (target - position).abs(); + float delta_mm[AXIS_COUNT]; + block->millimeters = 0.0; block->step_event_count = 0; + // // Compute direction bits for this block -- UNUSED FOR NOW + // block->direction_bits = 0; for (int i = 0; i < AXIS_COUNT; i++) { if (steps[i] > block->step_event_count) { block->step_event_count = steps[i]; } - } - -#if 1 - // // Compute direction bits for this block -- UNUSED FOR NOW - // block->direction_bits = 0; - // for (int i = 0; i < AXIS_COUNT; i++) { - // if (target[i] < position[i]) { block->direction_bits |= (1<millimeters = 0.0; - for (int i = 0; i < AXIS_COUNT; i++) { delta_mm[i] = ((float)steps[i])/axes[i].steps_per_mm; - block->millimeters += delta_mm[i] * delta_mm[i]; - } + if (i < A_AXIS) + block->millimeters += delta_mm[i] * delta_mm[i]; + // if (target[i] < position[i]) { block->direction_bits |= (1<millimeters = sqrt(block->millimeters); float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides @@ -607,7 +625,7 @@ namespace planner { block->acceleration_st = axes[i].max_acceleration; } block->acceleration = block->acceleration_st / steps_per_mm; - block->acceleration_rate = (int32_t)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? + // block->acceleration_rate = (int32_t)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? // Compute the speed trasitions, or "jerks" // Start with a safe speed @@ -619,8 +637,29 @@ namespace planner { } vmax_junction = min(vmax_junction, block->nominal_speed); + + // Determine the stopping speed for this move, in case it's the last one + // this is basically the same as the entry speed calcs (done next) except this move is + // the "previous" one and the "current" speed would be zero. + float jerk = sqrt(pow((current_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]), 2)); + float stop_vmax_junction = vmax_junction; // we re-use vmax_junction below, so keep it + if((current_speed[X_AXIS] != 0.0) || (current_speed[Y_AXIS] != 0.0)) { + stop_vmax_junction = block->nominal_speed; + } + if (jerk > max_xy_jerk) { + stop_vmax_junction *= (max_xy_jerk/jerk); + } + for(int i=Z_AXIS; i < AXIS_COUNT; i++) { + float axis_jerk = fabs(current_speed[i]); + if(axis_jerk > axes[i].max_axis_jerk) { + stop_vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); + } + } + block->stop_speed = stop_vmax_junction; + + // Now determine the safe max entry speed for this move if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { - float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } @@ -628,18 +667,17 @@ namespace planner { vmax_junction *= (max_xy_jerk/jerk); } - // account for Z, A, and B for(int i=Z_AXIS; i < AXIS_COUNT; i++) { float axis_jerk = fabs(current_speed[i] - previous_speed[i]); if(axis_jerk > axes[i].max_axis_jerk) { vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); - } + } } } block->max_entry_speed = vmax_junction; - // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - float v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + // Initialize block entry speed. Compute based on deceleration to junction jerk of appropriate axes + float v_allowable = max_allowable_speed(-block->acceleration,block->stop_speed,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -666,8 +704,7 @@ namespace planner { previous_nominal_speed = block->nominal_speed; - block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); -#endif + block->calculate_trapezoid(block->stop_speed); // Update position position = target; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 069fde6..5ebdccf 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -31,31 +31,22 @@ #include "CircularBuffer.hh" #include "Point.hh" -// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end -// of the buffer and all stops. This should not be much greater than zero and should only be changed -// if unwanted behavior is observed on a user's machine when running at very slow speeds. -#define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) - namespace planner { // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // the source g-code and may never actually be reached if acceleration management is active. class Block { public: // Fields used by the bresenham algorithm for tracing the line - // Point steps; // Step count and direction (may be negative) along each axis - Point target; // Step count and direction (may be negative) along each axis + Point target; // Final 5-axis target uint32_t step_event_count; // The number of step events required to complete this block - int32_t accelerate_until; // The index of the step event on which to stop acceleration - int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t acceleration_rate; // The acceleration rate used for acceleration calculation - // uint8_t direction_bits; // The direction bit set for this block - // uint8_t active_extruder; // Selects the active extruder - #ifdef ADVANCE - int32_t advance_rate; - volatile int32_t initial_advance; - volatile int32_t final_advance; - float advance; - #endif + int32_t accelerate_until; // The index of the step event on which to stop acceleration + int32_t time_to_accelerate; // number of microseconds (S*1,000,000) to accelerate + int32_t decelerate_after; // The index of the step event on which to start decelerating + int32_t time_to_decelerate; // number of microseconds (S*1,000,000) to decelerate + // int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + // uint8_t direction_bits; // The direction bit set for this block + // uint8_t active_extruder; // Selects the active extruder + // Fields used by the motion planner to manage acceleration // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis @@ -64,6 +55,7 @@ namespace planner { float max_entry_speed; // Maximum allowable junction entry speed in mm/min float millimeters; // The total travel of this block in mm float acceleration; // acceleration mm/sec^2 + float stop_speed; // Speed to decelerate to if this is the last move uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction uint8_t nominal_length_flag; // Planner flag for nominal speed always reached diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index a06454f..b64d91e 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -29,11 +29,13 @@ int32_t intervals; volatile int32_t intervals_remaining; struct feedrate_element { - int32_t intervals; + int32_t intervals; // interval value of the feedrate axis + int32_t steps; // number of steps of the master axis to change int32_t target; }; feedrate_element feedrate_elements[3]; volatile int32_t feedrate_intervals; +volatile int32_t feedrate_steps_remaining; volatile int32_t feedrate_intervals_remaining; volatile uint8_t current_feedrate_index; @@ -89,6 +91,7 @@ void abort() { timer_counter = 0; feedrate_scale_shift = 0; feedrate_intervals = 0; + feedrate_steps_remaining = 0; feedrate_intervals_remaining = 0; } @@ -191,6 +194,27 @@ is_running = true; } */ +void prepareFeedrateIntervals() { + feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; + feedrate_intervals = + feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; + axes[STEPRATE_AXIS].counter = -(feedrate_intervals>>1); + + // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled + int8_t scale_shift = 0; + while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { + scale_shift++; + } + if (scale_shift > 0) { + feedrate_scale_shift = scale_shift; + axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; + axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; + axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; + } else { + feedrate_scale_shift = 0; + } +} + // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { @@ -220,65 +244,46 @@ bool getNextMove() { } current_feedrate_index = 0; -#if 1 + // setup acceleration - feedrate_elements[0].intervals = current_block->accelerate_until; + feedrate_elements[0].steps = current_block->accelerate_until; + feedrate_elements[0].intervals = (current_block->time_to_accelerate)/INTERVAL_IN_MICROSECONDS; feedrate_elements[0].target = 1000000/current_block->nominal_rate; // setup plateau - feedrate_elements[1].intervals = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[1].steps = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[1].intervals = 1; feedrate_elements[1].target = 1000000/current_block->nominal_rate; // setup deceleration - feedrate_elements[2].intervals = current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[2].steps = current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[2].intervals = (current_block->time_to_decelerate)/INTERVAL_IN_MICROSECONDS; feedrate_elements[2].target = 1000000/current_block->final_rate; - if (feedrate_elements[0].intervals > 0) { + if (feedrate_elements[0].steps > 0) { // setup the acceleration speed axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - } else if (feedrate_elements[1].intervals > 0) { + } else if (feedrate_elements[1].steps > 0) { // setup the plateau speed axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); current_feedrate_index = 1; - } else if (feedrate_elements[2].intervals > 0) { + } else if (feedrate_elements[2].steps > 0) { // setup the deceleration speed axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); current_feedrate_index = 2; } - feedrate_intervals = - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; - axes[STEPRATE_AXIS].counter = -(feedrate_intervals / 2); -#else - axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - axes[STEPRATE_AXIS].counter = -(max_delta / 2); -#endif - - - // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled - int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { - scale_shift++; - } - if (scale_shift > 0) { - feedrate_scale_shift = scale_shift; - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; - axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; - axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; - } else { - feedrate_scale_shift = 0; - } + prepareFeedrateIntervals(); // We use += here so that the odd rounded-off time from the last move is still waited out timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; intervals = max_delta; intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; + const int32_t negative_half_interval = -(intervals>>1); for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } @@ -294,7 +299,7 @@ bool getNextMove() { void startHoming(const bool maximums, const uint8_t axes_enabled, const uint32_t us_per_step) { intervals_remaining = INT32_MAX; intervals = us_per_step / INTERVAL_IN_MICROSECONDS; - const int32_t negative_half_interval = -intervals / 2; + const int32_t negative_half_interval = -(intervals>>1); for (int i = 0; i < AXIS_COUNT; i++) { axes[i].counter = negative_half_interval; if ((axes_enabled & (1< 0) { + axes[STEPRATE_AXIS].doInterrupt(feedrate_intervals); + feedrate_intervals_remaining--; + } while (timer_counter <= 0) { if (intervals_remaining-- == 0) { getNextMove(); @@ -332,37 +341,13 @@ bool doInterrupt() { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } - #if 1 - if (feedrate_intervals_remaining-- == 0) { - axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; - + if (feedrate_steps_remaining-- == 0) { + axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; current_feedrate_index++; - axes[STEPRATE_AXIS].setTarget(feedrate_elements[current_feedrate_index].target, /*relative =*/ false); - - feedrate_intervals = - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; - axes[STEPRATE_AXIS].counter = -(feedrate_intervals / 2); - - // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled - int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { - scale_shift++; - } - if (scale_shift > 0) { - feedrate_scale_shift = scale_shift; - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; - axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; - axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; - } else { - feedrate_scale_shift = 0; - } + prepareFeedrateIntervals(); } - axes[STEPRATE_AXIS].doInterrupt(feedrate_intervals); - #else - axes[STEPRATE_AXIS].doInterrupt(intervals); - #endif timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; } } @@ -381,4 +366,4 @@ bool doInterrupt() { return false; } -} +} // namespace steppers diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index bf7a7ff..fbc6b1c 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -167,10 +167,15 @@ #define BLOCK_BUFFER_SIZE 16 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 20.0 -#define DEFAULT_MAX_Z_JERK 0.4 -#define DEFAULT_MAX_A_JERK 0.4 -#define DEFAULT_MAX_B_JERK 0.4 - +#define DEFAULT_ACCELERATION 1000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 10.0 +#define DEFAULT_MAX_Z_JERK 10.0 +#define DEFAULT_MAX_A_JERK 10.0 +#define DEFAULT_MAX_B_JERK 10.0 + +// // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// // of the buffer and all stops. This should not be much greater than zero and should only be changed +// // if unwanted behavior is observed on a user's machine when running at very slow speeds. +// #define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) +// #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ From 753f00943df49e43ce24aeac9da8e0e6ae2f80d4 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 4 Feb 2012 13:30:20 -0600 Subject: [PATCH 44/61] Fixed acceleration curve. Broke junction-jerk calks. I've added the centripetal calculations from Grbl, but they don't seem to work right, so I'm going to make them a compile-time option and switch back. --- firmware/src/Motherboard/Planner.cc | 129 ++++++++----- firmware/src/Motherboard/Planner.hh | 5 +- firmware/src/Motherboard/Steppers.cc | 178 ++++++++++-------- .../Motherboard/boards/mb24/Configuration.hh | 13 +- .../boards/rrmbv12/Configuration.hh | 29 +-- .../Motherboard/boards/rrmbv12/Motherboard.cc | 1 + firmware/src/shared/StepperAxis.cc | 40 ++-- 7 files changed, 227 insertions(+), 168 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 19d994e..392c131 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -52,13 +52,14 @@ IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) + vt + (1/2)at^2=X for t x + vt + (1/2)at^2=X x = destination X = position v = current speed a = acceleration rate t = time - See: http://www.wolframalpha.com/input/?i=x+%2B+vt+%2B+%281%2F2%29at%5E2%3DX+for+t + See: http://www.wolframalpha.com/input/?i=vt+%2B+%281%2F2%29at%5E2%3DX+for+t Solved for time, simplified (with a few multiplications as possible) gives: (sqrt(v^2-2*a*(x-X)) - v)/a @@ -227,8 +228,10 @@ namespace planner { PlannerAxis axes[AXIS_COUNT]; float default_acceleration; + float default_junction_deviation; Point position; // the current position (planning-wise, not bot/stepper-wise) in steps float previous_speed[AXIS_COUNT]; // Speed of previous path line segment + double previous_unit_vec[3]; float previous_nominal_speed; // Nominal speed of previous path line segment static float max_xy_jerk; @@ -246,16 +249,16 @@ namespace planner { position = Point(0,0,0,0,0); previous_nominal_speed = 0.0; - // axes[0].max_acceleration = 9000*axes[0].steps_per_mm; - // axes[1].max_acceleration = 9000*axes[1].steps_per_mm; - // axes[2].max_acceleration = 200*axes[2].steps_per_mm; - // axes[3].max_acceleration = 20000*axes[3].steps_per_mm; - // axes[4].max_acceleration = 20000*axes[4].steps_per_mm; - axes[0].max_acceleration = 1000*axes[0].steps_per_mm; - axes[1].max_acceleration = 1000*axes[1].steps_per_mm; + axes[0].max_acceleration = 900*axes[0].steps_per_mm; + axes[1].max_acceleration = 900*axes[1].steps_per_mm; axes[2].max_acceleration = 200*axes[2].steps_per_mm; axes[3].max_acceleration = 20000*axes[3].steps_per_mm; axes[4].max_acceleration = 20000*axes[4].steps_per_mm; + // axes[0].max_acceleration = 500*axes[0].steps_per_mm; + // axes[1].max_acceleration = 500*axes[1].steps_per_mm; + // axes[2].max_acceleration = 200*axes[2].steps_per_mm; + // axes[3].max_acceleration = 20000*axes[3].steps_per_mm; + // axes[4].max_acceleration = 20000*axes[4].steps_per_mm; } @@ -277,6 +280,10 @@ namespace planner { // axes[i].max_acceleration = default_acceleration * axes[i].steps_per_mm; // } } + + void setJunctionDeviation(float new_junction_deviation) { + default_junction_deviation = new_junction_deviation; + } // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. @@ -310,20 +317,22 @@ namespace planner { return 0.0; // acceleration was 0, set intersection distance to 0 } } - + +#if 0 // Calculates the time (not distance) in microseconds (S*1,000,000) it takes to go from initial_rate for distance at acceleration rate FORCE_INLINE uint32_t estimate_time_to_accelerate(float initial_rate, float acceleration, float distance) { - float abs_acceleration = abs(acceleration); - if (abs_acceleration!=0.0 && initial_rate == 0.0) { - return (sqrt(-2*abs_acceleration*distance)/acceleration) * 1000000; - } else if (abs_acceleration!=0.0) { - // (sqrt(v*v-2*a*D)-v)/a - return ((sqrt(initial_rate*initial_rate-2*abs_acceleration*distance)-initial_rate)/abs_acceleration) * 1000000; + /* + if (acceleration!=0.0 && initial_rate == 0.0) { + return (sqrt(-2*acceleration*distance)/acceleration) * 1000000; + } else */ + if (acceleration!=0.0) { + return abs((sqrt(2*acceleration*distance + initial_rate*initial_rate)-initial_rate)/acceleration) * 1000000; } else { return (distance/initial_rate) * 1000000; // no acceleration is just distance/rate } } +#endif // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); @@ -363,9 +372,7 @@ namespace planner { ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section if(busy == false) { // Don't update variables if block is busy. accelerate_until = accelerate_steps; - time_to_accelerate = estimate_time_to_accelerate(initial_rate, acceleration, accelerate_steps); decelerate_after = accelerate_steps+plateau_steps; - time_to_decelerate = estimate_time_to_accelerate(nominal_rate, -acceleration, decelerate_steps); // initial_rate = initial_rate; // final_rate = final_rate; } // So, ummm, what if it IS busy?! @@ -506,9 +513,9 @@ namespace planner { } block_index = block_buffer.getNextIndex( block_index ); } - // Last/newest block in buffer. Exit speed is set with next->stop_speed. Always recalculated. + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { - next->calculate_trapezoid(next->stop_speed); + next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); next->recalculate_flag = false; } } @@ -532,6 +539,7 @@ namespace planner { block_buffer.bumpTail(); } + // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) @@ -653,31 +661,57 @@ namespace planner { float axis_jerk = fabs(current_speed[i]); if(axis_jerk > axes[i].max_axis_jerk) { stop_vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); + block->acceleration_rate = block->acceleration_st / ACCELERATION_TICKS_PER_SECOND; + + // Compute path unit vector + double unit_vec[3]; + + unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; + unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; + unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; + + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Let a circle be tangent to both previous and current path line segments, where the junction + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous grbl version. This approach does not actually deviate + // from path, but used as a robust way to compute cornering speeds, as it takes into account the + // nonlinearities of both the junction angle and junction velocity. + double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + + // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. + if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) + // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. + double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; + + // Skip and use default max junction speed for 0 degree acute junction. + if (cos_theta < 0.95) { + vmax_junction = min(previous_nominal_speed,block->nominal_speed); + // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. + if (cos_theta > -0.95) { + // Compute maximum junction velocity based on maximum acceleration and junction deviation + double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + vmax_junction = min(vmax_junction, + sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + } } } - block->stop_speed = stop_vmax_junction; - // Now determine the safe max entry speed for this move - if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { - jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); - if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { - vmax_junction = block->nominal_speed; - } - if (jerk > max_xy_jerk) { - vmax_junction *= (max_xy_jerk/jerk); - } - - for(int i=Z_AXIS; i < AXIS_COUNT; i++) { - float axis_jerk = fabs(current_speed[i] - previous_speed[i]); - if(axis_jerk > axes[i].max_axis_jerk) { - vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); - } + for (int i_axis = A_AXIS; i_axis < B_AXIS; i_axis++) { + float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); + if (jerk > axes[i_axis].max_axis_jerk) { + vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); } } block->max_entry_speed = vmax_junction; - - // Initialize block entry speed. Compute based on deceleration to junction jerk of appropriate axes - float v_allowable = max_allowable_speed(-block->acceleration,block->stop_speed,block->millimeters); + + // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. + double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -695,16 +729,13 @@ namespace planner { block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed - // memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] - previous_speed[0] = current_speed[0]; - previous_speed[1] = current_speed[1]; - previous_speed[2] = current_speed[2]; - previous_speed[3] = current_speed[3]; - previous_speed[4] = current_speed[4]; + memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] + // Update previous path unit_vector and nominal speed + memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; - block->calculate_trapezoid(block->stop_speed); + block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); // Update position position = target; @@ -735,6 +766,10 @@ namespace planner { } block_buffer.clear(); + + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; } void definePosition(const Point& new_position) @@ -746,6 +781,10 @@ namespace planner { for (int i = 0; i < AXIS_COUNT; i++) { previous_speed[i] = 0.0; } + + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; } const Point getPosition() diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 5ebdccf..ecb5dc3 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -40,10 +40,8 @@ namespace planner { Point target; // Final 5-axis target uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration - int32_t time_to_accelerate; // number of microseconds (S*1,000,000) to accelerate int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t time_to_decelerate; // number of microseconds (S*1,000,000) to decelerate - // int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + int32_t acceleration_rate; // The acceleration rate used for acceleration calculation // uint8_t direction_bits; // The direction bit set for this block // uint8_t active_extruder; // Selects the active extruder @@ -106,6 +104,7 @@ namespace planner { void setMaxAxisJerk(float jerk, uint8_t axis); void setAcceleration(float acceleration); + void setJunctionDeviation(float new_junction_deviation); void setAxisStepsPerMM(float steps_per_mm, uint8_t axis); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index b64d91e..bda58ce 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -29,36 +29,31 @@ int32_t intervals; volatile int32_t intervals_remaining; struct feedrate_element { - int32_t intervals; // interval value of the feedrate axis + int32_t rate; // interval value of the feedrate axis int32_t steps; // number of steps of the master axis to change int32_t target; }; feedrate_element feedrate_elements[3]; -volatile int32_t feedrate_intervals; volatile int32_t feedrate_steps_remaining; -volatile int32_t feedrate_intervals_remaining; +volatile int32_t feedrate; +volatile int32_t feedrate_target; // convenient storage to save lookup time +volatile int8_t feedrate_dirty; // indicates if the feedrate_inverted needs recalculated +volatile int32_t feedrate_inverted; +volatile int32_t feedrate_changerate; +volatile int32_t acceleration_tick_counter; +volatile int32_t feedrate_start_halving; volatile uint8_t current_feedrate_index; volatile int32_t timer_counter; -volatile int8_t feedrate_scale_shift; -// The ALL_AXIS_COUNT includes the virtual "speed" axis -#define ALL_AXIS_COUNT STEPPER_COUNT+1 - -// STEPRATE_AXIS is the number of the virtual axis that control the steprate -#define STEPRATE_AXIS STEPPER_COUNT -StepperAxis axes[ALL_AXIS_COUNT]; +StepperAxis axes[STEPPER_COUNT]; volatile bool is_homing; +Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; + bool holdZ = false; planner::Block *current_block; -enum { - ACCELERATING = 1, - IN_PLATEAU, - DECELERATING -} current_phase; // pretend we are just leaving a block - bool isRunning() { return is_running || is_homing; } @@ -69,30 +64,34 @@ void init(Motherboard& motherboard) { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i] = StepperAxis(motherboard.getStepperInterface(i)); } - - axes[STEPRATE_AXIS] = StepperAxis(); timer_counter = 0; - feedrate_scale_shift = 0; - current_phase = DECELERATING; + current_block = 0; for (int i = 0; i < 3; i++) { feedrate_elements[i] = feedrate_element(); - feedrate_elements[i].intervals = 0; + feedrate_elements[i].rate = 0; feedrate_elements[i].target = 0; + feedrate_elements[i].steps = 0; } - feedrate_intervals_remaining = 0; + // feedrate_intervals_remaining = 0; + feedrate = 0; + stepperTimingDebugPin.setDirection(true); + stepperTimingDebugPin.setValue(false); } void abort() { is_running = false; is_homing = false; timer_counter = 0; - feedrate_scale_shift = 0; - feedrate_intervals = 0; + // feedrate_scale_shift = 0; + // feedrate_intervals = 0; + // feedrate_intervals_remaining = 0; feedrate_steps_remaining = 0; - feedrate_intervals_remaining = 0; + feedrate = 0; + feedrate_inverted = 0; + feedrate_dirty = 1; } /// Define current position as given point @@ -115,7 +114,7 @@ void setHoldZ(bool holdZ_in) { holdZ = holdZ_in; } - +#if 0 void setTarget(const Point& target, int32_t dda_interval) { int32_t max_delta = 0; for (int i = 0; i < AXIS_COUNT; i++) { @@ -193,26 +192,14 @@ axes[i].counter = negative_half_interval; is_running = true; } */ +#endif -void prepareFeedrateIntervals() { - feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; - feedrate_intervals = - feedrate_intervals_remaining = feedrate_elements[current_feedrate_index].intervals; - axes[STEPRATE_AXIS].counter = -(feedrate_intervals>>1); - - // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled - int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > feedrate_intervals) { - scale_shift++; - } - if (scale_shift > 0) { - feedrate_scale_shift = scale_shift; - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; - axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; - axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; - } else { - feedrate_scale_shift = 0; - } +inline void prepareFeedrateIntervals() { + feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; + feedrate_changerate = feedrate_elements[current_feedrate_index].rate; + feedrate_target = feedrate_elements[current_feedrate_index].target; + feedrate_dirty = 1; + // acceleration_tick_counter = 0; } // load up the next movment @@ -244,42 +231,46 @@ bool getNextMove() { } current_feedrate_index = 0; - + int feedrate_being_setup = 0; // setup acceleration - feedrate_elements[0].steps = current_block->accelerate_until; - feedrate_elements[0].intervals = (current_block->time_to_accelerate)/INTERVAL_IN_MICROSECONDS; - feedrate_elements[0].target = 1000000/current_block->nominal_rate; + feedrate = 0; + if (current_block->accelerate_until > 0) { + feedrate = current_block->initial_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } // setup plateau - feedrate_elements[1].steps = current_block->decelerate_after - current_block->accelerate_until; - feedrate_elements[1].intervals = 1; - feedrate_elements[1].target = 1000000/current_block->nominal_rate; + if (current_block->decelerate_after > current_block->accelerate_until) { + if (feedrate == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = 0; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } // setup deceleration - feedrate_elements[2].steps = current_block->step_event_count - current_block->decelerate_after; - feedrate_elements[2].intervals = (current_block->time_to_decelerate)/INTERVAL_IN_MICROSECONDS; - feedrate_elements[2].target = 1000000/current_block->final_rate; - - if (feedrate_elements[0].steps > 0) { - // setup the acceleration speed - axes[STEPRATE_AXIS].definePosition(1000000/current_block->initial_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - } else if (feedrate_elements[1].steps > 0) { - // setup the plateau speed - axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->nominal_rate, /*relative =*/ false); - current_feedrate_index = 1; - } else if (feedrate_elements[2].steps > 0) { - // setup the deceleration speed - axes[STEPRATE_AXIS].definePosition(1000000/current_block->nominal_rate); - axes[STEPRATE_AXIS].setTarget(1000000/current_block->final_rate, /*relative =*/ false); - current_feedrate_index = 2; + if (current_block->decelerate_after < current_block->step_event_count) { + if (feedrate == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->final_rate; } prepareFeedrateIntervals(); + feedrate_inverted = 1000000/feedrate; + feedrate_dirty = 0; + acceleration_tick_counter = TICKS_PER_ACCELERATION; // We use += here so that the odd rounded-off time from the last move is still waited out - timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; + timer_counter += feedrate; intervals = max_delta; intervals_remaining = intervals; @@ -325,32 +316,59 @@ void startRunning() { getNextMove(); } + bool doInterrupt() { if (is_running) { + stepperTimingDebugPin.setValue(true); timer_counter -= INTERVAL_IN_MICROSECONDS; - if (feedrate_intervals_remaining > 0) { - axes[STEPRATE_AXIS].doInterrupt(feedrate_intervals); - feedrate_intervals_remaining--; - } + while (timer_counter <= 0) { if (intervals_remaining-- == 0) { getNextMove(); + stepperTimingDebugPin.setValue(false); + return is_running; // is_running = false; } else { for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].doInterrupt(intervals); } - if (feedrate_steps_remaining-- == 0) { - axes[STEPRATE_AXIS].position = feedrate_elements[current_feedrate_index].target; + + if (feedrate_dirty) { + feedrate_inverted = 1000000/feedrate; + feedrate_dirty = 0; + } + + timer_counter += feedrate_inverted; + + if (feedrate_steps_remaining-- <= 0) { current_feedrate_index++; - axes[STEPRATE_AXIS].setTarget(feedrate_elements[current_feedrate_index].target, /*relative =*/ false); prepareFeedrateIntervals(); + break; } - - timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; } } + + if (feedrate_changerate != 0 && acceleration_tick_counter-- == 0) { + acceleration_tick_counter = TICKS_PER_ACCELERATION; + // Change our feedrate. Here it's important to note that we can over/undershoot + // To handle this, if we're accelerating, we simply clamp to max speed. + // If we are decelerating, we have to be more careful. + // But for now, we don't allow decelrating to a "stop" so we punt and just don't + // allow it to go under the set speed. + + feedrate += feedrate_changerate; + feedrate_dirty = 1; + + if ((feedrate_changerate > 0 && feedrate > feedrate_target) + || (feedrate_changerate < 0 && feedrate < feedrate_target)) { + feedrate_changerate = 0; + feedrate = feedrate_target; + } + + } + + stepperTimingDebugPin.setValue(false); return is_running; } else if (is_homing) { is_homing = false; diff --git a/firmware/src/Motherboard/boards/mb24/Configuration.hh b/firmware/src/Motherboard/boards/mb24/Configuration.hh index 4f3f3e4..ecd7922 100644 --- a/firmware/src/Motherboard/boards/mb24/Configuration.hh +++ b/firmware/src/Motherboard/boards/mb24/Configuration.hh @@ -27,8 +27,9 @@ /// be at least eight times this large. Reducing the interval can cause resource /// starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 128 -// 8MHz / INTERVAL_IN_MICROSECONDS = 125000 -#define INTERVALS_PER_SECOND (1000000 / INTERVAL_IN_MICROSECONDS) + +#define TICKS_PER_ACCELERATION 125 +#define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) // --- Secure Digital Card configuration --- // NOTE: If SD support is enabled, it is implicitly assumed that the @@ -190,4 +191,12 @@ #define INTERFACE_BAR_PIN Pin(PortL,0) #define INTERFACE_DEBUG_PIN Pin(PortB,7) +//#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 20.0 +#define DEFAULT_MAX_Z_JERK 10.0 +#define DEFAULT_MAX_A_JERK 10.0 +#define DEFAULT_MAX_B_JERK 10.0 + + #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index fbc6b1c..9d6f09d 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -26,9 +26,11 @@ // possible time between steps; in practical terms, your time between steps should // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. -#define INTERVAL_IN_MICROSECONDS 64 -// 1000000 / INTERVAL_IN_MICROSECONDS = 125000 -// #define INTERVALS_PER_SECOND (1000000 / INTERVAL_IN_MICROSECONDS) +#define INTERVAL_IN_MICROSECONDS 128 + +// TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded +#define TICKS_PER_ACCELERATION 5 // lower is better +#define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) // The pin that connects to the /PS_ON pin on the PSU header. This pin switches // on the PSU when pulled low. @@ -157,25 +159,28 @@ // The pin which controls the debug LED (active high) #define DEBUG_PIN Pin(PortB,0) +#define STEPPER_TIMER_DEBUG Pin(PortC,0) // SDA + // By default, debugging packets should be honored; this is made // configurable if we're short on cycles or EEPROM. // Define as 1 if debugging packets are honored; 0 if not. #define HONOR_DEBUG_PACKETS 1 // The number of movements we can plan ahead at a time -// THIS MUst BE A POWER OF 2! 4, 8, 16, 32, you get the idea... +// THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 1000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 10.0 -#define DEFAULT_MAX_Z_JERK 10.0 +#define DEFAULT_ACCELERATION 900.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 5.0 +#define DEFAULT_MAX_Z_JERK 5.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 -// // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end -// // of the buffer and all stops. This should not be much greater than zero and should only be changed -// // if unwanted behavior is observed on a user's machine when running at very slow speeds. -// #define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) -// +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) +#define DEFAULT_JUNCTION_DEVIATION 0.00005 // mm + #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index 7db214c..c2587ea 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -85,6 +85,7 @@ Motherboard::Motherboard(const Pin& psu_pin) : /* END FIX THIS FIX THIS FIX THIS FIX THIS */ planner::setAcceleration(DEFAULT_ACCELERATION); + planner::setJunctionDeviation(DEFAULT_JUNCTION_DEVIATION); planner::setMaxXYJerk(DEFAULT_MAX_XY_JERK); planner::setMaxAxisJerk(DEFAULT_MAX_Z_JERK, 2); planner::setMaxAxisJerk(DEFAULT_MAX_A_JERK, 3); diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index 0d4697c..b715884 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -17,19 +17,20 @@ void StepperAxis::setTarget(const int32_t target_in, bool relative) { target = target_in; } direction = true; - if (delta != 0 && interface != 0) { + if (delta != 0) { interface->setEnabled(true); } if (delta < 0) { delta = -delta; direction = false; } + interface->setDirection(direction); } void StepperAxis::setHoming(const bool direction_in) { direction = direction_in; - if (interface != 0) - interface->setEnabled(true); + interface->setDirection(direction); + interface->setEnabled(true); delta = 1; } @@ -38,8 +39,7 @@ void StepperAxis::definePosition(const int32_t position_in) { } void StepperAxis::enableStepper(bool enable) { - if (interface != 0) - interface->setEnabled(enable); + interface->setEnabled(enable); } void StepperAxis::reset() { @@ -57,12 +57,10 @@ void StepperAxis::reset() { bool StepperAxis::checkEndstop(const bool isHoming) { - if (!interface) - return false; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) - bool hit_endstop = direction ? interface->isAtMaximum() : interface->isAtMinimum(); + bool hit_endstop = interface->isAtMinimum(); // We must move at least ENDSTOP_DEBOUNCE from where we hit the endstop before we declare traveling - if (hit_endstop || ((endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE) && endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM))) { + if (hit_endstop || (endstop_status == (direction?ESS_AT_MAXIMUM:ESS_AT_MINIMUM) && (endstop_play < ENDSTOP_DEFAULT_PLAY - ENDSTOP_DEBOUNCE))) { hit_endstop = true; // Did we *just* hit the endstop? if (endstop_status == ESS_TRAVELING || (isHoming && endstop_status == ESS_UNKNOWN)) { @@ -101,26 +99,17 @@ bool StepperAxis::checkEndstop(const bool isHoming) { void StepperAxis::doInterrupt(const int32_t intervals) { counter += delta; - + if (counter >= 0) { counter -= intervals; - if (interface != 0) { - interface->setDirection(direction); - bool hit_endstop = checkEndstop(false); - if (direction) { - if (!hit_endstop) interface->step(true); - position++; - } else { - if (!hit_endstop) interface->step(true); - position--; - } - interface->step(false); + bool hit_endstop = checkEndstop(false); + if (!hit_endstop) interface->step(true); + if (direction) { + position++; } else { - if (direction) - position++; - else - position--; + position--; } + interface->step(false); } } @@ -130,7 +119,6 @@ bool StepperAxis::doHoming(const int32_t intervals) { counter += delta; if (counter >= 0) { counter -= intervals; - interface->setDirection(direction); bool hit_endstop = checkEndstop(true); if (direction) { if (!hit_endstop) { From ba6cc006bf452a3e2ca2c0f56743dbbb587a859a Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 4 Feb 2012 18:24:34 -0600 Subject: [PATCH 45/61] Made the centripetal calculations optional, and not the default. Getting pretty nice prints now. --- firmware/src/Motherboard/Planner.cc | 77 ++++++++++++------- firmware/src/Motherboard/Planner.hh | 3 +- .../boards/rrmbv12/Configuration.hh | 7 +- .../Motherboard/boards/rrmbv12/Motherboard.cc | 3 + 4 files changed, 58 insertions(+), 32 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 392c131..42c7298 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -228,10 +228,12 @@ namespace planner { PlannerAxis axes[AXIS_COUNT]; float default_acceleration; - float default_junction_deviation; Point position; // the current position (planning-wise, not bot/stepper-wise) in steps float previous_speed[AXIS_COUNT]; // Speed of previous path line segment - double previous_unit_vec[3]; +#ifdef CENTREPEDAL + float default_junction_deviation; + float previous_unit_vec[3]; +#endif float previous_nominal_speed; // Nominal speed of previous path line segment static float max_xy_jerk; @@ -252,8 +254,8 @@ namespace planner { axes[0].max_acceleration = 900*axes[0].steps_per_mm; axes[1].max_acceleration = 900*axes[1].steps_per_mm; axes[2].max_acceleration = 200*axes[2].steps_per_mm; - axes[3].max_acceleration = 20000*axes[3].steps_per_mm; - axes[4].max_acceleration = 20000*axes[4].steps_per_mm; + axes[3].max_acceleration = 10000*axes[3].steps_per_mm; + axes[4].max_acceleration = 10000*axes[4].steps_per_mm; // axes[0].max_acceleration = 500*axes[0].steps_per_mm; // axes[1].max_acceleration = 500*axes[1].steps_per_mm; // axes[2].max_acceleration = 200*axes[2].steps_per_mm; @@ -281,9 +283,11 @@ namespace planner { // } } +#ifdef CENTREPEDAL void setJunctionDeviation(float new_junction_deviation) { default_junction_deviation = new_junction_deviation; } +#endif // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. @@ -462,7 +466,7 @@ namespace planner { // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. if (!previous->nominal_length_flag) { if (previous->entry_speed < current->entry_speed) { - double entry_speed = min( current->entry_speed, + float entry_speed = min( current->entry_speed, max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); // Check for junction speed change @@ -633,8 +637,9 @@ namespace planner { block->acceleration_st = axes[i].max_acceleration; } block->acceleration = block->acceleration_st / steps_per_mm; - // block->acceleration_rate = (int32_t)((float)block->acceleration_st * 8.388608); //WHOA! Where is this coming from?!? - + block->acceleration_rate = block->acceleration_st / ACCELERATION_TICKS_PER_SECOND; + +#ifndef CENTREPEDAL // Compute the speed trasitions, or "jerks" // Start with a safe speed float vmax_junction = max_xy_jerk/2.0; @@ -650,21 +655,30 @@ namespace planner { // this is basically the same as the entry speed calcs (done next) except this move is // the "previous" one and the "current" speed would be zero. float jerk = sqrt(pow((current_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]), 2)); - float stop_vmax_junction = vmax_junction; // we re-use vmax_junction below, so keep it - if((current_speed[X_AXIS] != 0.0) || (current_speed[Y_AXIS] != 0.0)) { - stop_vmax_junction = block->nominal_speed; - } - if (jerk > max_xy_jerk) { - stop_vmax_junction *= (max_xy_jerk/jerk); - } - for(int i=Z_AXIS; i < AXIS_COUNT; i++) { - float axis_jerk = fabs(current_speed[i]); - if(axis_jerk > axes[i].max_axis_jerk) { - stop_vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); - block->acceleration_rate = block->acceleration_st / ACCELERATION_TICKS_PER_SECOND; + // Now determine the safe max entry speed for this move + if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + vmax_junction = block->nominal_speed; + } + if (jerk > max_xy_jerk) { + vmax_junction *= (max_xy_jerk/jerk); + } + + for(int i=Z_AXIS; i < AXIS_COUNT; i++) { + float axis_jerk = fabs(current_speed[i] - previous_speed[i]); + if(axis_jerk > axes[i].max_axis_jerk) { + vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); + } + } + } + block->max_entry_speed = vmax_junction; + +#else // CENTREPEDAL + // Compute path unit vector - double unit_vec[3]; + float unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; @@ -679,13 +693,13 @@ namespace planner { // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. - double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. - double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; @@ -695,13 +709,17 @@ namespace planner { // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation - double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. + float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } - + + // Update previous path unit_vector and nominal speed + memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] +#endif + for (int i_axis = A_AXIS; i_axis < B_AXIS; i_axis++) { float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); if (jerk > axes[i_axis].max_axis_jerk) { @@ -711,7 +729,7 @@ namespace planner { block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + float v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -728,11 +746,8 @@ namespace planner { block->nominal_length_flag = false; block->recalculate_flag = true; // Always calculate trapezoid for new block - // Update previous path unit_vector and nominal speed + // Update previous path speed and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] - - // Update previous path unit_vector and nominal speed - memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); @@ -767,9 +782,11 @@ namespace planner { block_buffer.clear(); +#ifdef CENTREPEDAL previous_unit_vec[0]= 0.0; previous_unit_vec[1]= 0.0; previous_unit_vec[2]= 0.0; +#endif } void definePosition(const Point& new_position) @@ -782,9 +799,11 @@ namespace planner { previous_speed[i] = 0.0; } +#ifdef CENTREPEDAL previous_unit_vec[0]= 0.0; previous_unit_vec[1]= 0.0; previous_unit_vec[2]= 0.0; +#endif } const Point getPosition() diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index ecb5dc3..c6ae17c 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -104,8 +104,9 @@ namespace planner { void setMaxAxisJerk(float jerk, uint8_t axis); void setAcceleration(float acceleration); +#ifdef CENTREPEDAL void setJunctionDeviation(float new_junction_deviation); - +#endif void setAxisStepsPerMM(float steps_per_mm, uint8_t axis); bool isBufferFull(); diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 9d6f09d..0408c44 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -168,11 +168,11 @@ // The number of movements we can plan ahead at a time // THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... -#define BLOCK_BUFFER_SIZE 16 +#define BLOCK_BUFFER_SIZE 32 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 900.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 5.0 +#define DEFAULT_MAX_XY_JERK 8.0 // <-- unused if CENTREPEDAL is defined below #define DEFAULT_MAX_Z_JERK 5.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 @@ -181,6 +181,9 @@ // of the buffer and all stops. This should not be much greater than zero and should only be changed // if unwanted behavior is observed on a user's machine when running at very slow speeds. #define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) + +// define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob +#undef CENTREPEDAL #define DEFAULT_JUNCTION_DEVIATION 0.00005 // mm #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index c2587ea..3a7fcb0 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -85,8 +85,11 @@ Motherboard::Motherboard(const Pin& psu_pin) : /* END FIX THIS FIX THIS FIX THIS FIX THIS */ planner::setAcceleration(DEFAULT_ACCELERATION); +#ifdef CENTREPEDAL planner::setJunctionDeviation(DEFAULT_JUNCTION_DEVIATION); +#else planner::setMaxXYJerk(DEFAULT_MAX_XY_JERK); +#endif planner::setMaxAxisJerk(DEFAULT_MAX_Z_JERK, 2); planner::setMaxAxisJerk(DEFAULT_MAX_A_JERK, 3); planner::setMaxAxisJerk(DEFAULT_MAX_B_JERK, 4); From 33a8bf3a208b1d144275e472119ad761f88a0a8c Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 4 Feb 2012 18:45:15 -0600 Subject: [PATCH 46/61] Moving the planner "flags" to a single flags variable. Hopefully to save space. --- firmware/src/Motherboard/Command.cc | 2 +- firmware/src/Motherboard/Planner.cc | 31 ++++++++----------- firmware/src/Motherboard/Planner.hh | 23 +++++++++----- firmware/src/Motherboard/Steppers.cc | 4 +-- .../boards/rrmbv12/Configuration.hh | 6 ++-- 5 files changed, 34 insertions(+), 32 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 559f4be..b7857a5 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -30,7 +30,7 @@ namespace command { -#define COMMAND_BUFFER_SIZE 256 +#define COMMAND_BUFFER_SIZE 512 uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 42c7298..05e0ad2 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -374,7 +374,7 @@ namespace planner { } ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section - if(busy == false) { // Don't update variables if block is busy. + if(!(flags & Block::Busy)) { // Don't update variables if block is busy. accelerate_until = accelerate_steps; decelerate_after = accelerate_steps+plateau_steps; // initial_rate = initial_rate; @@ -425,13 +425,13 @@ namespace planner { // If nominal length true, max junction speed is guaranteed to be reached. Only compute // for max allowable speed if block is decelerating and nominal length is false. - if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) { + if ((!(current->flags & Block::NominalLength)) && (current->max_entry_speed > next->entry_speed)) { current->entry_speed = min( current->max_entry_speed, max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); } else { current->entry_speed = current->max_entry_speed; } - current->recalculate_flag = true; + current->flags |= Block::Recalculate; } } // Skip last block. Already initialized and set for recalculation. @@ -464,7 +464,7 @@ namespace planner { // full speed change within the block, we need to adjust the entry speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. - if (!previous->nominal_length_flag) { + if (!(previous->flags & Block::NominalLength)) { if (previous->entry_speed < current->entry_speed) { float entry_speed = min( current->entry_speed, max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); @@ -472,7 +472,7 @@ namespace planner { // Check for junction speed change if (current->entry_speed != entry_speed) { current->entry_speed = entry_speed; - current->recalculate_flag = true; + current->flags |= Block::Recalculate; } } } @@ -509,10 +509,10 @@ namespace planner { next = &block_buffer[block_index]; if (current) { // Recalculate if current block entry or exit junction speed has changed. - if (current->recalculate_flag || next->recalculate_flag) { + if ((current->flags & Block::Recalculate) || (next->flags & Block::Recalculate)) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. current->calculate_trapezoid(next->entry_speed); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + current->flags &= ~Block::Recalculate; // Reset current only to ensure next trapezoid is computed } } block_index = block_buffer.getNextIndex( block_index ); @@ -520,7 +520,7 @@ namespace planner { // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); - next->recalculate_flag = false; + next->flags &= ~Block::Recalculate; } } @@ -553,7 +553,7 @@ namespace planner { Block *block = block_buffer.getHead(); // Mark block as not busy (Not executed by the stepper interrupt) - block->busy = false; + block->flags = 0; block->target = target; @@ -651,14 +651,9 @@ namespace planner { vmax_junction = min(vmax_junction, block->nominal_speed); - // Determine the stopping speed for this move, in case it's the last one - // this is basically the same as the entry speed calcs (done next) except this move is - // the "previous" one and the "current" speed would be zero. - float jerk = sqrt(pow((current_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]), 2)); - // Now determine the safe max entry speed for this move if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { - jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } @@ -741,10 +736,10 @@ namespace planner { // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) - block->nominal_length_flag = true; + block->flags |= Block::NominalLength; else - block->nominal_length_flag = false; - block->recalculate_flag = true; // Always calculate trapezoid for new block + block->flags &= ~Block::NominalLength; + block->flags &= ~Block::Recalculate; // Always calculate trapezoid for new block // Update previous path speed and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index c6ae17c..978fd9a 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -36,7 +36,13 @@ namespace planner { // the source g-code and may never actually be reached if acceleration management is active. class Block { public: - // Fields used by the bresenham algorithm for tracing the line + typedef enum { + Busy = 1<<0, + Recalculate = 1<<1, + NominalLength = 1<<2, + } PlannerFlags; + + // Fields used by the bresenham algorithm for tracing the line Point target; // Final 5-axis target uint32_t step_event_count; // The number of step events required to complete this block int32_t accelerate_until; // The index of the step event on which to stop acceleration @@ -46,23 +52,24 @@ namespace planner { // uint8_t active_extruder; // Selects the active extruder - // Fields used by the motion planner to manage acceleration - // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis + // Fields used by the motion planner to manage acceleration + // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis float nominal_speed; // The nominal speed for this block in mm/min float entry_speed; // Entry speed at previous-current junction in mm/min float max_entry_speed; // Maximum allowable junction entry speed in mm/min float millimeters; // The total travel of this block in mm float acceleration; // acceleration mm/sec^2 - float stop_speed; // Speed to decelerate to if this is the last move - uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction - uint8_t nominal_length_flag; // Planner flag for nominal speed always reached + // float stop_speed; // Speed to decelerate to if this is the last move + // uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction + // uint8_t nominal_length_flag; // Planner flag for nominal speed always reached - // Settings for the trapezoid generator + // Settings for the trapezoid generator uint32_t nominal_rate; // The nominal step rate for this block in step_events/sec uint32_t initial_rate; // The jerk-adjusted step rate at start of block uint32_t final_rate; // The minimal rate at exit uint32_t acceleration_st; // acceleration steps/sec^2 - uint8_t busy; + // uint8_t busy; + uint8_t flags; Block() : target() {}; diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index bda58ce..77ca9ed 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -212,7 +212,7 @@ bool getNextMove() { current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) - current_block->busy = true; + current_block->flags |= planner::Block::Busy; Point &target = current_block->target; @@ -280,7 +280,7 @@ bool getNextMove() { } is_running = true; - current_block->busy = false; + current_block->flags &= ~planner::Block::Busy; planner::doneWithNextBlock(); return true; diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 0408c44..fb4de37 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -168,11 +168,11 @@ // The number of movements we can plan ahead at a time // THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... -#define BLOCK_BUFFER_SIZE 32 +#define BLOCK_BUFFER_SIZE 16 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 900.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 8.0 // <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_XY_JERK 5.0 // <-- unused if CENTREPEDAL is defined below #define DEFAULT_MAX_Z_JERK 5.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 @@ -180,7 +180,7 @@ // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed // if unwanted behavior is observed on a user's machine when running at very slow speeds. -#define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) +#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec) // define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob #undef CENTREPEDAL From 71d62f6250f4da821b61de9d106c22b1ff0c1fa1 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sun, 5 Feb 2012 01:07:02 -0600 Subject: [PATCH 47/61] Now simulating dynamic micro step switching to free up CPU. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Since we can't do it in hardware … yet. --- firmware/src/Motherboard/Planner.cc | 12 +----- firmware/src/Motherboard/Steppers.cc | 34 ++++++++++++--- .../boards/rrmbv12/Configuration.hh | 2 +- firmware/src/shared/StepperAxis.cc | 42 ++++++++++--------- firmware/src/shared/StepperAxis.hh | 6 +++ 5 files changed, 61 insertions(+), 35 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 05e0ad2..df086f6 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -660,15 +660,7 @@ namespace planner { if (jerk > max_xy_jerk) { vmax_junction *= (max_xy_jerk/jerk); } - - for(int i=Z_AXIS; i < AXIS_COUNT; i++) { - float axis_jerk = fabs(current_speed[i] - previous_speed[i]); - if(axis_jerk > axes[i].max_axis_jerk) { - vmax_junction *= (axes[i].max_axis_jerk/axis_jerk); - } - } } - block->max_entry_speed = vmax_junction; #else // CENTREPEDAL @@ -715,7 +707,7 @@ namespace planner { memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] #endif - for (int i_axis = A_AXIS; i_axis < B_AXIS; i_axis++) { + for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); if (jerk > axes[i_axis].max_axis_jerk) { vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); @@ -750,7 +742,7 @@ namespace planner { // Update position position = target; - // Move buffer head -- should this move to after recalculate? + // Move buffer head block_buffer.bumpHead(); planner_recalculate(); diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 77ca9ed..f990ee8 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -41,7 +41,7 @@ volatile int8_t feedrate_dirty; // indicates if the feedrate_inverted needs rec volatile int32_t feedrate_inverted; volatile int32_t feedrate_changerate; volatile int32_t acceleration_tick_counter; -volatile int32_t feedrate_start_halving; +volatile int32_t feedrate_multiplier; // should always be 2^N and > 0 volatile uint8_t current_feedrate_index; volatile int32_t timer_counter; @@ -92,6 +92,7 @@ void abort() { feedrate = 0; feedrate_inverted = 0; feedrate_dirty = 1; + feedrate_multiplier = 1; } /// Define current position as given point @@ -266,6 +267,19 @@ bool getNextMove() { prepareFeedrateIntervals(); feedrate_inverted = 1000000/feedrate; + + // NOTE: the following code is duplicated in the interrupt, and should be a subroutine + + // if we are supposed to step too fast, we simulate double-size microsteps + feedrate_multiplier = 1; + while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + feedrate_multiplier <<= 1; // * 2 + feedrate_inverted <<= 1; // * 2 + } + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setStepMultiplier(feedrate_multiplier); + } + feedrate_dirty = 0; acceleration_tick_counter = TICKS_PER_ACCELERATION; @@ -322,8 +336,8 @@ bool doInterrupt() { stepperTimingDebugPin.setValue(true); timer_counter -= INTERVAL_IN_MICROSECONDS; - while (timer_counter <= 0) { - if (intervals_remaining-- == 0) { + if (timer_counter <= 0) { + if ((intervals_remaining -= feedrate_multiplier) == 0) { getNextMove(); stepperTimingDebugPin.setValue(false); return is_running; @@ -336,15 +350,25 @@ bool doInterrupt() { if (feedrate_dirty) { feedrate_inverted = 1000000/feedrate; + + // if we are supposed to step too fast, we simulate double-size microsteps + feedrate_multiplier = 1; + while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + feedrate_multiplier <<= 1; // * 2 + feedrate_inverted <<= 1; // * 2 + } + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setStepMultiplier(feedrate_multiplier); + } + feedrate_dirty = 0; } timer_counter += feedrate_inverted; - if (feedrate_steps_remaining-- <= 0) { + if ((feedrate_steps_remaining -= feedrate_multiplier) <= 0) { current_feedrate_index++; prepareFeedrateIntervals(); - break; } } } diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index fb4de37..66bd2e6 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -26,7 +26,7 @@ // possible time between steps; in practical terms, your time between steps should // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. -#define INTERVAL_IN_MICROSECONDS 128 +#define INTERVAL_IN_MICROSECONDS 256 // TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded #define TICKS_PER_ACCELERATION 5 // lower is better diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index b715884..e5aa762 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -8,6 +8,12 @@ StepperAxis::StepperAxis(StepperInterface& stepper_interface) : interface(&stepp reset(); } +void StepperAxis::setStepMultiplier(const int8_t new_multiplier) { + step_multiplier = new_multiplier; + step_change = direction ? step_multiplier : -step_multiplier; +} + + void StepperAxis::setTarget(const int32_t target_in, bool relative) { if (relative) { delta = target_in; @@ -23,6 +29,9 @@ void StepperAxis::setTarget(const int32_t target_in, bool relative) { if (delta < 0) { delta = -delta; direction = false; + step_change = -step_multiplier; + } else { + step_change = step_multiplier; } interface->setDirection(direction); } @@ -49,6 +58,8 @@ void StepperAxis::reset() { target = 0; counter = 0; delta = 0; + step_multiplier = 1; + step_change = 1; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) endstop_play = ENDSTOP_DEFAULT_PLAY; endstop_status = ESS_UNKNOWN; @@ -103,13 +114,12 @@ void StepperAxis::doInterrupt(const int32_t intervals) { if (counter >= 0) { counter -= intervals; bool hit_endstop = checkEndstop(false); - if (!hit_endstop) interface->step(true); - if (direction) { - position++; - } else { - position--; - } - interface->step(false); + if (!hit_endstop) + for (int8_t steps = step_multiplier; steps > 0; steps--) { + interface->step(true); + interface->step(false); + } + position += step_change; } } @@ -120,22 +130,16 @@ bool StepperAxis::doHoming(const int32_t intervals) { if (counter >= 0) { counter -= intervals; bool hit_endstop = checkEndstop(true); - if (direction) { - if (!hit_endstop) { + if (!hit_endstop) { + // we honor the step multiplier here, but it *really* should be 1 for homing + for (int8_t steps = step_multiplier; steps > 0; steps--) { interface->step(true); - } else { - return false; + interface->step(false); } - position++; } else { - if (!hit_endstop) { - interface->step(true); - } else { - return false; - } - position--; + return false; } - interface->step(false); + position += step_change; } return true; } \ No newline at end of file diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index 3252b80..cb3e7f0 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -21,6 +21,8 @@ public: ///< zero, a step is taken. volatile int32_t delta; ///< Amount to increment counter per tick volatile bool direction; ///< True for positive, false for negative + volatile int8_t step_multiplier; ///< Used to simulate dynamic microstep switching, must be > 0 and 2^N + volatile int8_t step_change; ///< Uses internally. step_change = direction ? step_multiplier : -step_multiplier; #if defined(SINGLE_SWITCH_ENDSTOPS) && (SINGLE_SWITCH_ENDSTOPS == 1) volatile bool prev_direction; ///< Record the previous direction for endstop detection volatile int32_t endstop_play; ///< Amount to move while endstop triggered, to see which way to move @@ -58,6 +60,10 @@ public: /// to be relative to the current position. void setTarget(const int32_t target_in, bool relative); + /// Set the step multiplier -- must be 2^N + /// \param[in] new_multiplier + void setStepMultiplier(const int8_t new_multiplier); + /// Start a homing procedure /// \param[in] direction_in If true, home in the positive direction. void setHoming(const bool direction_in); From ec9a8d02dc5bed18a4bd087226c241487a67e8e6 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Mon, 6 Feb 2012 15:16:20 -0600 Subject: [PATCH 48/61] Fixed various planner bugs. Switching directions now slows down. Co-linear movements no longer have slow-downs in between. --- firmware/src/Motherboard/Planner.cc | 116 ++++++++------- firmware/src/Motherboard/Steppers.cc | 133 +++++++++++++----- firmware/src/Motherboard/Steppers.hh | 6 +- .../Motherboard/boards/rrmbv12/Motherboard.cc | 14 +- 4 files changed, 173 insertions(+), 96 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index df086f6..dbce4a5 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -344,42 +344,44 @@ namespace planner { float entry_factor = entry_speed/nominal_speed; float exit_factor = exit_factor_speed/nominal_speed; - initial_rate = ceil((float)nominal_rate*entry_factor); // (step/min) - final_rate = ceil((float)nominal_rate*exit_factor); // (step/min) + uint32_t local_initial_rate = ceil((float)nominal_rate*entry_factor); // (step/min) + uint32_t local_final_rate = ceil((float)nominal_rate*exit_factor); // (step/min) // Limit minimal step rate (Otherwise the timer will overflow.) - if(initial_rate < 120) - initial_rate = 120; - if(final_rate < 120) - final_rate = 120; + if(local_initial_rate < 120) + local_initial_rate = 120; + if(local_final_rate < 120) + local_final_rate = 120; int32_t acceleration = acceleration_st; int32_t accelerate_steps = - ceil(estimate_acceleration_distance(initial_rate, nominal_rate, acceleration)); + ceil(estimate_acceleration_distance(local_initial_rate, nominal_rate, acceleration)); int32_t decelerate_steps = - floor(estimate_acceleration_distance(nominal_rate, final_rate, -acceleration)); + floor(estimate_acceleration_distance(nominal_rate, local_final_rate, -acceleration)); // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = step_event_count-accelerate_steps-decelerate_steps; // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking - // in order to reach the final_rate exactly at the end of this block. + // in order to reach the local_final_rate exactly at the end of this block. if (plateau_steps < 0) { accelerate_steps = ceil( - intersection_distance(initial_rate, final_rate, acceleration, step_event_count)); + intersection_distance(local_initial_rate, local_final_rate, acceleration, step_event_count)); accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); plateau_steps = 0; } ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section - if(!(flags & Block::Busy)) { // Don't update variables if block is busy. + if(!(flags & Block::Busy)) { accelerate_until = accelerate_steps; decelerate_after = accelerate_steps+plateau_steps; - // initial_rate = initial_rate; - // final_rate = final_rate; - } // So, ummm, what if it IS busy?! + initial_rate = local_initial_rate; + final_rate = local_final_rate; + } + // if(flags & Block::Busy) + // steppers::currentBlockChanged(); } // ISR state will be automatically restored here } @@ -392,20 +394,24 @@ namespace planner { // Recalculates the motion plan according to the following algorithm: // - // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. Block.entry_factor) + // 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) // so that: - // a. The junction jerk is within the set limit + // a. The junction speed is equal to or less than the maximum junction speed limit // b. No speed reduction within one block requires faster deceleration than the one, true constant // acceleration. - // 2. Go over every block in chronological order and dial down junction speed reduction values if - // a. The speed increase within one block would require faster accelleration than the one, true + // 2. Go over every block in chronological order and dial down junction speed values if + // a. The speed increase within one block would require faster acceleration than the one, true // constant acceleration. // - // When these stages are complete all blocks have an entry_factor that will allow all speed changes to - // be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than - // the set limit. Finally it will: + // When these stages are complete all blocks have an entry speed that will allow all speed changes to + // be performed using only the one, true constant acceleration, and where no junction speed is greater + // than the max limit. Finally it will: + // + // 3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids + // with no updated junction speeds will not be recalculated and assumed ok as is. // - // 3. Recalculate trapezoids for all blocks. + // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- + // off errors. Only when planned values are converted to stepper rate parameters, these are integers. void planner_recalculate() { planner_reverse_pass(); @@ -422,19 +428,17 @@ namespace planner { // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // check for maximum allowable speed reductions to ensure maximum possible planned speed. if (current->entry_speed != current->max_entry_speed) { - // If nominal length true, max junction speed is guaranteed to be reached. Only compute // for max allowable speed if block is decelerating and nominal length is false. - if ((!(current->flags & Block::NominalLength)) && (current->max_entry_speed > next->entry_speed)) { + if ((!(current->flags & Block::NominalLength)) && (current->max_entry_speed == next->entry_speed)) { current->entry_speed = min( current->max_entry_speed, max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters)); } else { current->entry_speed = current->max_entry_speed; } current->flags |= Block::Recalculate; - } - } // Skip last block. Already initialized and set for recalculation. + } } // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This @@ -459,13 +463,25 @@ namespace planner { // The kernel called by planner_recalculate() when scanning the plan from first to last entry. void planner_forward_pass_kernel(Block *previous, Block *current, Block *next) { if(!previous) { return; } + + // If the previous block is busy, then we're currently executing it! + // We have to be careful here, but we want to try to smooth out the movement if it's not too late. + // That smoothing will happen in Block::calculate_trapezoid later. + // However, if it *is* too late, then we need to fix the current entry speed. + // if (previous->flags & Block::Busy) { + // uint32_t current_step = steppers::getCurrentStep(); + // // If we are withing 10 steps, we're probably too late + // // if (current_step > (previous->decelerate_after - 10)) { + // // current->entry_speed = MINIMUM_PLANNER_SPEED; + // // } + // } // If the previous block is an acceleration block, but it is not long enough to complete the // full speed change within the block, we need to adjust the entry speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. if (!(previous->flags & Block::NominalLength)) { - if (previous->entry_speed < current->entry_speed) { + if (previous->entry_speed == current->entry_speed) { float entry_speed = min( current->entry_speed, max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) ); @@ -517,7 +533,8 @@ namespace planner { } block_index = block_buffer.getNextIndex( block_index ); } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. + + // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); next->flags &= ~Block::Recalculate; @@ -558,7 +575,7 @@ namespace planner { block->target = target; // // store the absolute number of steps in each direction, without direction - Point steps = (target - position).abs(); + Point steps = (target - position); float delta_mm[AXIS_COUNT]; block->millimeters = 0.0; @@ -566,8 +583,9 @@ namespace planner { // // Compute direction bits for this block -- UNUSED FOR NOW // block->direction_bits = 0; for (int i = 0; i < AXIS_COUNT; i++) { - if (steps[i] > block->step_event_count) { - block->step_event_count = steps[i]; + int32_t abs_steps = abs(steps[i]); + if (abs_steps > block->step_event_count) { + block->step_event_count = abs_steps; } delta_mm[i] = ((float)steps[i])/axes[i].steps_per_mm; if (i < A_AXIS) @@ -642,24 +660,26 @@ namespace planner { #ifndef CENTREPEDAL // Compute the speed trasitions, or "jerks" // Start with a safe speed - float vmax_junction = max_xy_jerk/2.0; - { - float half_max_z_axis_jerk = axes[Z_AXIS].max_axis_jerk/2.0; - if(fabs(current_speed[Z_AXIS]) > half_max_z_axis_jerk) - vmax_junction = half_max_z_axis_jerk; - } - - vmax_junction = min(vmax_junction, block->nominal_speed); + float vmax_junction = MINIMUM_PLANNER_SPEED; // Now determine the safe max entry speed for this move + // Skip the first block if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } + if (jerk > max_xy_jerk) { vmax_junction *= (max_xy_jerk/jerk); } + + for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { + jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); + if (jerk > axes[i_axis].max_axis_jerk) { + vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); + } + } } #else // CENTREPEDAL @@ -701,22 +721,22 @@ namespace planner { sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } + + for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { + float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); + if (jerk > axes[i_axis].max_axis_jerk) { + vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); + } + } } // Update previous path unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] #endif - - for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { - float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); - if (jerk > axes[i_axis].max_axis_jerk) { - vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); - } - } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - float v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); + float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -731,7 +751,7 @@ namespace planner { block->flags |= Block::NominalLength; else block->flags &= ~Block::NominalLength; - block->flags &= ~Block::Recalculate; // Always calculate trapezoid for new block + block->flags |= Block::Recalculate; // Always calculate trapezoid for new block // Update previous path speed and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index f990ee8..9a7cd1c 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -21,6 +21,7 @@ #include "Planner.hh" #include #include +#include namespace steppers { @@ -66,7 +67,7 @@ void init(Motherboard& motherboard) { } timer_counter = 0; - current_block = 0; + current_block = NULL; for (int i = 0; i < 3; i++) { feedrate_elements[i] = feedrate_element(); @@ -85,6 +86,7 @@ void abort() { is_running = false; is_homing = false; timer_counter = 0; + current_block = NULL; // feedrate_scale_shift = 0; // feedrate_intervals = 0; // feedrate_intervals_remaining = 0; @@ -203,13 +205,44 @@ inline void prepareFeedrateIntervals() { // acceleration_tick_counter = 0; } +inline void recalcFeedrate() { + feedrate_inverted = 1000000/feedrate; + + // if we are supposed to step too fast, we simulate double-size microsteps + feedrate_multiplier = 1; + while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + feedrate_multiplier <<= 1; // * 2 + feedrate_inverted <<= 1; // * 2 + } + + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setStepMultiplier(feedrate_multiplier); + } + + feedrate_dirty = 0; +} + +uint32_t getCurrentStep() { + return intervals - intervals_remaining; +} + + // load up the next movment // WARNING: called from inside the ISR, so get out fast -bool getNextMove() { +bool getNextMove() { + stepperTimingDebugPin.setValue(true); is_running = false; // this ensures that the interrupt does not .. interrupt us - if (planner::isBufferEmpty()) + if (current_block != NULL) { + current_block->flags &= ~planner::Block::Busy; + planner::doneWithNextBlock(); + current_block = NULL; + } + + if (planner::isBufferEmpty()) { + stepperTimingDebugPin.setValue(false); return false; + } current_block = planner::getNextBlock(); // Mark block as busy (being executed by the stepper interrupt) @@ -266,25 +299,11 @@ bool getNextMove() { } prepareFeedrateIntervals(); - feedrate_inverted = 1000000/feedrate; - - // NOTE: the following code is duplicated in the interrupt, and should be a subroutine - - // if we are supposed to step too fast, we simulate double-size microsteps - feedrate_multiplier = 1; - while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - feedrate_multiplier <<= 1; // * 2 - feedrate_inverted <<= 1; // * 2 - } - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setStepMultiplier(feedrate_multiplier); - } - - feedrate_dirty = 0; + recalcFeedrate(); acceleration_tick_counter = TICKS_PER_ACCELERATION; // We use += here so that the odd rounded-off time from the last move is still waited out - timer_counter += feedrate; + timer_counter += feedrate_inverted; intervals = max_delta; intervals_remaining = intervals; @@ -294,12 +313,60 @@ bool getNextMove() { } is_running = true; - current_block->flags &= ~planner::Block::Busy; - planner::doneWithNextBlock(); - + stepperTimingDebugPin.setValue(false); return true; } +void currentBlockChanged() { + // If we are here, then we are moving AND the interrupts are frozen, so get out *fast* + uint32_t current_step = intervals - intervals_remaining; + + current_feedrate_index = 0; + int feedrate_being_setup = 0; + // setup acceleration + feedrate = 0; + if (current_block->accelerate_until > current_step) { + feedrate = current_block->initial_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } else { + // reclac current_step to be the current step inside this submovement + current_step -= current_block->accelerate_until; + } + + // setup plateau + if (current_block->decelerate_after > current_block->accelerate_until && current_block->decelerate_after > current_step) { + if (feedrate == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].rate = 0; + feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; + feedrate_being_setup++; + } else { + // reclac current_step to be the current step inside this submovement + current_step -= current_block->decelerate_after - current_block->accelerate_until; + } + + // setup deceleration + if (current_block->decelerate_after < current_block->step_event_count) { + if (feedrate == 0) + feedrate = current_block->nominal_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->step_event_count - current_block->decelerate_after; + feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; + feedrate_elements[feedrate_being_setup].target = current_block->final_rate; + } + + prepareFeedrateIntervals(); + // remove the amount of steps we've already taken... + feedrate_steps_remaining -= current_step; + recalcFeedrate(); +} + /// Start homing void startHoming(const bool maximums, const uint8_t axes_enabled, const uint32_t us_per_step) { intervals_remaining = INT32_MAX; @@ -333,13 +400,13 @@ void startRunning() { bool doInterrupt() { if (is_running) { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); timer_counter -= INTERVAL_IN_MICROSECONDS; if (timer_counter <= 0) { - if ((intervals_remaining -= feedrate_multiplier) == 0) { + if ((intervals_remaining -= feedrate_multiplier) <= 0) { getNextMove(); - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); return is_running; // is_running = false; } else { @@ -349,19 +416,7 @@ bool doInterrupt() { } if (feedrate_dirty) { - feedrate_inverted = 1000000/feedrate; - - // if we are supposed to step too fast, we simulate double-size microsteps - feedrate_multiplier = 1; - while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - feedrate_multiplier <<= 1; // * 2 - feedrate_inverted <<= 1; // * 2 - } - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setStepMultiplier(feedrate_multiplier); - } - - feedrate_dirty = 0; + recalcFeedrate(); } timer_counter += feedrate_inverted; @@ -392,7 +447,7 @@ bool doInterrupt() { } - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); return is_running; } else if (is_homing) { is_homing = false; diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index ee42f2b..2d2b732 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -93,9 +93,11 @@ namespace steppers { /// disabled when not moving. void setHoldZ(bool holdZ); - // bool getNextMove(); - void startRunning(); + + void currentBlockChanged(); + + uint32_t getCurrentStep(); }; #endif // STEPPERS_HH_ diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index 3a7fcb0..738b651 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -76,13 +76,13 @@ Motherboard::Motherboard(const Pin& psu_pin) : eeprom::AXIS_INVERSION); #endif - /* FIX THIS FIX THIS FIX THIS FIX THIS */ - planner::setAxisStepsPerMM(94.1397046, 0); - planner::setAxisStepsPerMM(94.1397046, 1); - planner::setAxisStepsPerMM(2560.0, 2); - planner::setAxisStepsPerMM(100.470957613814818, 3); - planner::setAxisStepsPerMM(100.470957613814818, 4); - /* END FIX THIS FIX THIS FIX THIS FIX THIS */ + /* FIX THIS FIX THIS FIX THIS FIX THIS */ + planner::setAxisStepsPerMM(94.1397046, 0); + planner::setAxisStepsPerMM(94.1397046, 1); + planner::setAxisStepsPerMM(2560.0, 2); + planner::setAxisStepsPerMM(100.470957613814818, 3); + planner::setAxisStepsPerMM(100.470957613814818, 4); + /* END FIX THIS FIX THIS FIX THIS FIX THIS */ planner::setAcceleration(DEFAULT_ACCELERATION); #ifdef CENTREPEDAL From d6bf0d85ebe10c005d418845903da07591853cef Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 9 Feb 2012 01:48:07 -0600 Subject: [PATCH 49/61] Optimizing for speed. --- firmware/src/Motherboard/Command.cc | 9 +++ firmware/src/Motherboard/Planner.cc | 81 +++++++++++++++---- firmware/src/Motherboard/Steppers.cc | 54 ++++++++----- .../boards/rrmbv12/Configuration.hh | 13 ++- 4 files changed, 114 insertions(+), 43 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index b7857a5..5dd7dc0 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -30,6 +30,8 @@ namespace command { +// Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; + #define COMMAND_BUFFER_SIZE 512 uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); @@ -114,8 +116,12 @@ void reset() { // Handle movement comands -- called from a few places static void handleMovementCommand(const uint8_t &command) { + // stepperTimingDebugPin.setDirection(true); + // stepperTimingDebugPin.setValue(false); // if we're already moving, check to make sure the buffer isn't full if (mode == MOVING && planner::isBufferFull()) { + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); return; // we'll be back! } if (command == HOST_CMD_QUEUE_POINT_ABS) { @@ -385,6 +391,9 @@ void runCommandSlice() { } } else { } + } else { // command buffer is empty + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); } } } diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index dbce4a5..94f959e 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -121,6 +121,7 @@ inline const T& max(const T& a, const T& b) { return (a)>(b)?(a):(b); } namespace planner { + Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; // Super-simple circular buffer, where old nodes are reused // TODO: Move to a seperate file @@ -224,6 +225,28 @@ namespace planner { // note that X+Y has it's own setting, and this if for all the rest float max_axis_jerk; }; +#if 0 + // precision is the number of bit to use for the fractional part + template + struct QuickFixedPoint + { + base_type value; + QuickFixedPoint(float init_value) : value((base_type)(init_value * (float)(1<>a.precision; + } + base_type operator/(QuickFixedPoint a) { + return (value / a)>>a.precision; + } + }; +#endif PlannerAxis axes[AXIS_COUNT]; @@ -251,8 +274,8 @@ namespace planner { position = Point(0,0,0,0,0); previous_nominal_speed = 0.0; - axes[0].max_acceleration = 900*axes[0].steps_per_mm; - axes[1].max_acceleration = 900*axes[1].steps_per_mm; + axes[0].max_acceleration = 3000*axes[0].steps_per_mm; + axes[1].max_acceleration = 3000*axes[1].steps_per_mm; axes[2].max_acceleration = 200*axes[2].steps_per_mm; axes[3].max_acceleration = 10000*axes[3].steps_per_mm; axes[4].max_acceleration = 10000*axes[4].steps_per_mm; @@ -261,6 +284,10 @@ namespace planner { // axes[2].max_acceleration = 200*axes[2].steps_per_mm; // axes[3].max_acceleration = 20000*axes[3].steps_per_mm; // axes[4].max_acceleration = 20000*axes[4].steps_per_mm; + + stepperTimingDebugPin.setDirection(true); + stepperTimingDebugPin.setValue(false); + } @@ -292,18 +319,19 @@ namespace planner { // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { - return sqrt(target_velocity*target_velocity-2*acceleration*distance); + return sqrt((target_velocity*target_velocity)-(acceleration*2.0)*distance); } // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the - // given acceleration: - FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) + // given acceleration. + // Rates are given pre-squared, and accelereation pre-doubled for caching in the calling subroutine. + FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) { - if (acceleration!=0) { - return((target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration)); + if (acceleration_doubled!=0) { + return((target_rate_squared-initial_rate_squared)/(acceleration_doubled)); } else { - return 0.0; // acceleration was 0, set acceleration distance to 0 + return 0; // acceleration was 0, set acceleration distance to 0 } } @@ -312,10 +340,11 @@ namespace planner { // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) + // Rates are given pre-squared, and accelereation pre-doubled for caching in the calling subroutine. + FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_doubled, int32_t distance) { - if (acceleration!=0) { - return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration)); + if (acceleration_doubled!=0) { + return((acceleration_doubled*distance-initial_rate_squared+final_rate_squared)/(acceleration_doubled<<1)); } else { return 0.0; // acceleration was 0, set intersection distance to 0 @@ -341,6 +370,8 @@ namespace planner { // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); void Block::calculate_trapezoid(float exit_factor_speed) { + stepperTimingDebugPin.setValue(true); + float entry_factor = entry_speed/nominal_speed; float exit_factor = exit_factor_speed/nominal_speed; @@ -353,11 +384,19 @@ namespace planner { if(local_final_rate < 120) local_final_rate = 120; + uint32_t local_initial_rate_squared = local_initial_rate; + local_initial_rate_squared = (local_initial_rate_squared*local_initial_rate_squared); + uint32_t local_final_rate_squared = local_final_rate; + local_final_rate_squared = (local_final_rate_squared*local_final_rate_squared); + uint32_t nominal_rate_squared = nominal_rate; + nominal_rate_squared = (nominal_rate_squared*nominal_rate_squared); + int32_t acceleration = acceleration_st; + int32_t acceleration_doubled = acceleration_st<<1; // * 2 int32_t accelerate_steps = - ceil(estimate_acceleration_distance(local_initial_rate, nominal_rate, acceleration)); + /*ceil*/(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, acceleration_doubled)); int32_t decelerate_steps = - floor(estimate_acceleration_distance(nominal_rate, local_final_rate, -acceleration)); + /*floor*/(estimate_acceleration_distance(nominal_rate_squared, local_final_rate_squared, -acceleration_doubled)); // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = step_event_count-accelerate_steps-decelerate_steps; @@ -366,8 +405,8 @@ namespace planner { // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the local_final_rate exactly at the end of this block. if (plateau_steps < 0) { - accelerate_steps = ceil( - intersection_distance(local_initial_rate, local_final_rate, acceleration, step_event_count)); + accelerate_steps = /*ceil*/( + intersection_distance(local_initial_rate_squared, local_final_rate_squared, acceleration_doubled, step_event_count)); accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); plateau_steps = 0; @@ -383,6 +422,8 @@ namespace planner { // if(flags & Block::Busy) // steppers::currentBlockChanged(); } // ISR state will be automatically restored here + + stepperTimingDebugPin.setValue(false); } // forward declare, so we can order the code in a slightly more readable fashion @@ -565,8 +606,13 @@ namespace planner { // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly bool addMoveToBuffer(const Point& target, int32_t us_per_step) { - if (block_buffer.isFull()) + // stepperTimingDebugPin.setValue(true); + if (block_buffer.isFull()) { + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); return false; + // stepperTimingDebugPin.setValue(false); + } Block *block = block_buffer.getHead(); // Mark block as not busy (Not executed by the stepper interrupt) @@ -757,7 +803,7 @@ namespace planner { memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; - block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + // block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); // Update position position = target; @@ -768,6 +814,7 @@ namespace planner { planner_recalculate(); steppers::startRunning(); + // stepperTimingDebugPin.setValue(false); return true; } diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 9a7cd1c..d929ae8 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -107,7 +107,11 @@ void definePosition(const Point& position) { /// Get current position const Point getPosition() { #if STEPPER_COUNT > 3 - return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,axes[4].position); + #if STEPPER_COUNT > 4 + return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,axes[4].position); + #else + return Point(axes[0].position,axes[1].position,axes[2].position,axes[3].position,0); + #endif #else return Point(axes[0].position,axes[1].position,axes[2].position); #endif @@ -208,16 +212,16 @@ inline void prepareFeedrateIntervals() { inline void recalcFeedrate() { feedrate_inverted = 1000000/feedrate; - // if we are supposed to step too fast, we simulate double-size microsteps - feedrate_multiplier = 1; - while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - feedrate_multiplier <<= 1; // * 2 - feedrate_inverted <<= 1; // * 2 - } - - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setStepMultiplier(feedrate_multiplier); - } + // // if we are supposed to step too fast, we simulate double-size microsteps + // feedrate_multiplier = 1; + // while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + // feedrate_multiplier <<= 1; // * 2 + // feedrate_inverted <<= 1; // * 2 + // } + // + // for (int i = 0; i < STEPPER_COUNT; i++) { + // axes[i].setStepMultiplier(feedrate_multiplier); + // } feedrate_dirty = 0; } @@ -230,7 +234,7 @@ uint32_t getCurrentStep() { // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); is_running = false; // this ensures that the interrupt does not .. interrupt us if (current_block != NULL) { @@ -240,6 +244,7 @@ bool getNextMove() { } if (planner::isBufferEmpty()) { + stepperTimingDebugPin.setValue(true); stepperTimingDebugPin.setValue(false); return false; } @@ -313,7 +318,7 @@ bool getNextMove() { } is_running = true; - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); return true; } @@ -410,21 +415,32 @@ bool doInterrupt() { return is_running; // is_running = false; } else { - + // if we are supposed to step too fast, we simulate double-size microsteps + feedrate_multiplier = 1; + while (timer_counter <= -feedrate_inverted && feedrate_steps_remaining > 0) { + feedrate_multiplier++; + timer_counter += feedrate_inverted; + feedrate_steps_remaining--; + } + for (int i = 0; i < STEPPER_COUNT; i++) { + axes[i].setStepMultiplier(feedrate_multiplier); axes[i].doInterrupt(intervals); } + if (feedrate_steps_remaining-- <= 0) { + current_feedrate_index++; + prepareFeedrateIntervals(); + } + if (feedrate_dirty) { recalcFeedrate(); + if (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + feedrate_inverted = INTERVAL_IN_MICROSECONDS; + } } timer_counter += feedrate_inverted; - - if ((feedrate_steps_remaining -= feedrate_multiplier) <= 0) { - current_feedrate_index++; - prepareFeedrateIntervals(); - } } } diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 66bd2e6..974b0b1 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -26,7 +26,7 @@ // possible time between steps; in practical terms, your time between steps should // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. -#define INTERVAL_IN_MICROSECONDS 256 +#define INTERVAL_IN_MICROSECONDS 128 // TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded #define TICKS_PER_ACCELERATION 5 // lower is better @@ -74,8 +74,7 @@ #define STEPPER_COUNT 3 #else // Rob G's hacked G3 motherboard supports four steppers. - // why 5? Because everywhere there are checks for more than 3 that assume 5 - #define STEPPER_COUNT 5 + #define STEPPER_COUNT 4 #endif // FOURTH_STEPPER // --- Stepper and endstop configuration --- @@ -171,9 +170,9 @@ #define BLOCK_BUFFER_SIZE 16 //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 900.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 5.0 // <-- unused if CENTREPEDAL is defined below -#define DEFAULT_MAX_Z_JERK 5.0 +#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 15.0 // <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 10.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 @@ -184,6 +183,6 @@ // define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob #undef CENTREPEDAL -#define DEFAULT_JUNCTION_DEVIATION 0.00005 // mm +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ From 4c98d0b819b6707a0232ffcef5b3a33482bb0826 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 9 Feb 2012 02:38:53 -0600 Subject: [PATCH 50/61] More optimizing. Added hack to set min segment size. Also removed the use of % (modulus) from the CircularBuffer. --- firmware/src/Motherboard/Command.cc | 2 +- firmware/src/Motherboard/Planner.cc | 9 +++++++++ firmware/src/Motherboard/Planner.hh | 1 - firmware/src/Motherboard/Steppers.cc | 6 +++--- firmware/src/shared/CircularBuffer.hh | 9 +++++---- 5 files changed, 18 insertions(+), 9 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index 5dd7dc0..fbb24fa 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -32,7 +32,7 @@ namespace command { // Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; -#define COMMAND_BUFFER_SIZE 512 +#define COMMAND_BUFFER_SIZE 512 // Must be 2^N: 256, 512, 1204, etc uint8_t buffer_data[COMMAND_BUFFER_SIZE]; CircularBuffer command_buffer(COMMAND_BUFFER_SIZE, buffer_data); diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 94f959e..ee68093 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -78,6 +78,9 @@ #include "Steppers.hh" #include "Point.hh" +// My tests show it takes about 72ms to plan one segment on a Gen3 MB, so we need to give it room. -Rob +#define MIN_MS_PER_SEGMENT 1200 + #define X_AXIS 0 #define Y_AXIS 1 #define Z_AXIS 2 @@ -640,6 +643,12 @@ namespace planner { } block->millimeters = sqrt(block->millimeters); + // CLEAN ME: Ugly dirty check to prevent a lot of small moves from causing a planner buffer underrun + // For now, we'll just make sure each movement takes at least MIN_MS_PER_SEGMENT millisesconds to complete + if ((us_per_step * block->step_event_count) < MIN_MS_PER_SEGMENT) { + us_per_step = MIN_MS_PER_SEGMENT / block->step_event_count; + } + float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate 1 second/(seconds for this movement) float inverse_second = 1000000.0/(float)(us_per_step * block->step_event_count); diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index 978fd9a..ea09a47 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -28,7 +28,6 @@ #include "Configuration.hh" #include -#include "CircularBuffer.hh" #include "Point.hh" namespace planner { diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index d929ae8..c81b983 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -435,9 +435,9 @@ bool doInterrupt() { if (feedrate_dirty) { recalcFeedrate(); - if (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - feedrate_inverted = INTERVAL_IN_MICROSECONDS; - } + // if (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { + // feedrate_inverted = INTERVAL_IN_MICROSECONDS; + // } } timer_counter += feedrate_inverted; diff --git a/firmware/src/shared/CircularBuffer.hh b/firmware/src/shared/CircularBuffer.hh index a27cd7b..40aae55 100644 --- a/firmware/src/shared/CircularBuffer.hh +++ b/firmware/src/shared/CircularBuffer.hh @@ -34,6 +34,7 @@ public: typedef T BufDataType; private: const BufSizeType size; /// Size of this buffer + const BufSizeType size_mask; /// Mask of the buffer size, in binary, as size-1. volatile BufSizeType length; /// Current length of valid buffer data volatile BufSizeType start; /// Current start point of valid bufffer data BufDataType* const data; /// Pointer to buffer data @@ -41,7 +42,7 @@ private: volatile bool underflow; /// Underflow indicator public: CircularBufferTempl(BufSizeType size_in, BufDataType* data_in) : - size(size_in), length(0), start(0), data(data_in), overflow(false), + size(size_in), size_mask(size-1), length(0), start(0), data(data_in), overflow(false), underflow(false) { } @@ -69,7 +70,7 @@ public: return BufDataType(); } const BufDataType& popped_byte = operator[](0); - start = (start + 1) % size; + start = (start + 1) & size_mask; length--; return popped_byte; } @@ -82,7 +83,7 @@ public: underflow = true; sz = length; } - start = (start + sz) % size; + start = (start + sz) & size_mask; length -= sz; } @@ -102,7 +103,7 @@ public: } /// Read the buffer directly inline BufDataType& operator[](BufSizeType index) { - const BufSizeType actual_index = (index + start) % size; + const BufSizeType actual_index = (index + start) & size_mask; return data[actual_index]; } /// Check the overflow flag From fb2545661c2ce0534836dfba8d7aea8e3454234c Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 9 Feb 2012 02:40:26 -0600 Subject: [PATCH 51/61] Changed the stepper tick timing to slower (256ms). Now that the stepper dynamically multi-step, we can step less often and get perceptively as smooth of operation. We really need to use a dedicated timer for this, though. --- firmware/src/Motherboard/boards/rrmbv12/Configuration.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 974b0b1..71a9641 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -26,7 +26,7 @@ // possible time between steps; in practical terms, your time between steps should // be at least eight times this large. Reducing the interval can cause resource // starvation; leave this at 64uS or greater unless you know what you're doing. -#define INTERVAL_IN_MICROSECONDS 128 +#define INTERVAL_IN_MICROSECONDS 256 // TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded #define TICKS_PER_ACCELERATION 5 // lower is better From c784684deeb8709a8743894fa861e3e76fe87207 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 9 Feb 2012 21:59:42 -0600 Subject: [PATCH 52/61] Cleaned up the 4/5 axis thing some more. --- firmware/src/Motherboard/Point.hh | 6 +++++- firmware/src/Motherboard/Steppers.cc | 12 ++++++------ .../src/Motherboard/boards/rrmbv12/Configuration.hh | 2 +- firmware/src/shared/StepperAxis.cc | 5 +++-- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/firmware/src/Motherboard/Point.hh b/firmware/src/Motherboard/Point.hh index cb36dd7..21a4d55 100644 --- a/firmware/src/Motherboard/Point.hh +++ b/firmware/src/Motherboard/Point.hh @@ -5,7 +5,11 @@ #include -#define AXIS_COUNT STEPPER_COUNT +#if STEPPER_COUNT > 3 +#define AXIS_COUNT 5 +#else +#define AXIS_COUNT 3 +#endif /// Class that represents an N-dimensional point, where N is the number of /// stepper axes present in the system. Can support 3 or 5 axes. diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index c81b983..f6eb679 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -30,9 +30,9 @@ int32_t intervals; volatile int32_t intervals_remaining; struct feedrate_element { - int32_t rate; // interval value of the feedrate axis - int32_t steps; // number of steps of the master axis to change - int32_t target; + uint32_t rate; // interval value of the feedrate axis + uint32_t steps; // number of steps of the master axis to change + uint32_t target; }; feedrate_element feedrate_elements[3]; volatile int32_t feedrate_steps_remaining; @@ -255,6 +255,7 @@ bool getNextMove() { Point &target = current_block->target; + feedrate_multiplier = 1; // setTarget sets the multiplier to one int32_t max_delta = current_block->step_event_count; for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], false); @@ -417,10 +418,9 @@ bool doInterrupt() { } else { // if we are supposed to step too fast, we simulate double-size microsteps feedrate_multiplier = 1; - while (timer_counter <= -feedrate_inverted && feedrate_steps_remaining > 0) { + while (timer_counter <= -feedrate_inverted) { feedrate_multiplier++; timer_counter += feedrate_inverted; - feedrate_steps_remaining--; } for (int i = 0; i < STEPPER_COUNT; i++) { @@ -428,7 +428,7 @@ bool doInterrupt() { axes[i].doInterrupt(intervals); } - if (feedrate_steps_remaining-- <= 0) { + if ((feedrate_steps_remaining-=feedrate_multiplier) <= 0) { current_feedrate_index++; prepareFeedrateIntervals(); } diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 71a9641..9fdc42e 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -171,7 +171,7 @@ //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ #define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 15.0 // <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_XY_JERK 10.0 // <-- unused if CENTREPEDAL is defined below #define DEFAULT_MAX_Z_JERK 10.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index e5aa762..290945d 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -26,12 +26,13 @@ void StepperAxis::setTarget(const int32_t target_in, bool relative) { if (delta != 0) { interface->setEnabled(true); } + step_multiplier = 1; if (delta < 0) { delta = -delta; direction = false; - step_change = -step_multiplier; + step_change = -1; } else { - step_change = step_multiplier; + step_change = 1; } interface->setDirection(direction); } From aa7e4ecbdaaf92324b25ee387b56b76303b9d897 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 9 Feb 2012 22:00:00 -0600 Subject: [PATCH 53/61] Trying to figure out the Z-Axis overrun. --- firmware/src/Motherboard/Planner.cc | 71 ++++++++++++++--------------- 1 file changed, 33 insertions(+), 38 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index ee68093..0175425 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -78,8 +78,8 @@ #include "Steppers.hh" #include "Point.hh" -// My tests show it takes about 72ms to plan one segment on a Gen3 MB, so we need to give it room. -Rob -#define MIN_MS_PER_SEGMENT 1200 +// Give the processor some time to breathe and plan... +#define MIN_MS_PER_SEGMENT 1500 #define X_AXIS 0 #define Y_AXIS 1 @@ -251,11 +251,11 @@ namespace planner { }; #endif - PlannerAxis axes[AXIS_COUNT]; + PlannerAxis axes[STEPPER_COUNT]; float default_acceleration; Point position; // the current position (planning-wise, not bot/stepper-wise) in steps - float previous_speed[AXIS_COUNT]; // Speed of previous path line segment + float previous_speed[STEPPER_COUNT]; // Speed of previous path line segment #ifdef CENTREPEDAL float default_junction_deviation; float previous_unit_vec[3]; @@ -269,7 +269,7 @@ namespace planner { void init() { - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { // axes[i] = PlannerAxis(); // redundant, or a reset? previous_speed[i] = 0.0; } @@ -308,7 +308,7 @@ namespace planner { void setAcceleration(float new_acceleration) { default_acceleration = new_acceleration; - // for (int i = 0; i < AXIS_COUNT; i++) { + // for (int i = 0; i < STEPPER_COUNT; i++) { // axes[i].max_acceleration = default_acceleration * axes[i].steps_per_mm; // } } @@ -326,12 +326,11 @@ namespace planner { } // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the - // given acceleration. - // Rates are given pre-squared, and accelereation pre-doubled for caching in the calling subroutine. - FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) + // given acceleration: + FORCE_INLINE float estimate_acceleration_distance(float initial_rate_squared, float target_rate_squared, float acceleration_doubled) { if (acceleration_doubled!=0) { - return((target_rate_squared-initial_rate_squared)/(acceleration_doubled)); + return (int32_t)((int64_t)(target_rate_squared-initial_rate_squared)/acceleration_doubled); } else { return 0; // acceleration was 0, set acceleration distance to 0 @@ -343,14 +342,13 @@ namespace planner { // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - // Rates are given pre-squared, and accelereation pre-doubled for caching in the calling subroutine. - FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_doubled, int32_t distance) + FORCE_INLINE float intersection_distance(float initial_rate_squared, float final_rate_squared, float acceleration_doubled, float distance) { if (acceleration_doubled!=0) { - return((acceleration_doubled*distance-initial_rate_squared+final_rate_squared)/(acceleration_doubled<<1)); + return (int32_t)((int64_t)(acceleration_doubled*distance-initial_rate_squared+final_rate_squared)/(acceleration_doubled*2.0)); } else { - return 0.0; // acceleration was 0, set intersection distance to 0 + return 0; // acceleration was 0, set intersection distance to 0 } } @@ -386,23 +384,20 @@ namespace planner { local_initial_rate = 120; if(local_final_rate < 120) local_final_rate = 120; - - uint32_t local_initial_rate_squared = local_initial_rate; - local_initial_rate_squared = (local_initial_rate_squared*local_initial_rate_squared); - uint32_t local_final_rate_squared = local_final_rate; - local_final_rate_squared = (local_final_rate_squared*local_final_rate_squared); - uint32_t nominal_rate_squared = nominal_rate; - nominal_rate_squared = (nominal_rate_squared*nominal_rate_squared); - int32_t acceleration = acceleration_st; - int32_t acceleration_doubled = acceleration_st<<1; // * 2 + float local_initial_rate_squared = ((float)local_initial_rate*(float)local_initial_rate); + float local_final_rate_squared = ((float)local_final_rate *(float)local_final_rate); + float nominal_rate_squared = ((float)nominal_rate *(float)nominal_rate); + + int32_t acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 + int32_t accelerate_steps = - /*ceil*/(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, acceleration_doubled)); + ceil(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, acceleration_doubled)); int32_t decelerate_steps = - /*floor*/(estimate_acceleration_distance(nominal_rate_squared, local_final_rate_squared, -acceleration_doubled)); + floor(estimate_acceleration_distance(nominal_rate_squared, local_final_rate_squared, -acceleration_doubled)); // Calculate the size of Plateau of Nominal Rate. - int32_t plateau_steps = step_event_count-accelerate_steps-decelerate_steps; + int64_t plateau_steps = (int64_t)step_event_count-accelerate_steps-decelerate_steps; // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking @@ -626,18 +621,18 @@ namespace planner { // // store the absolute number of steps in each direction, without direction Point steps = (target - position); - float delta_mm[AXIS_COUNT]; + float delta_mm[STEPPER_COUNT]; block->millimeters = 0.0; block->step_event_count = 0; // // Compute direction bits for this block -- UNUSED FOR NOW // block->direction_bits = 0; - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { int32_t abs_steps = abs(steps[i]); if (abs_steps > block->step_event_count) { block->step_event_count = abs_steps; } delta_mm[i] = ((float)steps[i])/axes[i].steps_per_mm; - if (i < A_AXIS) + if (i < A_AXIS || block->millimeters == 0) // cound distznce of A and B only if X, Y, and Z don't move block->millimeters += delta_mm[i] * delta_mm[i]; // if (target[i] < position[i]) { block->direction_bits |= (1< max_feedrate[i]) // speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); // } @@ -705,7 +700,7 @@ namespace planner { // Compute and limit the acceleration rate for the trapezoid generator. block->acceleration_st = ceil(default_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis - for(int i=0; i < AXIS_COUNT; i++) { + for(int i=0; i < STEPPER_COUNT; i++) { if((uint32_t)((float)block->acceleration_st * (float)steps[i] / (float)block->step_event_count) > axes[i].max_acceleration) block->acceleration_st = axes[i].max_acceleration; } @@ -721,7 +716,7 @@ namespace planner { // Skip the first block if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); - if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0) || (previous_speed[Z_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } @@ -729,7 +724,7 @@ namespace planner { vmax_junction *= (max_xy_jerk/jerk); } - for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { + for (int i_axis = Z_AXIS; i_axis < STEPPER_COUNT; i_axis++) { jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); if (jerk > axes[i_axis].max_axis_jerk) { vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); @@ -777,7 +772,7 @@ namespace planner { } } - for (int i_axis = Z_AXIS; i_axis < AXIS_COUNT; i_axis++) { + for (int i_axis = Z_AXIS; i_axis < STEPPER_COUNT; i_axis++) { float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); if (jerk > axes[i_axis].max_axis_jerk) { vmax_junction *= (axes[i_axis].max_axis_jerk/jerk); @@ -839,7 +834,7 @@ namespace planner { position = steppers::getPosition(); // reset speed - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { previous_speed[i] = 0.0; } @@ -858,7 +853,7 @@ namespace planner { steppers::definePosition(new_position); // reset speed - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { previous_speed[i] = 0.0; } From 2566ac0f8661cd145b9e3f1fc3bd01ddf3cf675c Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Fri, 10 Feb 2012 01:47:04 -0600 Subject: [PATCH 54/61] Fixed arithmetic overflow errors with the Z axis. Tuned the parameters for my Cupcake some. --- firmware/src/Motherboard/Planner.cc | 119 ++++++++++++------ firmware/src/Motherboard/Planner.hh | 6 +- firmware/src/Motherboard/Steppers.cc | 23 ++-- .../boards/rrmbv12/Configuration.hh | 8 +- 4 files changed, 101 insertions(+), 55 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 0175425..e6d83ee 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -79,7 +79,7 @@ #include "Point.hh" // Give the processor some time to breathe and plan... -#define MIN_MS_PER_SEGMENT 1500 +#define MIN_MS_PER_SEGMENT 2000 #define X_AXIS 0 #define Y_AXIS 1 @@ -104,22 +104,27 @@ template inline const T& min(const T& a, const T& b) { return (a)<(b)?(a):(b); } -// template inline const int32_t& min(const int32_t&, const int32_t&); -// template inline const uint32_t& min(const uint32_t&, const uint32_t&); template inline const T& max(const T& a, const T& b) { return (a)>(b)?(a):(b); } -// template inline const int32_t& max(const int32_t&, const int32_t&); -// template inline const uint32_t& max(const uint32_t&, const uint32_t&); -// template -// inline T abs(T x) { return (x)>0?(x):-(x); } +// undefine stdlib's abs if encountered +#ifdef abs +#undef abs +#endif + +#ifdef labs +#undef labs +#endif -// #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) -// #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) -// #define radians(deg) ((deg)*DEG_TO_RAD) -// #define degrees(rad) ((rad)*RAD_TO_DEG) -// #define sq(x) ((x)*(x)) +template +inline T abs(T x) { return (x)>0?(x):-(x); } + +template <> +inline int abs(int x) { return __builtin_abs(x); } + +template <> +inline long abs(long x) { return __builtin_labs(x); } namespace planner { @@ -265,7 +270,9 @@ namespace planner { Block block_buffer_data[BLOCK_BUFFER_SIZE]; ReusingCircularBufferTempl block_buffer(BLOCK_BUFFER_SIZE, block_buffer_data); - + + // let's get verbose + volatile bool is_planning_and_using_prev_speed = false; void init() { @@ -277,16 +284,11 @@ namespace planner { position = Point(0,0,0,0,0); previous_nominal_speed = 0.0; - axes[0].max_acceleration = 3000*axes[0].steps_per_mm; - axes[1].max_acceleration = 3000*axes[1].steps_per_mm; - axes[2].max_acceleration = 200*axes[2].steps_per_mm; + axes[0].max_acceleration = 2000*axes[0].steps_per_mm; + axes[1].max_acceleration = 2000*axes[1].steps_per_mm; + axes[2].max_acceleration = 10*axes[2].steps_per_mm; axes[3].max_acceleration = 10000*axes[3].steps_per_mm; axes[4].max_acceleration = 10000*axes[4].steps_per_mm; - // axes[0].max_acceleration = 500*axes[0].steps_per_mm; - // axes[1].max_acceleration = 500*axes[1].steps_per_mm; - // axes[2].max_acceleration = 200*axes[2].steps_per_mm; - // axes[3].max_acceleration = 20000*axes[3].steps_per_mm; - // axes[4].max_acceleration = 20000*axes[4].steps_per_mm; stepperTimingDebugPin.setDirection(true); stepperTimingDebugPin.setValue(false); @@ -327,10 +329,10 @@ namespace planner { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: - FORCE_INLINE float estimate_acceleration_distance(float initial_rate_squared, float target_rate_squared, float acceleration_doubled) + FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) { if (acceleration_doubled!=0) { - return (int32_t)((int64_t)(target_rate_squared-initial_rate_squared)/acceleration_doubled); + return ((target_rate_squared-initial_rate_squared)/acceleration_doubled); } else { return 0; // acceleration was 0, set acceleration distance to 0 @@ -342,10 +344,10 @@ namespace planner { // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - FORCE_INLINE float intersection_distance(float initial_rate_squared, float final_rate_squared, float acceleration_doubled, float distance) + FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) { - if (acceleration_doubled!=0) { - return (int32_t)((int64_t)(acceleration_doubled*distance-initial_rate_squared+final_rate_squared)/(acceleration_doubled*2.0)); + if (acceleration_quadrupled!=0) { + return ((acceleration_mangled*distance-initial_rate_squared+final_rate_squared)/acceleration_quadrupled); } else { return 0; // acceleration was 0, set intersection distance to 0 @@ -385,26 +387,44 @@ namespace planner { if(local_final_rate < 120) local_final_rate = 120; - float local_initial_rate_squared = ((float)local_initial_rate*(float)local_initial_rate); - float local_final_rate_squared = ((float)local_final_rate *(float)local_final_rate); - float nominal_rate_squared = ((float)nominal_rate *(float)nominal_rate); + // If we will overflow, then throw away 4 bits of resolution. + // 0xFFFF+1 == sqrt(0xFFFFFFFF+1) + uint8_t bit_shift_amount = 0; + uint8_t bit_shift_fix_amount = 0; + // if (local_initial_rate > 0xFFFF || local_final_rate > 0xFFFF || nominal_rate > 0xFFFF) { + // bit_shift_amount = 8; + // bit_shift_fix_amount = 16; + // } + + // We use two passed for each variable, using it as a temp the first pass. + int32_t local_initial_rate_squared = local_initial_rate >> bit_shift_amount; + local_initial_rate_squared = (local_initial_rate_squared * local_initial_rate_squared); + + int32_t local_final_rate_squared = local_final_rate >> bit_shift_amount; + local_final_rate_squared = (local_final_rate_squared * local_final_rate_squared); + + int32_t nominal_rate_squared = nominal_rate >> bit_shift_amount; + nominal_rate_squared = (nominal_rate_squared * nominal_rate_squared); - int32_t acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 + int32_t local_acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 int32_t accelerate_steps = - ceil(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, acceleration_doubled)); + /*ceil*/(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, local_acceleration_doubled))<>(bit_shift_fix_amount), local_acceleration_quadrupled, step_event_count))<<(bit_shift_fix_amount); accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); plateau_steps = 0; @@ -581,13 +601,20 @@ namespace planner { } bool isBufferFull() { - // return false; return block_buffer.isFull(); } bool isBufferEmpty() { - // return false; - return block_buffer.isEmpty(); + bool is_buffer_empty = block_buffer.isEmpty(); + + // if we buffer underrun, we need to make sure the planner starts from "stopped" + if (is_buffer_empty && !is_planning_and_using_prev_speed) { + for (int i = 0; i < STEPPER_COUNT; i++) { + previous_speed[i] = 0.0; + } + previous_nominal_speed = 0.0; + } + return is_buffer_empty; } Block *getNextBlock() { @@ -701,7 +728,10 @@ namespace planner { block->acceleration_st = ceil(default_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis for(int i=0; i < STEPPER_COUNT; i++) { - if((uint32_t)((float)block->acceleration_st * (float)steps[i] / (float)block->step_event_count) > axes[i].max_acceleration) + // warning: arethmetic overflow is easy here. Try to mitigate. + float step_scale = (float)steps[i] / (float)block->step_event_count; + float axis_acceleration_st = (float)block->acceleration_st * step_scale; + if((uint32_t)axis_acceleration_st > axes[i].max_acceleration) block->acceleration_st = axes[i].max_acceleration; } block->acceleration = block->acceleration_st / steps_per_mm; @@ -715,6 +745,9 @@ namespace planner { // Now determine the safe max entry speed for this move // Skip the first block if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { + // block clearing of previous_speed + is_planning_and_using_prev_speed = true; + float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0) || (previous_speed[Z_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; @@ -768,10 +801,12 @@ namespace planner { // Compute maximum junction velocity based on maximum acceleration and junction deviation float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, - sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); + (float)sqrt(default_acceleration * default_junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } - + + // block clearing of previous_speed + is_planning_and_using_prev_speed = true; for (int i_axis = Z_AXIS; i_axis < STEPPER_COUNT; i_axis++) { float jerk = abs(previous_speed[i_axis] - current_speed[i_axis]); if (jerk > axes[i_axis].max_axis_jerk) { @@ -818,6 +853,10 @@ namespace planner { planner_recalculate(); steppers::startRunning(); + + // allow clearing of previous speed again + is_planning_and_using_prev_speed = false; + // stepperTimingDebugPin.setValue(false); return true; } diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index ea09a47..f8add1f 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -44,9 +44,9 @@ namespace planner { // Fields used by the bresenham algorithm for tracing the line Point target; // Final 5-axis target uint32_t step_event_count; // The number of step events required to complete this block - int32_t accelerate_until; // The index of the step event on which to stop acceleration - int32_t decelerate_after; // The index of the step event on which to start decelerating - int32_t acceleration_rate; // The acceleration rate used for acceleration calculation + uint32_t accelerate_until; // The index of the step event on which to stop acceleration + uint32_t decelerate_after; // The index of the step event on which to start decelerating + uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation // uint8_t direction_bits; // The direction bit set for this block // uint8_t active_extruder; // Selects the active extruder diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index f6eb679..72d8f6c 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -124,7 +124,7 @@ void setHoldZ(bool holdZ_in) { #if 0 void setTarget(const Point& target, int32_t dda_interval) { int32_t max_delta = 0; - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], false); const int32_t delta = axes[i].delta; // Only shut z axis on inactivity @@ -171,7 +171,7 @@ void setTarget(const Point& target, int32_t dda_interval) { intervals = max_delta; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < ALL_AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; @@ -179,7 +179,7 @@ void setTarget(const Point& target, int32_t dda_interval) { /* void setTargetNew(const Point& target, int32_t us, uint8_t relative) { -for (int i = 0; i < AXIS_COUNT; i++) { +for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].setTarget(target[i], (relative & (1 << i)) != 0); // Only shut z axis on inactivity const int32_t delta = axes[i].delta; @@ -193,7 +193,7 @@ axes[i].enableStepper(true); intervals = us / INTERVAL_IN_MICROSECONDS; intervals_remaining = intervals; const int32_t negative_half_interval = -intervals / 2; -for (int i = 0; i < AXIS_COUNT; i++) { +for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; } is_running = true; @@ -244,8 +244,8 @@ bool getNextMove() { } if (planner::isBufferEmpty()) { - stepperTimingDebugPin.setValue(true); - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); return false; } @@ -292,7 +292,12 @@ bool getNextMove() { feedrate_elements[feedrate_being_setup].rate = 0; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - } + }/* + else { + stepperTimingDebugPin.setValue(true); + stepperTimingDebugPin.setValue(false); + }*/ + // setup deceleration if (current_block->decelerate_after < current_block->step_event_count) { @@ -378,7 +383,7 @@ void startHoming(const bool maximums, const uint8_t axes_enabled, const uint32_t intervals_remaining = INT32_MAX; intervals = us_per_step / INTERVAL_IN_MICROSECONDS; const int32_t negative_half_interval = -(intervals>>1); - for (int i = 0; i < AXIS_COUNT; i++) { + for (int i = 0; i < STEPPER_COUNT; i++) { axes[i].counter = negative_half_interval; if ((axes_enabled & (1< Date: Sat, 11 Feb 2012 01:47:34 -0600 Subject: [PATCH 55/61] Can now update plan of currently executing segment. --- firmware/src/Motherboard/Planner.cc | 76 +++++++++++++--------- firmware/src/Motherboard/Steppers.cc | 97 ++++++++++++++++++---------- firmware/src/Motherboard/Steppers.hh | 1 + 3 files changed, 109 insertions(+), 65 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index e6d83ee..13601d7 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -144,12 +144,13 @@ namespace planner { private: volatile BufSizeType head, tail; + volatile bool full; BufSizeType size; BufSizeType size_mask; BufDataType* const data; /// Pointer to buffer data public: - ReusingCircularBufferTempl(BufSizeType size_in, BufDataType* buffer_in) : head(0), tail(0), size(size_in), size_mask(size_in-1), data(buffer_in) { + ReusingCircularBufferTempl(BufSizeType size_in, BufDataType* buffer_in) : head(0), tail(0), full(false), size(size_in), size_mask(size_in-1), data(buffer_in) { for (BufSizeType i = 0; i < size; i++) { data[i] = BufDataType(); } @@ -191,29 +192,33 @@ namespace planner { // WARNING: no sanity checks! inline void bumpHead() { head = getNextIndex(head); + if (getNextIndex(head) == tail) + full = true; } // bump the tail with buffer--. cannot return anything useful, so it doesn't // WARNING: no sanity checks! inline void bumpTail() { tail = getNextIndex(tail); + full = false; } inline bool isEmpty() { - return head == tail; + return !full && head == tail; } inline bool isFull() { - return getNextIndex(head) == tail; + return full; } inline BufSizeType getUsedCount() { - return ((head-tail+size) & size_mask); + return full ? size : ((head-tail+size) & size_mask); } inline void clear() { head = 0; tail = 0; + full = false; } }; @@ -277,7 +282,6 @@ namespace planner { void init() { for (int i = 0; i < STEPPER_COUNT; i++) { - // axes[i] = PlannerAxis(); // redundant, or a reset? previous_speed[i] = 0.0; } @@ -292,7 +296,14 @@ namespace planner { stepperTimingDebugPin.setDirection(true); stepperTimingDebugPin.setValue(false); + + block_buffer.clear(); +#ifdef CENTREPEDAL + previous_unit_vec[0]= 0.0; + previous_unit_vec[1]= 0.0; + previous_unit_vec[2]= 0.0; +#endif } @@ -373,7 +384,7 @@ namespace planner { // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. // calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, exit_factor_speed/block->nominal_speed); void Block::calculate_trapezoid(float exit_factor_speed) { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); float entry_factor = entry_speed/nominal_speed; float exit_factor = exit_factor_speed/nominal_speed; @@ -395,7 +406,7 @@ namespace planner { // bit_shift_amount = 8; // bit_shift_fix_amount = 16; // } - + // // We use two passed for each variable, using it as a temp the first pass. int32_t local_initial_rate_squared = local_initial_rate >> bit_shift_amount; local_initial_rate_squared = (local_initial_rate_squared * local_initial_rate_squared); @@ -431,17 +442,17 @@ namespace planner { } ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { // Fill variables used by the stepper in a critical section - if(!(flags & Block::Busy)) { + // if(!(flags & Block::Busy)) { accelerate_until = accelerate_steps; decelerate_after = accelerate_steps+plateau_steps; initial_rate = local_initial_rate; final_rate = local_final_rate; - } - // if(flags & Block::Busy) - // steppers::currentBlockChanged(); + // } + if(flags & Block::Busy) + steppers::currentBlockChanged(); } // ISR state will be automatically restored here - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); } // forward declare, so we can order the code in a slightly more readable fashion @@ -527,13 +538,18 @@ namespace planner { // We have to be careful here, but we want to try to smooth out the movement if it's not too late. // That smoothing will happen in Block::calculate_trapezoid later. // However, if it *is* too late, then we need to fix the current entry speed. - // if (previous->flags & Block::Busy) { - // uint32_t current_step = steppers::getCurrentStep(); - // // If we are withing 10 steps, we're probably too late - // // if (current_step > (previous->decelerate_after - 10)) { - // // current->entry_speed = MINIMUM_PLANNER_SPEED; - // // } - // } + if (previous->flags & Block::Busy) { + stepperTimingDebugPin.setValue(true); + uint32_t current_step = steppers::getCurrentStep(); + uint32_t current_feedrate = steppers::getCurrentFeedrate(); + // current_feedrate is in steps/second, but entry_speed is in mm/s + float current_speed = (float)current_feedrate * (previous->millimeters/(float)previous->step_event_count); + + if (current_speed < current->entry_speed && current_step > previous->decelerate_after) { + current->entry_speed = current_speed; + } + stepperTimingDebugPin.setValue(false); + } // If the previous block is an acceleration block, but it is not long enough to complete the // full speed change within the block, we need to adjust the entry speed accordingly. Entry @@ -594,10 +610,8 @@ namespace planner { } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - if(next != NULL) { - next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); - next->flags &= ~Block::Recalculate; - } + next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + next->flags &= ~Block::Recalculate; } bool isBufferFull() { @@ -634,7 +648,7 @@ namespace planner { // stepperTimingDebugPin.setValue(true); if (block_buffer.isFull()) { // stepperTimingDebugPin.setValue(true); - // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); return false; // stepperTimingDebugPin.setValue(false); } @@ -744,12 +758,12 @@ namespace planner { // Now determine the safe max entry speed for this move // Skip the first block + is_planning_and_using_prev_speed = true; if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { // block clearing of previous_speed - is_planning_and_using_prev_speed = true; float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); - if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0) || (previous_speed[Z_AXIS] != 0.0)) { + if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } @@ -842,6 +856,9 @@ namespace planner { memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; + // allow clearing of previous speed again + is_planning_and_using_prev_speed = false; + // block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); // Update position @@ -854,9 +871,6 @@ namespace planner { steppers::startRunning(); - // allow clearing of previous speed again - is_planning_and_using_prev_speed = false; - // stepperTimingDebugPin.setValue(false); return true; } @@ -876,6 +890,7 @@ namespace planner { for (int i = 0; i < STEPPER_COUNT; i++) { previous_speed[i] = 0.0; } + previous_nominal_speed = 0.0; block_buffer.clear(); @@ -895,7 +910,8 @@ namespace planner { for (int i = 0; i < STEPPER_COUNT; i++) { previous_speed[i] = 0.0; } - + previous_nominal_speed = 0.0; + #ifdef CENTREPEDAL previous_unit_vec[0]= 0.0; previous_unit_vec[1]= 0.0; diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 72d8f6c..84c84f0 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -62,6 +62,7 @@ bool isRunning() { //public: void init(Motherboard& motherboard) { is_running = false; + is_homing = false; for (int i = 0; i < STEPPER_COUNT; i++) { axes[i] = StepperAxis(motherboard.getStepperInterface(i)); } @@ -75,8 +76,14 @@ void init(Motherboard& motherboard) { feedrate_elements[i].target = 0; feedrate_elements[i].steps = 0; } - // feedrate_intervals_remaining = 0; + + feedrate_steps_remaining = 0; feedrate = 0; + feedrate_inverted = 0; + feedrate_dirty = 1; + feedrate_multiplier = 1; + acceleration_tick_counter = 0; + current_feedrate_index = 0; stepperTimingDebugPin.setDirection(true); stepperTimingDebugPin.setValue(false); @@ -95,6 +102,8 @@ void abort() { feedrate_inverted = 0; feedrate_dirty = 1; feedrate_multiplier = 1; + acceleration_tick_counter = 0; + current_feedrate_index = 0; } /// Define current position as given point @@ -202,6 +211,8 @@ is_running = true; #endif inline void prepareFeedrateIntervals() { + if (current_feedrate_index > 2) + return; feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; feedrate_changerate = feedrate_elements[current_feedrate_index].rate; feedrate_target = feedrate_elements[current_feedrate_index].target; @@ -210,6 +221,8 @@ inline void prepareFeedrateIntervals() { } inline void recalcFeedrate() { + if (feedrate == 0) + return; // SHRIEK! feedrate_inverted = 1000000/feedrate; // // if we are supposed to step too fast, we simulate double-size microsteps @@ -230,11 +243,14 @@ uint32_t getCurrentStep() { return intervals - intervals_remaining; } +uint32_t getCurrentFeedrate() { + return feedrate; +} // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { - // stepperTimingDebugPin.setValue(true); + stepperTimingDebugPin.setValue(true); is_running = false; // this ensures that the interrupt does not .. interrupt us if (current_block != NULL) { @@ -285,36 +301,39 @@ bool getNextMove() { // setup plateau if (current_block->decelerate_after > current_block->accelerate_until) { - if (feedrate == 0) + if (feedrate_being_setup == 0) feedrate = current_block->nominal_rate; feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; feedrate_elements[feedrate_being_setup].rate = 0; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - }/* - else { - stepperTimingDebugPin.setValue(true); - stepperTimingDebugPin.setValue(false); - }*/ + } // setup deceleration if (current_block->decelerate_after < current_block->step_event_count) { - if (feedrate == 0) + if (feedrate_being_setup == 0) feedrate = current_block->nominal_rate; - feedrate_elements[feedrate_being_setup].steps = current_block->step_event_count - current_block->decelerate_after; + // To prevent "falling off the end" we will say we have a "bazillion" steps left... + feedrate_elements[feedrate_being_setup].steps = INT32_MAX; //current_block->step_event_count - current_block->decelerate_after; feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; feedrate_elements[feedrate_being_setup].target = current_block->final_rate; + } else { + // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... + feedrate_elements[feedrate_being_setup-1].steps = INT32_MAX; + // We don't setup anything else because we limit to the target speed anyway. } - + + if (feedrate == 0) + feedrate = 10; // well, it's gotta be something! + prepareFeedrateIntervals(); recalcFeedrate(); - acceleration_tick_counter = TICKS_PER_ACCELERATION; - - // We use += here so that the odd rounded-off time from the last move is still waited out - timer_counter += feedrate_inverted; + // acceleration_tick_counter = TICKS_PER_ACCELERATION; + + timer_counter = feedrate_inverted; intervals = max_delta; intervals_remaining = intervals; @@ -324,11 +343,12 @@ bool getNextMove() { } is_running = true; - // stepperTimingDebugPin.setValue(false); + stepperTimingDebugPin.setValue(false); return true; } void currentBlockChanged() { + stepperTimingDebugPin.setValue(true); // If we are here, then we are moving AND the interrupts are frozen, so get out *fast* uint32_t current_step = intervals - intervals_remaining; @@ -337,45 +357,52 @@ void currentBlockChanged() { // setup acceleration feedrate = 0; if (current_block->accelerate_until > current_step) { - feedrate = current_block->initial_rate; - - feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until - current_step; feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - } else { - // reclac current_step to be the current step inside this submovement - current_step -= current_block->accelerate_until; + + feedrate = ((current_block->nominal_rate+current_block->initial_rate)*current_step)/current_block->accelerate_until; } // setup plateau if (current_block->decelerate_after > current_block->accelerate_until && current_block->decelerate_after > current_step) { - if (feedrate == 0) - feedrate = current_block->nominal_rate; - feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; feedrate_elements[feedrate_being_setup].rate = 0; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - } else { - // reclac current_step to be the current step inside this submovement - current_step -= current_block->decelerate_after - current_block->accelerate_until; + + if (feedrate_being_setup == 0) { + feedrate = current_block->nominal_rate; + feedrate_elements[feedrate_being_setup].steps -= current_step; + } } // setup deceleration if (current_block->decelerate_after < current_block->step_event_count) { - if (feedrate == 0) - feedrate = current_block->nominal_rate; - - feedrate_elements[feedrate_being_setup].steps = current_block->step_event_count - current_block->decelerate_after; + stepperTimingDebugPin.setValue(false); + stepperTimingDebugPin.setValue(true); + // To prevent "falling off the end" we will say we have a "bazillion" steps left... + feedrate_elements[feedrate_being_setup].steps = INT32_MAX; //current_block->step_event_count - current_block->decelerate_after; feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; feedrate_elements[feedrate_being_setup].target = current_block->final_rate; + + if (feedrate_being_setup == 0) { + feedrate = ((current_block->final_rate+current_block->nominal_rate)*(current_step - current_block->decelerate_after))/(current_block->step_event_count - current_block->decelerate_after); + } + } else { + // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... + feedrate_elements[feedrate_being_setup-1].steps = INT32_MAX; } prepareFeedrateIntervals(); - // remove the amount of steps we've already taken... - feedrate_steps_remaining -= current_step; recalcFeedrate(); + + timer_counter = feedrate_inverted; + + // the steppers themselves havne't changed... + + stepperTimingDebugPin.setValue(false); } /// Start homing @@ -451,7 +478,7 @@ bool doInterrupt() { } } - if (feedrate_changerate != 0 && acceleration_tick_counter-- == 0) { + if (feedrate_changerate != 0 && acceleration_tick_counter-- <= 0) { acceleration_tick_counter = TICKS_PER_ACCELERATION; // Change our feedrate. Here it's important to note that we can over/undershoot // To handle this, if we're accelerating, we simply clamp to max speed. diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index 2d2b732..061f7b2 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -98,6 +98,7 @@ namespace steppers { void currentBlockChanged(); uint32_t getCurrentStep(); + uint32_t getCurrentFeedrate(); }; #endif // STEPPERS_HH_ From 3235ddc4986d8c422d460e110af43e77cd63c022 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 11 Feb 2012 02:01:39 -0600 Subject: [PATCH 56/61] Set initial feedrate to 10, and fixed feedrate math. Fixed the math used for when a block is replanted. It uses sort, though, which I don't like. Needs optimized. --- firmware/src/Motherboard/Planner.cc | 2 +- firmware/src/Motherboard/Steppers.cc | 23 +++++++++++------------ 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 13601d7..00143de 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -285,7 +285,7 @@ namespace planner { previous_speed[i] = 0.0; } - position = Point(0,0,0,0,0); + position = steppers::getPosition(); previous_nominal_speed = 0.0; axes[0].max_acceleration = 2000*axes[0].steps_per_mm; diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 84c84f0..a133f13 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -94,9 +94,6 @@ void abort() { is_homing = false; timer_counter = 0; current_block = NULL; - // feedrate_scale_shift = 0; - // feedrate_intervals = 0; - // feedrate_intervals_remaining = 0; feedrate_steps_remaining = 0; feedrate = 0; feedrate_inverted = 0; @@ -326,12 +323,12 @@ bool getNextMove() { // We don't setup anything else because we limit to the target speed anyway. } - if (feedrate == 0) - feedrate = 10; // well, it's gotta be something! + // if (feedrate == 0) + // feedrate = 10; // well, it's gotta be something! prepareFeedrateIntervals(); recalcFeedrate(); - // acceleration_tick_counter = TICKS_PER_ACCELERATION; + acceleration_tick_counter = TICKS_PER_ACCELERATION; timer_counter = feedrate_inverted; @@ -362,7 +359,10 @@ void currentBlockChanged() { feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - feedrate = ((current_block->nominal_rate+current_block->initial_rate)*current_step)/current_block->accelerate_until; + // use linear interpolation -- wrong, but close enough + // feedrate = ((current_block->nominal_rate+current_block->initial_rate)*current_step)/current_block->accelerate_until; + // // Sqrt[2 a d + s^2] + // feedrate = sqrt(2*current_block->acceleration_rate*current_step + current_block->initial_rate*current_block->initial_rate); } // setup plateau @@ -388,7 +388,10 @@ void currentBlockChanged() { feedrate_elements[feedrate_being_setup].target = current_block->final_rate; if (feedrate_being_setup == 0) { - feedrate = ((current_block->final_rate+current_block->nominal_rate)*(current_step - current_block->decelerate_after))/(current_block->step_event_count - current_block->decelerate_after); + // use linear interpolation -- wrong, but close enough + // feedrate = ((current_block->final_rate+current_block->nominal_rate)*(current_step - current_block->decelerate_after))/(current_block->step_event_count - current_block->decelerate_after); + // // Sqrt[2 a d + s^2] + // feedrate = sqrt(-2*current_block->acceleration_rate*(current_step - current_block->decelerate_after) + current_block->nominal_rate*current_block->nominal_rate); } } else { // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... @@ -481,10 +484,6 @@ bool doInterrupt() { if (feedrate_changerate != 0 && acceleration_tick_counter-- <= 0) { acceleration_tick_counter = TICKS_PER_ACCELERATION; // Change our feedrate. Here it's important to note that we can over/undershoot - // To handle this, if we're accelerating, we simply clamp to max speed. - // If we are decelerating, we have to be more careful. - // But for now, we don't allow decelrating to a "stop" so we punt and just don't - // allow it to go under the set speed. feedrate += feedrate_changerate; feedrate_dirty = 1; From 7418f9e999b40119d710d706ad5ec7c0debaba7a Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sat, 11 Feb 2012 23:28:27 -0600 Subject: [PATCH 57/61] Fixed first-movevment-after-startup-not-working bug. --- firmware/src/Motherboard/Main.cc | 2 +- firmware/src/Motherboard/Planner.cc | 48 +++++++++++++++------------- firmware/src/Motherboard/Steppers.cc | 48 ++++++++++++---------------- 3 files changed, 47 insertions(+), 51 deletions(-) diff --git a/firmware/src/Motherboard/Main.cc b/firmware/src/Motherboard/Main.cc index 37d53ff..4228a16 100644 --- a/firmware/src/Motherboard/Main.cc +++ b/firmware/src/Motherboard/Main.cc @@ -34,7 +34,7 @@ void reset(bool hard_reset) { ATOMIC_BLOCK(ATOMIC_FORCEON) { Motherboard& board = Motherboard::getBoard(); sdcard::reset(); - steppers::abort(); + planner::abort(); // calls steppers::abort() command::reset(); eeprom::init(); board.reset(); diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 00143de..677923d 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -79,7 +79,7 @@ #include "Point.hh" // Give the processor some time to breathe and plan... -#define MIN_MS_PER_SEGMENT 2000 +#define MIN_MS_PER_SEGMENT 6000 #define X_AXIS 0 #define Y_AXIS 1 @@ -281,12 +281,7 @@ namespace planner { void init() { - for (int i = 0; i < STEPPER_COUNT; i++) { - previous_speed[i] = 0.0; - } - - position = steppers::getPosition(); - previous_nominal_speed = 0.0; + abort(); axes[0].max_acceleration = 2000*axes[0].steps_per_mm; axes[1].max_acceleration = 2000*axes[1].steps_per_mm; @@ -296,8 +291,6 @@ namespace planner { stepperTimingDebugPin.setDirection(true); stepperTimingDebugPin.setValue(false); - - block_buffer.clear(); #ifdef CENTREPEDAL previous_unit_vec[0]= 0.0; @@ -340,10 +333,10 @@ namespace planner { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: - FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) + FORCE_INLINE int32_t estimate_acceleration_distance(int64_t initial_rate_squared, int64_t target_rate_squared, int32_t acceleration_doubled) { if (acceleration_doubled!=0) { - return ((target_rate_squared-initial_rate_squared)/acceleration_doubled); + return (((int64_t)target_rate_squared-(int64_t)initial_rate_squared)/acceleration_doubled); } else { return 0; // acceleration was 0, set acceleration distance to 0 @@ -355,10 +348,10 @@ namespace planner { // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) + FORCE_INLINE int32_t intersection_distance(int64_t initial_rate_squared, int64_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) { if (acceleration_quadrupled!=0) { - return ((acceleration_mangled*distance-initial_rate_squared+final_rate_squared)/acceleration_quadrupled); + return (((int64_t)acceleration_mangled*(int64_t)distance-(int64_t)initial_rate_squared+(int64_t)final_rate_squared)/acceleration_quadrupled); } else { return 0; // acceleration was 0, set intersection distance to 0 @@ -408,13 +401,13 @@ namespace planner { // } // // We use two passed for each variable, using it as a temp the first pass. - int32_t local_initial_rate_squared = local_initial_rate >> bit_shift_amount; + int64_t local_initial_rate_squared = local_initial_rate >> bit_shift_amount; local_initial_rate_squared = (local_initial_rate_squared * local_initial_rate_squared); - int32_t local_final_rate_squared = local_final_rate >> bit_shift_amount; + int64_t local_final_rate_squared = local_final_rate >> bit_shift_amount; local_final_rate_squared = (local_final_rate_squared * local_final_rate_squared); - int32_t nominal_rate_squared = nominal_rate >> bit_shift_amount; + int64_t nominal_rate_squared = nominal_rate >> bit_shift_amount; nominal_rate_squared = (nominal_rate_squared * nominal_rate_squared); int32_t local_acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 @@ -543,11 +536,18 @@ namespace planner { uint32_t current_step = steppers::getCurrentStep(); uint32_t current_feedrate = steppers::getCurrentFeedrate(); // current_feedrate is in steps/second, but entry_speed is in mm/s - float current_speed = (float)current_feedrate * (previous->millimeters/(float)previous->step_event_count); + // use the ratio of nominal_speed/nominal_rate to figure the current speed + float current_speed = ((float)current_feedrate * previous->nominal_speed)/(float)previous->nominal_rate; - if (current_speed < current->entry_speed && current_step > previous->decelerate_after) { - current->entry_speed = current_speed; - } + // adjust the previous block to just cover the space left, and firect recalculation + previous->entry_speed = previous->max_entry_speed = current_speed; + + // Recalculate the length of the movement -- for acceleration only. + // The Stepper/Axis objects have track of actual movement length by now. + previous->step_event_count = previous->step_event_count - current_step; + previous->flags |= Block::Recalculate; + // assume it's not nominal length, to be safe + previous->flags &= ~Block::NominalLength; stepperTimingDebugPin.setValue(false); } @@ -679,6 +679,9 @@ namespace planner { } block->millimeters = sqrt(block->millimeters); + if (block->step_event_count == 0) + return false; + // CLEAN ME: Ugly dirty check to prevent a lot of small moves from causing a planner buffer underrun // For now, we'll just make sure each movement takes at least MIN_MS_PER_SEGMENT millisesconds to complete if ((us_per_step * block->step_event_count) < MIN_MS_PER_SEGMENT) { @@ -756,11 +759,12 @@ namespace planner { // Start with a safe speed float vmax_junction = MINIMUM_PLANNER_SPEED; + // block clearing of previous_speed + is_planning_and_using_prev_speed = true; + // Now determine the safe max entry speed for this move // Skip the first block - is_planning_and_using_prev_speed = true; if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { - // block clearing of previous_speed float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index a133f13..5d76947 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -213,7 +213,7 @@ inline void prepareFeedrateIntervals() { feedrate_steps_remaining = feedrate_elements[current_feedrate_index].steps; feedrate_changerate = feedrate_elements[current_feedrate_index].rate; feedrate_target = feedrate_elements[current_feedrate_index].target; - feedrate_dirty = 1; + // feedrate_dirty = 1; // acceleration_tick_counter = 0; } @@ -247,7 +247,7 @@ uint32_t getCurrentFeedrate() { // load up the next movment // WARNING: called from inside the ISR, so get out fast bool getNextMove() { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); is_running = false; // this ensures that the interrupt does not .. interrupt us if (current_block != NULL) { @@ -323,8 +323,10 @@ bool getNextMove() { // We don't setup anything else because we limit to the target speed anyway. } - // if (feedrate == 0) - // feedrate = 10; // well, it's gotta be something! + if (feedrate == 0) { + is_running = false; + return false; + } prepareFeedrateIntervals(); recalcFeedrate(); @@ -340,62 +342,52 @@ bool getNextMove() { } is_running = true; - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); return true; } void currentBlockChanged() { stepperTimingDebugPin.setValue(true); // If we are here, then we are moving AND the interrupts are frozen, so get out *fast* - uint32_t current_step = intervals - intervals_remaining; current_feedrate_index = 0; int feedrate_being_setup = 0; // setup acceleration feedrate = 0; - if (current_block->accelerate_until > current_step) { - feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until - current_step; + if (current_block->accelerate_until > 0) { + feedrate = current_block->initial_rate; + + feedrate_elements[feedrate_being_setup].steps = current_block->accelerate_until; feedrate_elements[feedrate_being_setup].rate = current_block->acceleration_rate; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - - // use linear interpolation -- wrong, but close enough - // feedrate = ((current_block->nominal_rate+current_block->initial_rate)*current_step)/current_block->accelerate_until; - // // Sqrt[2 a d + s^2] - // feedrate = sqrt(2*current_block->acceleration_rate*current_step + current_block->initial_rate*current_block->initial_rate); } // setup plateau - if (current_block->decelerate_after > current_block->accelerate_until && current_block->decelerate_after > current_step) { + if (current_block->decelerate_after > current_block->accelerate_until) { + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + feedrate_elements[feedrate_being_setup].steps = current_block->decelerate_after - current_block->accelerate_until; feedrate_elements[feedrate_being_setup].rate = 0; feedrate_elements[feedrate_being_setup].target = current_block->nominal_rate; feedrate_being_setup++; - - if (feedrate_being_setup == 0) { - feedrate = current_block->nominal_rate; - feedrate_elements[feedrate_being_setup].steps -= current_step; - } } + // setup deceleration if (current_block->decelerate_after < current_block->step_event_count) { - stepperTimingDebugPin.setValue(false); - stepperTimingDebugPin.setValue(true); + if (feedrate_being_setup == 0) + feedrate = current_block->nominal_rate; + // To prevent "falling off the end" we will say we have a "bazillion" steps left... feedrate_elements[feedrate_being_setup].steps = INT32_MAX; //current_block->step_event_count - current_block->decelerate_after; feedrate_elements[feedrate_being_setup].rate = -current_block->acceleration_rate; feedrate_elements[feedrate_being_setup].target = current_block->final_rate; - - if (feedrate_being_setup == 0) { - // use linear interpolation -- wrong, but close enough - // feedrate = ((current_block->final_rate+current_block->nominal_rate)*(current_step - current_block->decelerate_after))/(current_block->step_event_count - current_block->decelerate_after); - // // Sqrt[2 a d + s^2] - // feedrate = sqrt(-2*current_block->acceleration_rate*(current_step - current_block->decelerate_after) + current_block->nominal_rate*current_block->nominal_rate); - } } else { // and in case there wasn't a deceleration phase, we'll do the same for whichever phase was last... feedrate_elements[feedrate_being_setup-1].steps = INT32_MAX; + // We don't setup anything else because we limit to the target speed anyway. } prepareFeedrateIntervals(); From c8da14f3a95765b9fb70a6b17416f2380163b703 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sun, 12 Feb 2012 00:36:49 -0600 Subject: [PATCH 58/61] Fixed some errors with overflow correction. --- firmware/src/Motherboard/Planner.cc | 37 +++++++++-------------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 677923d..c949058 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -79,7 +79,7 @@ #include "Point.hh" // Give the processor some time to breathe and plan... -#define MIN_MS_PER_SEGMENT 6000 +#define MIN_MS_PER_SEGMENT 12000 #define X_AXIS 0 #define Y_AXIS 1 @@ -333,10 +333,10 @@ namespace planner { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: - FORCE_INLINE int32_t estimate_acceleration_distance(int64_t initial_rate_squared, int64_t target_rate_squared, int32_t acceleration_doubled) + FORCE_INLINE int32_t estimate_acceleration_distance(int32_t initial_rate_squared, int32_t target_rate_squared, int32_t acceleration_doubled) { if (acceleration_doubled!=0) { - return (((int64_t)target_rate_squared-(int64_t)initial_rate_squared)/acceleration_doubled); + return ((target_rate_squared-initial_rate_squared)/acceleration_doubled); } else { return 0; // acceleration was 0, set acceleration distance to 0 @@ -348,10 +348,10 @@ namespace planner { // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) - FORCE_INLINE int32_t intersection_distance(int64_t initial_rate_squared, int64_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) + FORCE_INLINE int32_t intersection_distance(int32_t initial_rate_squared, int32_t final_rate_squared, int32_t acceleration_mangled, int32_t acceleration_quadrupled, int32_t distance) { if (acceleration_quadrupled!=0) { - return (((int64_t)acceleration_mangled*(int64_t)distance-(int64_t)initial_rate_squared+(int64_t)final_rate_squared)/acceleration_quadrupled); + return ((acceleration_mangled*distance-initial_rate_squared+final_rate_squared)/acceleration_quadrupled); } else { return 0; // acceleration was 0, set intersection distance to 0 @@ -391,31 +391,16 @@ namespace planner { if(local_final_rate < 120) local_final_rate = 120; - // If we will overflow, then throw away 4 bits of resolution. - // 0xFFFF+1 == sqrt(0xFFFFFFFF+1) - uint8_t bit_shift_amount = 0; - uint8_t bit_shift_fix_amount = 0; - // if (local_initial_rate > 0xFFFF || local_final_rate > 0xFFFF || nominal_rate > 0xFFFF) { - // bit_shift_amount = 8; - // bit_shift_fix_amount = 16; - // } - // - // We use two passed for each variable, using it as a temp the first pass. - int64_t local_initial_rate_squared = local_initial_rate >> bit_shift_amount; - local_initial_rate_squared = (local_initial_rate_squared * local_initial_rate_squared); - - int64_t local_final_rate_squared = local_final_rate >> bit_shift_amount; - local_final_rate_squared = (local_final_rate_squared * local_final_rate_squared); - - int64_t nominal_rate_squared = nominal_rate >> bit_shift_amount; - nominal_rate_squared = (nominal_rate_squared * nominal_rate_squared); + int32_t local_initial_rate_squared = (local_initial_rate * local_initial_rate); + int32_t local_final_rate_squared = (local_final_rate * local_final_rate); + int32_t nominal_rate_squared = (nominal_rate * nominal_rate); int32_t local_acceleration_doubled = acceleration_st<<(1); // == acceleration_st*2 int32_t accelerate_steps = - /*ceil*/(estimate_acceleration_distance(local_initial_rate_squared, nominal_rate_squared, local_acceleration_doubled))<>(bit_shift_fix_amount), local_acceleration_quadrupled, step_event_count))<<(bit_shift_fix_amount); + intersection_distance(local_initial_rate_squared, local_final_rate_squared, local_acceleration_doubled, local_acceleration_quadrupled, step_event_count)); accelerate_steps = max(accelerate_steps, 0L); // Check limits due to numerical round-off accelerate_steps = min(accelerate_steps, (int32_t)step_event_count); plateau_steps = 0; From 26c39651eed30ce06e8f2a1c5ed6f3d35eb8833a Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Sun, 12 Feb 2012 01:27:49 -0600 Subject: [PATCH 59/61] Now, it's usable. --- firmware/src/Motherboard/Planner.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index c949058..5703425 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -516,7 +516,7 @@ namespace planner { // We have to be careful here, but we want to try to smooth out the movement if it's not too late. // That smoothing will happen in Block::calculate_trapezoid later. // However, if it *is* too late, then we need to fix the current entry speed. - if (previous->flags & Block::Busy) { + if (previous->flags & Block::Busy && current->flags & Block::Recalculate) { stepperTimingDebugPin.setValue(true); uint32_t current_step = steppers::getCurrentStep(); uint32_t current_feedrate = steppers::getCurrentFeedrate(); @@ -730,7 +730,7 @@ namespace planner { block->acceleration_st = ceil(default_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis for(int i=0; i < STEPPER_COUNT; i++) { - // warning: arethmetic overflow is easy here. Try to mitigate. + // warning: arithmetic overflow is easy here. Try to mitigate. float step_scale = (float)steps[i] / (float)block->step_event_count; float axis_acceleration_st = (float)block->acceleration_st * step_scale; if((uint32_t)axis_acceleration_st > axes[i].max_acceleration) From eb383354ad28167d2ac3fdbc8cee267b50d66959 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Thu, 16 Feb 2012 22:35:15 -0600 Subject: [PATCH 60/61] Eeprom cleanup, and Gen4 support. --- firmware/src/Motherboard/Command.cc | 56 +++++++++-------- firmware/src/Motherboard/EepromMap.hh | 36 ++++++++--- firmware/src/Motherboard/Main.cc | 4 +- firmware/src/Motherboard/Planner.cc | 56 +++++++++-------- firmware/src/Motherboard/Planner.hh | 5 +- firmware/src/Motherboard/Steppers.cc | 16 ++--- .../Motherboard/boards/mb24/Configuration.hh | 23 +++++-- .../Motherboard/boards/mb24/Motherboard.cc | 42 +++++++++++++ .../boards/rrmbv12/Configuration.hh | 2 +- .../Motherboard/boards/rrmbv12/Motherboard.cc | 62 +++++++++++++------ firmware/src/shared/Eeprom.cc | 12 ++++ firmware/src/shared/Eeprom.hh | 2 + firmware/src/shared/Menu.cc | 3 +- 13 files changed, 223 insertions(+), 96 deletions(-) diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index fbb24fa..b3bd1e7 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -124,18 +124,20 @@ static void handleMovementCommand(const uint8_t &command) { // stepperTimingDebugPin.setValue(false); return; // we'll be back! } + /* if (command == HOST_CMD_QUEUE_POINT_ABS) { - // check for completion - if (command_buffer.getLength() >= 17) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t dda = pop32(); - // planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA - } - } else if (command == HOST_CMD_QUEUE_POINT_EXT) { + // check for completion + if (command_buffer.getLength() >= 17) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t dda = pop32(); + // planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA + } + } else */ + if (command == HOST_CMD_QUEUE_POINT_EXT) { // check for completion if (command_buffer.getLength() >= 25) { command_buffer.pop(); // remove the command code @@ -148,21 +150,23 @@ static void handleMovementCommand(const uint8_t &command) { int32_t dda = pop32(); planner::addMoveToBuffer(Point(x,y,z,a,b), dda); } - } else if (command == HOST_CMD_QUEUE_POINT_NEW) { - // check for completion - if (command_buffer.getLength() >= 26) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); - int32_t us = pop32(); - uint8_t relative = pop8(); - //steppers::setTargetNew(Point(x,y,z,a,b),us,relative); - } - } + }/* + else if (command == HOST_CMD_QUEUE_POINT_NEW) { + // check for completion + if (command_buffer.getLength() >= 26) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t us = pop32(); + uint8_t relative = pop8(); + //steppers::setTargetNew(Point(x,y,z,a,b),us,relative); + } + }*/ + } // A fast slice for processing commands and refilling the stepper queue, etc. diff --git a/firmware/src/Motherboard/EepromMap.hh b/firmware/src/Motherboard/EepromMap.hh index 26b4f5e..7ce0321 100644 --- a/firmware/src/Motherboard/EepromMap.hh +++ b/firmware/src/Motherboard/EepromMap.hh @@ -23,32 +23,50 @@ namespace eeprom { -const static uint16_t EEPROM_SIZE = 0x0200; +const static uint16_t EEPROM_SIZE = 0x0200; /// Version, low byte: 1 byte -const static uint16_t VERSION_LOW = 0x0000; +const static uint16_t VERSION_LOW = 0x0000; /// Version, high byte: 1 byte -const static uint16_t VERSION_HIGH = 0x0001; +const static uint16_t VERSION_HIGH = 0x0001; /// Axis inversion flags: 1 byte. -/// Axis N (where X=0, Y=1, etc.) is inverted if the Nth bit is set. +/// Axis N (where X = 0, Y=1, etc.) is inverted if the Nth bit is set. /// Bit 7 is used for HoldZ OFF: 1 = off, 0 = on -const static uint16_t AXIS_INVERSION = 0x0002; +const static uint16_t AXIS_INVERSION = 0x0002; /// Endstop inversion flags: 1 byte. -/// The endstops for axis N (where X=0, Y=1, etc.) are considered +/// The endstops for axis N (where X = 0, Y=1, etc.) are considered /// to be logically inverted if the Nth bit is set. /// Bit 7 is set to indicate endstops are present; it is zero to indicate /// that endstops are not present. /// Ordinary endstops (H21LOB et. al.) are inverted. -const static uint16_t ENDSTOP_INVERSION = 0x0003; +const static uint16_t ENDSTOP_INVERSION = 0x0003; /// Name of this machine: 32 bytes. -const static uint16_t MACHINE_NAME = 0x0020; +const static uint16_t MACHINE_NAME = 0x0020; /// Default locations for the axis: 5 x 32 bit = 20 bytes -const static uint16_t AXIS_HOME_POSITIONS = 0x0060; +const static uint16_t AXIS_HOME_POSITIONS = 0x0060; +/// Default steps/mm for each axis: 32 bits = 4 bytes +const static uint16_t ESTOP_CONFIG = 0x0074; // .. 0x0077 + +/// Default steps/mm for each axis: 5 x 32 bit = 20 bytes +const static uint16_t STEPS_PER_MM = 0x0078; // .. 0x008B + +/// Master acceleration rate for each axis: 32 bits = 4 bytes +const static uint16_t MASTER_ACCELERATION_RATE = 0x008C; // .. 0x008F + +/// Default acceleration rates for each axis: 5 x 32 bit = 20 bytes +const static uint16_t AXIS_ACCELERATION_RATES = 0x0090; // .. 0x00A3 + +/// Default acceleration rates for each axis: 4 x 32 bit = 16 bytes +/// X+Y have an integrated value, and Z, A, and B have their own values. +const static uint16_t AXIS_JUNCTION_JERK = 0x00A4; // .. 0x00B3 + +/// Default minimum planner speed: 32 bits = 1 byte +const static uint16_t MINIMUM_PLANNER_SPEED = 0x00B4; // .. 0x00B5 /// Reset all data in the EEPROM to a default. void setDefaults(); diff --git a/firmware/src/Motherboard/Main.cc b/firmware/src/Motherboard/Main.cc index 4228a16..3c38485 100644 --- a/firmware/src/Motherboard/Main.cc +++ b/firmware/src/Motherboard/Main.cc @@ -34,10 +34,10 @@ void reset(bool hard_reset) { ATOMIC_BLOCK(ATOMIC_FORCEON) { Motherboard& board = Motherboard::getBoard(); sdcard::reset(); - planner::abort(); // calls steppers::abort() command::reset(); eeprom::init(); - board.reset(); + board.reset(); // sets up the steps/mm and such for the planner... + planner::abort(); // calls steppers::abort() sei(); // If we've just come from a hard reset, wait for 2.5 seconds before // trying to ping an extruder. This gives the extruder time to boot diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 5703425..6e7139b 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -129,7 +129,7 @@ inline long abs(long x) { return __builtin_labs(x); } namespace planner { - Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; + // Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; // Super-simple circular buffer, where old nodes are reused // TODO: Move to a seperate file @@ -264,6 +264,7 @@ namespace planner { PlannerAxis axes[STEPPER_COUNT]; float default_acceleration; + float minimum_planner_speed; Point position; // the current position (planning-wise, not bot/stepper-wise) in steps float previous_speed[STEPPER_COUNT]; // Speed of previous path line segment #ifdef CENTREPEDAL @@ -282,15 +283,9 @@ namespace planner { void init() { abort(); - - axes[0].max_acceleration = 2000*axes[0].steps_per_mm; - axes[1].max_acceleration = 2000*axes[1].steps_per_mm; - axes[2].max_acceleration = 10*axes[2].steps_per_mm; - axes[3].max_acceleration = 10000*axes[3].steps_per_mm; - axes[4].max_acceleration = 10000*axes[4].steps_per_mm; - stepperTimingDebugPin.setDirection(true); - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setDirection(true); + // stepperTimingDebugPin.setValue(false); #ifdef CENTREPEDAL previous_unit_vec[0]= 0.0; @@ -301,7 +296,8 @@ namespace planner { void setMaxAxisJerk(float jerk, uint8_t axis) { - axes[axis].max_axis_jerk = jerk; + if (axis < STEPPER_COUNT) + axes[axis].max_axis_jerk = jerk; } void setMaxXYJerk(float jerk) { @@ -309,14 +305,22 @@ namespace planner { } void setAxisStepsPerMM(float steps_per_mm, uint8_t axis) { - axes[axis].steps_per_mm = steps_per_mm; + if (axis < STEPPER_COUNT) + axes[axis].steps_per_mm = steps_per_mm; } - void setAcceleration(float new_acceleration) { - default_acceleration = new_acceleration; - // for (int i = 0; i < STEPPER_COUNT; i++) { - // axes[i].max_acceleration = default_acceleration * axes[i].steps_per_mm; - // } + void setAcceleration(int32_t new_acceleration) { + default_acceleration = (float)new_acceleration; + } + + // This is in steps/mm. + void setAxisAcceleration(int32_t new_acceleration, uint8_t axis) { + if (axis < STEPPER_COUNT) + axes[axis].max_acceleration = (float)new_acceleration*axes[axis].steps_per_mm; + } + + void setMinimumPlannerSpeed(float speed) { + minimum_planner_speed = speed; } #ifdef CENTREPEDAL @@ -517,7 +521,7 @@ namespace planner { // That smoothing will happen in Block::calculate_trapezoid later. // However, if it *is* too late, then we need to fix the current entry speed. if (previous->flags & Block::Busy && current->flags & Block::Recalculate) { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); uint32_t current_step = steppers::getCurrentStep(); uint32_t current_feedrate = steppers::getCurrentFeedrate(); // current_feedrate is in steps/second, but entry_speed is in mm/s @@ -533,7 +537,7 @@ namespace planner { previous->flags |= Block::Recalculate; // assume it's not nominal length, to be safe previous->flags &= ~Block::NominalLength; - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); } // If the previous block is an acceleration block, but it is not long enough to complete the @@ -594,8 +598,8 @@ namespace planner { block_index = block_buffer.getNextIndex( block_index ); } - // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. - next->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + // Last/newest block in buffer. Exit speed is set with minimum_planner_speed. Always recalculated. + next->calculate_trapezoid(minimum_planner_speed); next->flags &= ~Block::Recalculate; } @@ -731,7 +735,7 @@ namespace planner { // Limit acceleration per axis for(int i=0; i < STEPPER_COUNT; i++) { // warning: arithmetic overflow is easy here. Try to mitigate. - float step_scale = (float)steps[i] / (float)block->step_event_count; + float step_scale = (float)abs(steps[i]) / (float)block->step_event_count; float axis_acceleration_st = (float)block->acceleration_st * step_scale; if((uint32_t)axis_acceleration_st > axes[i].max_acceleration) block->acceleration_st = axes[i].max_acceleration; @@ -742,7 +746,7 @@ namespace planner { #ifndef CENTREPEDAL // Compute the speed trasitions, or "jerks" // Start with a safe speed - float vmax_junction = MINIMUM_PLANNER_SPEED; + float vmax_junction = minimum_planner_speed; // block clearing of previous_speed is_planning_and_using_prev_speed = true; @@ -786,7 +790,7 @@ namespace planner { // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. - float vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed + float vmax_junction = minimum_planner_speed; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((!block_buffer.isEmpty()) && (previous_nominal_speed > 0.0)) { @@ -823,8 +827,8 @@ namespace planner { #endif block->max_entry_speed = vmax_junction; - // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); + // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed. + float v_allowable = max_allowable_speed(-block->acceleration, minimum_planner_speed, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -848,7 +852,7 @@ namespace planner { // allow clearing of previous speed again is_planning_and_using_prev_speed = false; - // block->calculate_trapezoid(MINIMUM_PLANNER_SPEED); + // block->calculate_trapezoid(minimum_planner_speed); // Update position position = target; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index f8add1f..e813be7 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -109,7 +109,10 @@ namespace planner { void setMaxXYJerk(float jerk); void setMaxAxisJerk(float jerk, uint8_t axis); - void setAcceleration(float acceleration); + void setMinimumPlannerSpeed(float speed); + + void setAcceleration(int32_t acceleration); + void setAxisAcceleration(int32_t new_acceleration, uint8_t axis); #ifdef CENTREPEDAL void setJunctionDeviation(float new_junction_deviation); #endif diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index 5d76947..b1b32ae 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -49,7 +49,7 @@ volatile int32_t timer_counter; StepperAxis axes[STEPPER_COUNT]; volatile bool is_homing; -Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; +// Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; bool holdZ = false; @@ -85,8 +85,8 @@ void init(Motherboard& motherboard) { acceleration_tick_counter = 0; current_feedrate_index = 0; - stepperTimingDebugPin.setDirection(true); - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setDirection(true); + // stepperTimingDebugPin.setValue(false); } void abort() { @@ -240,7 +240,9 @@ uint32_t getCurrentStep() { return intervals - intervals_remaining; } +// WARNING: Freezes the current feedrate! uint32_t getCurrentFeedrate() { + feedrate_changerate = 0; return feedrate; } @@ -347,7 +349,7 @@ bool getNextMove() { } void currentBlockChanged() { - stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(true); // If we are here, then we are moving AND the interrupts are frozen, so get out *fast* current_feedrate_index = 0; @@ -397,7 +399,7 @@ void currentBlockChanged() { // the steppers themselves havne't changed... - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(false); } /// Start homing @@ -457,8 +459,8 @@ bool doInterrupt() { if ((feedrate_steps_remaining-=feedrate_multiplier) <= 0) { current_feedrate_index++; - stepperTimingDebugPin.setValue(true); - stepperTimingDebugPin.setValue(false); + // stepperTimingDebugPin.setValue(true); + // stepperTimingDebugPin.setValue(false); prepareFeedrateIntervals(); } diff --git a/firmware/src/Motherboard/boards/mb24/Configuration.hh b/firmware/src/Motherboard/boards/mb24/Configuration.hh index ecd7922..eea74b1 100644 --- a/firmware/src/Motherboard/boards/mb24/Configuration.hh +++ b/firmware/src/Motherboard/boards/mb24/Configuration.hh @@ -28,7 +28,8 @@ /// starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 128 -#define TICKS_PER_ACCELERATION 125 +// TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded +#define TICKS_PER_ACCELERATION 5 // lower is better #define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) // --- Secure Digital Card configuration --- @@ -191,12 +192,26 @@ #define INTERFACE_BAR_PIN Pin(PortL,0) #define INTERFACE_DEBUG_PIN Pin(PortB,7) + +// The number of movements we can plan ahead at a time +// THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... +#define BLOCK_BUFFER_SIZE 16 + //#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 20.0 -#define DEFAULT_MAX_Z_JERK 10.0 +#define DEFAULT_ACCELERATION 2000.0 /* mm/s/s */ +#define DEFAULT_MAX_XY_JERK 8.0 // <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 8.0 #define DEFAULT_MAX_A_JERK 10.0 #define DEFAULT_MAX_B_JERK 10.0 +// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end +// of the buffer and all stops. This should not be much greater than zero and should only be changed +// if unwanted behavior is observed on a user's machine when running at very slow speeds. +#define DEFAULT_MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) + +// define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob +#undef CENTREPEDAL +#define DEFAULT_JUNCTION_DEVIATION 0.05 // mm + #endif // BOARDS_RRMBV12_CONFIGURATION_HH_ diff --git a/firmware/src/Motherboard/boards/mb24/Motherboard.cc b/firmware/src/Motherboard/boards/mb24/Motherboard.cc index 6d5149b..00be7b1 100644 --- a/firmware/src/Motherboard/boards/mb24/Motherboard.cc +++ b/firmware/src/Motherboard/boards/mb24/Motherboard.cc @@ -129,6 +129,48 @@ void Motherboard::reset() { for (int i = 0; i < STEPPER_COUNT; i++) { stepper[i].init(i); } + + + // Defaults are from my cupcake -Rob + //X 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); + //Y 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); + //Z 2560.0 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); + //A 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); + //B 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + + // Master acceleraion + planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + + //X -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, 2000), 0); + //Y -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, 2000), 1); + //Z -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, 10), 2); + //A -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, 5000), 3); + //B -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, 5000), 4); + + + #ifdef CENTREPEDAL + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); + #else + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); + #endif + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); + + + planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); + // Initialize the host and slave UARTs UART::getHostUART().enable(true); UART::getHostUART().in.reset(); diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index 560d11d..c0194b5 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -179,7 +179,7 @@ // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed // if unwanted behavior is observed on a user's machine when running at very slow speeds. -#define MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) +#define DEFAULT_MINIMUM_PLANNER_SPEED 4.0 // (mm/sec) // define CENTREPEDAL to use centrepedal calucations -- so far I can't get there to work -Rob #undef CENTREPEDAL diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index 738b651..51a4ebb 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -75,25 +75,6 @@ Motherboard::Motherboard(const Pin& psu_pin) : Pin(), eeprom::AXIS_INVERSION); #endif - - /* FIX THIS FIX THIS FIX THIS FIX THIS */ - planner::setAxisStepsPerMM(94.1397046, 0); - planner::setAxisStepsPerMM(94.1397046, 1); - planner::setAxisStepsPerMM(2560.0, 2); - planner::setAxisStepsPerMM(100.470957613814818, 3); - planner::setAxisStepsPerMM(100.470957613814818, 4); - /* END FIX THIS FIX THIS FIX THIS FIX THIS */ - - planner::setAcceleration(DEFAULT_ACCELERATION); -#ifdef CENTREPEDAL - planner::setJunctionDeviation(DEFAULT_JUNCTION_DEVIATION); -#else - planner::setMaxXYJerk(DEFAULT_MAX_XY_JERK); -#endif - planner::setMaxAxisJerk(DEFAULT_MAX_Z_JERK, 2); - planner::setMaxAxisJerk(DEFAULT_MAX_A_JERK, 3); - planner::setMaxAxisJerk(DEFAULT_MAX_B_JERK, 4); - } /// Reset the motherboard to its initial state. @@ -121,6 +102,49 @@ void Motherboard::reset() { for (int i = 0; i < STEPPER_COUNT; i++) { stepper[i].init(i); } + + // Defaults are from my cupcake -Rob + //X 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); + //Y 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); + //Z 2560.0 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); + //A 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); + //B 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + + + // Master acceleraion + planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + + + //X -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, 2000), 0); + //Y -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, 2000), 1); + //Z -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, 10), 2); + //A -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, 5000), 3); + //B -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, 5000), 4); + + +#ifdef CENTREPEDAL + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); +#else + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); +#endif + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); + + + planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); + // Initialize the host and slave UARTs UART::getHostUART().enable(true); UART::getHostUART().in.reset(); diff --git a/firmware/src/shared/Eeprom.cc b/firmware/src/shared/Eeprom.cc index 693beb1..09edffa 100644 --- a/firmware/src/shared/Eeprom.cc +++ b/firmware/src/shared/Eeprom.cc @@ -40,4 +40,16 @@ float getEepromFixed16(const uint16_t location, const float default_value) { return ((float)data[0]) + ((float)data[1])/256.0; } +uint32_t getEeprom32(const uint16_t location, const uint32_t default_value) { + uint32_t data = eeprom_read_dword((const uint32_t*)location); + if (data == 0xffffffff) return default_value; + return data; +} + +float getEepromFixed32(const uint16_t location, const float default_value) { + int32_t data = getEeprom32(location, 0xffffffff); + if (data == 0xffffffff) return default_value; + return ((float)data)/65536.0; +} + } // namespace eeprom diff --git a/firmware/src/shared/Eeprom.hh b/firmware/src/shared/Eeprom.hh index ab3da86..672d997 100644 --- a/firmware/src/shared/Eeprom.hh +++ b/firmware/src/shared/Eeprom.hh @@ -9,7 +9,9 @@ void init(); uint8_t getEeprom8(const uint16_t location, const uint8_t default_value); uint16_t getEeprom16(const uint16_t location, const uint16_t default_value); +uint32_t getEeprom32(const uint16_t location, const uint32_t default_value); float getEepromFixed16(const uint16_t location, const float default_value); +float getEepromFixed32(const uint16_t location, const float default_value); } diff --git a/firmware/src/shared/Menu.cc b/firmware/src/shared/Menu.cc index 397dd19..b4656a2 100644 --- a/firmware/src/shared/Menu.cc +++ b/firmware/src/shared/Menu.cc @@ -5,6 +5,7 @@ #ifdef HAS_INTERFACE_BOARD #include "Steppers.hh" +#include "Planner.hh" #include "Commands.hh" #include "Errors.hh" #include "Tool.hh" @@ -178,7 +179,7 @@ void JogMode::jog(ButtonArray::ButtonName direction) { break; } - steppers::setTarget(position, interval); + planner::addMoveToBuffer(position, interval); } void JogMode::notifyButtonPressed(ButtonArray::ButtonName button) { From ba3bbe6968e9117f6dc20ad8ba06b55aa6946508 Mon Sep 17 00:00:00 2001 From: Robert Giseburt Date: Fri, 17 Feb 2012 18:34:39 -0600 Subject: [PATCH 61/61] Final changes for acceleration. --- firmware/current_version.txt | 2 +- firmware/src/Motherboard/Command.cc | 67 ++++++------- firmware/src/Motherboard/Planner.cc | 59 ++++++------ firmware/src/Motherboard/Planner.hh | 9 ++ firmware/src/Motherboard/Steppers.cc | 96 +------------------ firmware/src/Motherboard/Steppers.hh | 17 ---- .../Motherboard/boards/mb24/Configuration.hh | 18 ++-- .../Motherboard/boards/mb24/Motherboard.cc | 39 +++----- .../boards/rrmbv12/Configuration.hh | 17 ++-- .../Motherboard/boards/rrmbv12/Motherboard.cc | 79 ++++++++------- firmware/src/shared/StepperAxis.cc | 1 + firmware/src/shared/StepperAxis.hh | 2 +- 12 files changed, 146 insertions(+), 260 deletions(-) diff --git a/firmware/current_version.txt b/firmware/current_version.txt index 8c50098..a3ec5a4 100644 --- a/firmware/current_version.txt +++ b/firmware/current_version.txt @@ -1 +1 @@ -3.1 +3.2 diff --git a/firmware/src/Motherboard/Command.cc b/firmware/src/Motherboard/Command.cc index b3bd1e7..7b6c9a6 100644 --- a/firmware/src/Motherboard/Command.cc +++ b/firmware/src/Motherboard/Command.cc @@ -30,7 +30,6 @@ namespace command { -// Pin stepperTimingDebugPin = STEPPER_TIMER_DEBUG; #define COMMAND_BUFFER_SIZE 512 // Must be 2^N: 256, 512, 1204, etc uint8_t buffer_data[COMMAND_BUFFER_SIZE]; @@ -116,27 +115,10 @@ void reset() { // Handle movement comands -- called from a few places static void handleMovementCommand(const uint8_t &command) { - // stepperTimingDebugPin.setDirection(true); - // stepperTimingDebugPin.setValue(false); // if we're already moving, check to make sure the buffer isn't full if (mode == MOVING && planner::isBufferFull()) { - // stepperTimingDebugPin.setValue(true); - // stepperTimingDebugPin.setValue(false); return; // we'll be back! } - /* - if (command == HOST_CMD_QUEUE_POINT_ABS) { - // check for completion - if (command_buffer.getLength() >= 17) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t dda = pop32(); - // planner::addMoveToBuffer(Point(x,y,z), dda); // <- this is a BAD IDEA - } - } else */ if (command == HOST_CMD_QUEUE_POINT_EXT) { // check for completion if (command_buffer.getLength() >= 25) { @@ -150,22 +132,22 @@ static void handleMovementCommand(const uint8_t &command) { int32_t dda = pop32(); planner::addMoveToBuffer(Point(x,y,z,a,b), dda); } - }/* + } else if (command == HOST_CMD_QUEUE_POINT_NEW) { - // check for completion - if (command_buffer.getLength() >= 26) { - command_buffer.pop(); // remove the command code - mode = MOVING; - int32_t x = pop32(); - int32_t y = pop32(); - int32_t z = pop32(); - int32_t a = pop32(); - int32_t b = pop32(); - int32_t us = pop32(); - uint8_t relative = pop8(); - //steppers::setTargetNew(Point(x,y,z,a,b),us,relative); - } - }*/ + // check for completion + if (command_buffer.getLength() >= 26) { + command_buffer.pop(); // remove the command code + mode = MOVING; + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t a = pop32(); + int32_t b = pop32(); + int32_t us = pop32(); + uint8_t relative = pop8(); + planner::addMoveToBufferRelative(Point(x,y,z,a,b), us, relative); + } + } } @@ -191,7 +173,7 @@ void runCommandSlice() { } else { if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; - if (command == HOST_CMD_QUEUE_POINT_ABS || command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { + if (command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { handleMovementCommand(command); } } @@ -253,7 +235,18 @@ void runCommandSlice() { if (command_buffer.getLength() > 0) { uint8_t command = command_buffer[0]; - if (command == HOST_CMD_QUEUE_POINT_EXT) { + if (command == HOST_CMD_QUEUE_POINT_ABS) { + // check for completion + if (command_buffer.getLength() >= 17) { + // No longer supported, but we clear it off of the buffer to avoid a inifinite loop + command_buffer.pop(); // remove the command code + int32_t x = pop32(); + int32_t y = pop32(); + int32_t z = pop32(); + int32_t dda = pop32(); + // steppers::setTarget(Point(x,y,z),dda); + } + } else if (command == HOST_CMD_QUEUE_POINT_EXT || command == HOST_CMD_QUEUE_POINT_NEW) { handleMovementCommand(command); } else if (command == HOST_CMD_CHANGE_TOOL) { if (command_buffer.getLength() >= 2) { @@ -323,7 +316,7 @@ void runCommandSlice() { tool_wait_timeout.start(toolTimeout*1000000L); } } else if (command == HOST_CMD_WAIT_FOR_PLATFORM) { - // FIXME: Almost equivalent to WAIT_FOR_TOOL + // FIXME: Almost equivalent to WAIT_FOR_TOOL if (command_buffer.getLength() >= 6) { mode = WAIT_ON_PLATFORM; command_buffer.pop(); @@ -396,8 +389,6 @@ void runCommandSlice() { } else { } } else { // command buffer is empty - // stepperTimingDebugPin.setValue(true); - // stepperTimingDebugPin.setValue(false); } } } diff --git a/firmware/src/Motherboard/Planner.cc b/firmware/src/Motherboard/Planner.cc index 6e7139b..5995d40 100644 --- a/firmware/src/Motherboard/Planner.cc +++ b/firmware/src/Motherboard/Planner.cc @@ -188,7 +188,7 @@ namespace planner { return data[index]; } - // bump the head with buffer++. cannot return anything useful, so it doesn't + // bump the head. cannot return anything useful, so it doesn't // WARNING: no sanity checks! inline void bumpHead() { head = getNextIndex(head); @@ -196,7 +196,7 @@ namespace planner { full = true; } - // bump the tail with buffer--. cannot return anything useful, so it doesn't + // bump the tail. cannot return anything useful, so it doesn't // WARNING: no sanity checks! inline void bumpTail() { tail = getNextIndex(tail); @@ -238,28 +238,6 @@ namespace planner { // note that X+Y has it's own setting, and this if for all the rest float max_axis_jerk; }; -#if 0 - // precision is the number of bit to use for the fractional part - template - struct QuickFixedPoint - { - base_type value; - QuickFixedPoint(float init_value) : value((base_type)(init_value * (float)(1<>a.precision; - } - base_type operator/(QuickFixedPoint a) { - return (value / a)>>a.precision; - } - }; -#endif PlannerAxis axes[STEPPER_COUNT]; @@ -331,6 +309,7 @@ namespace planner { // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the // acceleration within the allotted distance. + // Needs to be conbverted to fixed-point. FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) { return sqrt((target_velocity*target_velocity)-(acceleration*2.0)*distance); } @@ -362,7 +341,8 @@ namespace planner { } } -#if 0 +// Disabled because it's not used, but if it is in the future, here's how +#if 0 // Calculates the time (not distance) in microseconds (S*1,000,000) it takes to go from initial_rate for distance at acceleration rate FORCE_INLINE uint32_t estimate_time_to_accelerate(float initial_rate, float acceleration, float distance) { /* @@ -629,6 +609,26 @@ namespace planner { block_buffer.bumpTail(); } + bool addMoveToBufferRelative(const Point& move, const int32_t ms, const int8_t relative) { + Point target; + int32_t max_delta = 0; + for (int i = 0; i < STEPPER_COUNT; i++) { + int32_t delta = 0; + if ((relative & (1 << i))) { + target[i] = position[i] + move[i]; + delta = abs(move[i]); + } else { + target[i] = move[i]; + delta = abs(target[i] - position[i]); + + } + if (delta > max_delta) { + max_delta = delta; + } + } + + return addMoveToBuffer(target, ms/max_delta); + } // Buffer the move. IOW, add a new block, and recalculate the acceleration accordingly @@ -700,7 +700,8 @@ namespace planner { current_speed[i] = delta_mm[i] * inverse_second; } - // // Limit speed per axis (already done in RepG) + // Limit speed per axis (already done in RepG, so I'm killing it here. Left for reference. -Rob) + // float speed_factor = 1.0; //factor <=1 do decrease speed // for(int i=0; i < STEPPER_COUNT; i++) { // if(fabs(current_speed[i]) > max_feedrate[i]) @@ -773,7 +774,9 @@ namespace planner { } #else // CENTREPEDAL - + + // NEEDS MORE TESTING + // Compute path unit vector float unit_vec[3]; @@ -852,8 +855,6 @@ namespace planner { // allow clearing of previous speed again is_planning_and_using_prev_speed = false; - // block->calculate_trapezoid(minimum_planner_speed); - // Update position position = target; diff --git a/firmware/src/Motherboard/Planner.hh b/firmware/src/Motherboard/Planner.hh index e813be7..9b53f4a 100644 --- a/firmware/src/Motherboard/Planner.hh +++ b/firmware/src/Motherboard/Planner.hh @@ -85,6 +85,15 @@ namespace planner { /// \return If the move was buffered bool addMoveToBuffer(const Point& target, int32_t us_per_step); + /// Buffer a movement to the target point (in step-space). We should avoid this, as it requires more calculation. + /// \param[in] target New position to move to, in step-space + /// \param[in] ms Duration of the move, in milliseconds + /// \param[in] relative Bitfield specifying whether each axis should + /// interpret the new position as absolute or + /// relative. + /// \return If the move was buffered + bool addMoveToBufferRelative(const Point& target, const int32_t ms, const int8_t relative); + /// Home one or more axes /// \param[in] maximums If true, home in the positive direction /// \param[in] axes_enabled Bitfield specifiying which axes to diff --git a/firmware/src/Motherboard/Steppers.cc b/firmware/src/Motherboard/Steppers.cc index b1b32ae..8be613f 100644 --- a/firmware/src/Motherboard/Steppers.cc +++ b/firmware/src/Motherboard/Steppers.cc @@ -127,86 +127,6 @@ void setHoldZ(bool holdZ_in) { holdZ = holdZ_in; } -#if 0 -void setTarget(const Point& target, int32_t dda_interval) { - int32_t max_delta = 0; - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].setTarget(target[i], false); - const int32_t delta = axes[i].delta; - // Only shut z axis on inactivity - if (i == 2 && !holdZ) axes[i].enableStepper(delta != 0); - else if (delta != 0) axes[i].enableStepper(true); - if (delta > max_delta) { - max_delta = delta; - } - } - // // compute number of intervals for this move - //intervals = ((max_delta * dda_interval) / INTERVAL_IN_MICROSECONDS); - - // reset feedrate_scale_shift and feedrate position - if (feedrate_scale_shift != 0) { - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position << feedrate_scale_shift; - feedrate_scale_shift = 0; - } - if (axes[STEPRATE_AXIS].position == 0) { - axes[STEPRATE_AXIS].definePosition(dda_interval); - } - - axes[STEPRATE_AXIS].setTarget(dda_interval, /*relative =*/ false); - - if (max_delta == 0) { - is_running = false; - return; - } - - // WARNING: Edge case where axes[STEPRATE_AXIS].delta > INT32_MAX is unhandled - int8_t scale_shift = 0; - while ((axes[STEPRATE_AXIS].delta >> scale_shift) > max_delta) { - scale_shift++; - } - if (scale_shift > 0) { - feedrate_scale_shift = scale_shift; - axes[STEPRATE_AXIS].position = axes[STEPRATE_AXIS].position >> feedrate_scale_shift; - axes[STEPRATE_AXIS].target = axes[STEPRATE_AXIS].target >> feedrate_scale_shift; - axes[STEPRATE_AXIS].delta = axes[STEPRATE_AXIS].delta >> feedrate_scale_shift; - } - - // We use += here so that the odd rounded-off time from the last move is still waited out - timer_counter += axes[STEPRATE_AXIS].position << feedrate_scale_shift; - - intervals = max_delta; - intervals_remaining = intervals; - const int32_t negative_half_interval = -intervals / 2; - for (int i = 0; i < STEPPER_COUNT; i++) { - axes[i].counter = negative_half_interval; - } - is_running = true; -} - -/* -void setTargetNew(const Point& target, int32_t us, uint8_t relative) { -for (int i = 0; i < STEPPER_COUNT; i++) { -axes[i].setTarget(target[i], (relative & (1 << i)) != 0); -// Only shut z axis on inactivity -const int32_t delta = axes[i].delta; -if (i == 2 && !holdZ) { -axes[i].enableStepper(delta != 0); -} else if (delta != 0) { -axes[i].enableStepper(true); -} -} -// compute number of intervals for this move -intervals = us / INTERVAL_IN_MICROSECONDS; -intervals_remaining = intervals; -const int32_t negative_half_interval = -intervals / 2; -for (int i = 0; i < STEPPER_COUNT; i++) { -axes[i].counter = negative_half_interval; -} -is_running = true; -} -*/ -#endif - inline void prepareFeedrateIntervals() { if (current_feedrate_index > 2) return; @@ -222,17 +142,6 @@ inline void recalcFeedrate() { return; // SHRIEK! feedrate_inverted = 1000000/feedrate; - // // if we are supposed to step too fast, we simulate double-size microsteps - // feedrate_multiplier = 1; - // while (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - // feedrate_multiplier <<= 1; // * 2 - // feedrate_inverted <<= 1; // * 2 - // } - // - // for (int i = 0; i < STEPPER_COUNT; i++) { - // axes[i].setStepMultiplier(feedrate_multiplier); - // } - feedrate_dirty = 0; } @@ -397,7 +306,7 @@ void currentBlockChanged() { timer_counter = feedrate_inverted; - // the steppers themselves havne't changed... + // the steppers themselves haven't changed... // stepperTimingDebugPin.setValue(false); } @@ -466,9 +375,6 @@ bool doInterrupt() { if (feedrate_dirty) { recalcFeedrate(); - // if (feedrate_inverted < INTERVAL_IN_MICROSECONDS) { - // feedrate_inverted = INTERVAL_IN_MICROSECONDS; - // } } timer_counter += feedrate_inverted; diff --git a/firmware/src/Motherboard/Steppers.hh b/firmware/src/Motherboard/Steppers.hh index 061f7b2..57dd3e8 100644 --- a/firmware/src/Motherboard/Steppers.hh +++ b/firmware/src/Motherboard/Steppers.hh @@ -47,23 +47,6 @@ namespace steppers { /// \param[in] enable If true, enable the axis. If false, disable. void enableAxis(uint8_t index, bool enable); - /// Instruct the stepper subsystem to move the machine to the - /// given position. - /// \param[in] target Position to move to - /// \param[in] dda_interval Motion speed, in us per step. - void setTarget(const Point& target, int32_t dda_interval); - - /// Instruct the stepper subsystem to move the machine to the - /// given position. - /// \param[in] target Position to move to - /// \param[in] ms Duration of the move, in milliseconds - /// \param[in] relative Bitfield specifying whether each axis should - /// interpret the new position as absolute or - /// relative. - void setTargetNew(const Point& target, - int32_t ms, - uint8_t relative =0); - /// Home one or more axes /// \param[in] maximums If true, home in the positive direction /// \param[in] axes_enabled Bitfield specifiying which axes to diff --git a/firmware/src/Motherboard/boards/mb24/Configuration.hh b/firmware/src/Motherboard/boards/mb24/Configuration.hh index eea74b1..c4e919e 100644 --- a/firmware/src/Motherboard/boards/mb24/Configuration.hh +++ b/firmware/src/Motherboard/boards/mb24/Configuration.hh @@ -28,7 +28,6 @@ /// starvation; leave this at 64uS or greater unless you know what you're doing. #define INTERVAL_IN_MICROSECONDS 128 -// TICKS_PER_ACCELERATION should be set to that ACCELERATION_TICKS_PER_SECOND is not rounded #define TICKS_PER_ACCELERATION 5 // lower is better #define ACCELERATION_TICKS_PER_SECOND (1000000/(INTERVAL_IN_MICROSECONDS*TICKS_PER_ACCELERATION)) @@ -197,12 +196,17 @@ // THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 -//#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 2000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 8.0 // <-- unused if CENTREPEDAL is defined below -#define DEFAULT_MAX_Z_JERK 8.0 -#define DEFAULT_MAX_A_JERK 10.0 -#define DEFAULT_MAX_B_JERK 10.0 +#define DEFAULT_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_X_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Y_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Z_ACCELERATION 10.0 // mm/s/s +#define DEFAULT_A_ACCELERATION 200.0 // mm/s/s +#define DEFAULT_B_ACCELERATION 200.0 // mm/s/s + +#define DEFAULT_MAX_XY_JERK 8.0 // ms/s <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 8.0 // mm/s +#define DEFAULT_MAX_A_JERK 10.0 // mm/s +#define DEFAULT_MAX_B_JERK 10.0 // mm/s // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed diff --git a/firmware/src/Motherboard/boards/mb24/Motherboard.cc b/firmware/src/Motherboard/boards/mb24/Motherboard.cc index 00be7b1..a9624ea 100644 --- a/firmware/src/Motherboard/boards/mb24/Motherboard.cc +++ b/firmware/src/Motherboard/boards/mb24/Motherboard.cc @@ -91,21 +91,6 @@ Motherboard::Motherboard() : Pin(), eeprom::AXIS_INVERSION); #endif - - /* FIX THIS FIX THIS FIX THIS FIX THIS */ - planner::setAxisStepsPerMM(94.1397046, 0); - planner::setAxisStepsPerMM(94.1397046, 1); - planner::setAxisStepsPerMM(2560.0, 2); - planner::setAxisStepsPerMM(100.470957613814818, 3); - planner::setAxisStepsPerMM(100.470957613814818, 4); - /* END FIX THIS FIX THIS FIX THIS FIX THIS */ - - planner::setAcceleration(DEFAULT_ACCELERATION); - planner::setMaxXYJerk(DEFAULT_MAX_XY_JERK); - planner::setMaxAxisJerk(DEFAULT_MAX_Z_JERK, 2); - planner::setMaxAxisJerk(DEFAULT_MAX_A_JERK, 3); - planner::setMaxAxisJerk(DEFAULT_MAX_B_JERK, 4); - } /// Reset the motherboard to its initial state. @@ -143,27 +128,29 @@ void Motherboard::reset() { //B 100.470957613814818 planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + // Master acceleraion planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + //X -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, 2000), 0); + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, DEFAULT_X_ACCELERATION), 0); //Y -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, 2000), 1); + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, DEFAULT_Y_ACCELERATION), 1); //Z -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, 10), 2); + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, DEFAULT_Z_ACCELERATION), 2); //A -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, 5000), 3); + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, DEFAULT_A_ACCELERATION), 3); //B -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, 5000), 4); + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, DEFAULT_B_ACCELERATION), 4); - #ifdef CENTREPEDAL - // uses the same eeprom address as the X/Y junction jerk~ - planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); - #else - planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); - #endif +#ifdef CENTREPEDAL + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); +#else + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); +#endif planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); diff --git a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh index c0194b5..f6a1ed3 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh +++ b/firmware/src/Motherboard/boards/rrmbv12/Configuration.hh @@ -169,12 +169,17 @@ // THIS MUST BE A POWER OF 2! 4, 8, 16, 32, you get the idea... #define BLOCK_BUFFER_SIZE 16 -//#define DEFAULT_ACCELERATION 3000.0 /* mm/s/s */ -#define DEFAULT_ACCELERATION 2000.0 /* mm/s/s */ -#define DEFAULT_MAX_XY_JERK 8.0 // <-- unused if CENTREPEDAL is defined below -#define DEFAULT_MAX_Z_JERK 8.0 -#define DEFAULT_MAX_A_JERK 10.0 -#define DEFAULT_MAX_B_JERK 10.0 +#define DEFAULT_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_X_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Y_ACCELERATION 2000.0 // mm/s/s +#define DEFAULT_Z_ACCELERATION 10.0 // mm/s/s +#define DEFAULT_A_ACCELERATION 200.0 // mm/s/s +#define DEFAULT_B_ACCELERATION 200.0 // mm/s/s + +#define DEFAULT_MAX_XY_JERK 8.0 // ms/s <-- unused if CENTREPEDAL is defined below +#define DEFAULT_MAX_Z_JERK 8.0 // mm/s +#define DEFAULT_MAX_A_JERK 10.0 // mm/s +#define DEFAULT_MAX_B_JERK 10.0 // mm/s // Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end // of the buffer and all stops. This should not be much greater than zero and should only be changed diff --git a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc index 51a4ebb..ce23973 100644 --- a/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc +++ b/firmware/src/Motherboard/boards/rrmbv12/Motherboard.cc @@ -103,63 +103,62 @@ void Motherboard::reset() { stepper[i].init(i); } - // Defaults are from my cupcake -Rob - //X 94.1397046 - planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); - //Y 94.1397046 - planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); - //Z 2560.0 - planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); - //A 100.470957613814818 - planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); - //B 100.470957613814818 - planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); + // Defaults are from my cupcake -Rob + //X 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 0, 94.1397046), 0); + //Y 94.1397046 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 4, 94.1397046), 1); + //Z 2560.0 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+ 8, 2560.0), 2); + //A 100.470957613814818 + planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+12, 100.470957613814818), 3); + //B 100.470957613814818 + // planner::setAxisStepsPerMM(eeprom::getEepromFixed32(eeprom::STEPS_PER_MM+16, 100.470957613814818), 4); // Master acceleraion - planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); + planner::setAcceleration(eeprom::getEeprom32(eeprom::MASTER_ACCELERATION_RATE, DEFAULT_ACCELERATION)); - //X -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, 2000), 0); - //Y -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, 2000), 1); - //Z -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, 10), 2); - //A -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, 5000), 3); - //B -- default conservative - planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, 5000), 4); + //X -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 0, DEFAULT_X_ACCELERATION), 0); + //Y -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 4, DEFAULT_Y_ACCELERATION), 1); + //Z -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+ 8, DEFAULT_Z_ACCELERATION), 2); + //A -- default conservative + planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+12, DEFAULT_A_ACCELERATION), 3); + //B -- default conservative + // planner::setAxisAcceleration(eeprom::getEeprom32(eeprom::AXIS_ACCELERATION_RATES+16, DEFAULT_B_ACCELERATION), 4); #ifdef CENTREPEDAL - // uses the same eeprom address as the X/Y junction jerk~ - planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); + // uses the same eeprom address as the X/Y junction jerk~ + planner::setJunctionDeviation(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_JUNCTION_DEVIATION)); #else - planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); + planner::setMaxXYJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 0, DEFAULT_MAX_XY_JERK)); #endif - planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); - planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); - planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 4, DEFAULT_MAX_Z_JERK), 2); + planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+ 8, DEFAULT_MAX_A_JERK), 3); + // planner::setMaxAxisJerk(eeprom::getEepromFixed32(eeprom::AXIS_JUNCTION_JERK+12, DEFAULT_MAX_B_JERK), 4); - - planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); + planner::setMinimumPlannerSpeed(eeprom::getEepromFixed32(eeprom::MINIMUM_PLANNER_SPEED, DEFAULT_MINIMUM_PLANNER_SPEED)); // Initialize the host and slave UARTs - UART::getHostUART().enable(true); - UART::getHostUART().in.reset(); + UART::getHostUART().enable(true); + UART::getHostUART().in.reset(); - // TODO: These aren't done on other platforms, are they necessary? - UART::getHostUART().reset(); - UART::getHostUART().out.reset(); + // TODO: These aren't done on other platforms, are they necessary? + UART::getHostUART().reset(); + UART::getHostUART().out.reset(); - UART::getSlaveUART().enable(true); - UART::getSlaveUART().in.reset(); + UART::getSlaveUART().enable(true); + UART::getSlaveUART().in.reset(); - // TODO: These aren't done on other platforms, are they necessary? - UART::getSlaveUART().reset(); - UART::getSlaveUART().out.reset(); + // TODO: These aren't done on other platforms, are they necessary? + UART::getSlaveUART().reset(); + UART::getSlaveUART().out.reset(); // Reset and configure timer 1, the microsecond and stepper diff --git a/firmware/src/shared/StepperAxis.cc b/firmware/src/shared/StepperAxis.cc index 290945d..d099686 100644 --- a/firmware/src/shared/StepperAxis.cc +++ b/firmware/src/shared/StepperAxis.cc @@ -42,6 +42,7 @@ void StepperAxis::setHoming(const bool direction_in) { interface->setDirection(direction); interface->setEnabled(true); delta = 1; + step_change = direction ? 1 : -1; } void StepperAxis::definePosition(const int32_t position_in) { diff --git a/firmware/src/shared/StepperAxis.hh b/firmware/src/shared/StepperAxis.hh index cb3e7f0..738cadd 100644 --- a/firmware/src/shared/StepperAxis.hh +++ b/firmware/src/shared/StepperAxis.hh @@ -60,7 +60,7 @@ public: /// to be relative to the current position. void setTarget(const int32_t target_in, bool relative); - /// Set the step multiplier -- must be 2^N + /// Set the step multiplier /// \param[in] new_multiplier void setStepMultiplier(const int8_t new_multiplier);