diff --git a/Firmware/.vscode/settings.json b/Firmware/.vscode/settings.json index 0ed2a17..2fe9d6a 100755 --- a/Firmware/.vscode/settings.json +++ b/Firmware/.vscode/settings.json @@ -12,5 +12,6 @@ "numeric": "cpp", "ostream": "cpp" }, - "C_Cpp.errorSquiggles": "disabled" + "C_Cpp.errorSquiggles": "disabled", + "cmake.configureOnOpen": false } \ No newline at end of file diff --git a/Firmware/include/StepGen.h b/Firmware/include/StepGen.h index 0a5cbf5..dc2eff7 100755 --- a/Firmware/include/StepGen.h +++ b/Firmware/include/StepGen.h @@ -15,21 +15,23 @@ public: volatile double_t actualPosition; volatile double_t requestedPosition; HardwareTimer *MyTim; - uint32_t stepsPerMM; + uint16_t stepsPerMM; static uint32_t sync0CycleTime; uint8_t dirPin; - uint8_t stepPin; - uint8_t timerChan; + PinName stepPin; + uint32_t timerChan; const uint32_t maxFreq = 100000; + volatile uint32_t prevFreq1 = 0; + volatile uint32_t prevFreq2 = 0; - StepGen(TIM_TypeDef *Timer, uint8_t timerChannel, uint8_t stepPin, uint8_t dirPin, void irq(void)); + StepGen(TIM_TypeDef *Timer, uint32_t timerChannel, PinName stepPin, uint8_t dirPin, void irq(void)); void reqPos(double_t pos); double reqPos(); void actPos(double_t pos); double actPos(); void handleStepper(void); void timerCB(); - void setScale(int32_t spm); + void setScale(int16_t spm); }; #endif \ No newline at end of file diff --git a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml index 26eb77a..738a2d1 100755 --- a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml +++ b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml @@ -125,7 +125,7 @@ DT1602 - 80 + 144 0 Max SubIndex @@ -146,10 +146,20 @@ ro + + 2 + StepsPerMM + ULINT + 64 + 80 + + ro + + DT1603 - 80 + 144 0 Max SubIndex @@ -170,6 +180,16 @@ ro + + 2 + StepsPerMM + ULINT + 64 + 80 + + ro + + DT1A00 @@ -487,7 +507,7 @@ DT7002 - 80 + 96 0 Max SubIndex @@ -509,10 +529,21 @@ R + + 2 + StepsPerMM + INT + 16 + 80 + + ro + R + + DT7003 - 80 + 96 0 Max SubIndex @@ -534,6 +565,17 @@ R + + 2 + StepsPerMM + INT + 16 + 80 + + ro + R + + ULINT @@ -567,6 +609,10 @@ DINT 32 + + INT + 16 + @@ -710,12 +756,12 @@ #x1602 StepGenIn1 DT1602 - 80 + 144 Max SubIndex - 1 + 2 @@ -724,6 +770,12 @@ #x70020140 + + StepsPerMM + + #x70020210 + + ro @@ -733,12 +785,12 @@ #x1603 StepGenIn2 DT1603 - 80 + 144 Max SubIndex - 1 + 2 @@ -747,6 +799,12 @@ #x70030140 + + StepsPerMM + + #x70030210 + + ro @@ -1195,12 +1253,12 @@ #x7002 StepGenIn1 DT7002 - 80 + 96 Max SubIndex - 1 + 2 @@ -1209,6 +1267,12 @@ 0 + + StepsPerMM + + 0 + + ro @@ -1218,12 +1282,12 @@ #x7003 StepGenIn2 DT7003 - 80 + 96 Max SubIndex - 1 + 2 @@ -1232,6 +1296,12 @@ 0 + + StepsPerMM + + 0 + + ro @@ -1279,6 +1349,13 @@ CommandedPosition LREAL + + #x7002 + #x2 + 16 + StepsPerMM + INT + #x1603 @@ -1290,6 +1367,13 @@ CommandedPosition LREAL + + #x7003 + #x2 + 16 + StepsPerMM + INT + #x1A00 diff --git a/Firmware/lib/soes/ecat_options.h b/Firmware/lib/soes/ecat_options.h index 30684ae..7bc2f2b 100755 --- a/Firmware/lib/soes/ecat_options.h +++ b/Firmware/lib/soes/ecat_options.h @@ -33,7 +33,7 @@ #define SM3_smc 0x20 #define SM3_act 1 -#define MAX_MAPPINGS_SM2 4 +#define MAX_MAPPINGS_SM2 6 #define MAX_MAPPINGS_SM3 7 #define MAX_RXPDO_SIZE 512 diff --git a/Firmware/lib/soes/esi.json b/Firmware/lib/soes/esi.json index bd9e5a0..23ea7ea 100755 --- a/Firmware/lib/soes/esi.json +++ b/Firmware/lib/soes/esi.json @@ -183,6 +183,13 @@ "data": "&Obj.StepGenIn1.CommandedPosition", "value": "0", "access": "RO" + }, + { + "name": "StepsPerMM", + "dtype": "INTEGER16", + "value": "0", + "access": "RO", + "data": "&Obj.StepGenIn1.StepsPerMM" } ], "pdo_mappings": [ @@ -203,6 +210,13 @@ "data": "&Obj.StepGenIn2.CommandedPosition", "value": "0", "access": "RO" + }, + { + "name": "StepsPerMM", + "dtype": "INTEGER16", + "value": "0", + "access": "RO", + "data": "&Obj.StepGenIn2.StepsPerMM" } ], "pdo_mappings": [ diff --git a/Firmware/lib/soes/objectlist.c b/Firmware/lib/soes/objectlist.c index 93d4d8d..b92939f 100755 --- a/Firmware/lib/soes/objectlist.c +++ b/Firmware/lib/soes/objectlist.c @@ -22,9 +22,11 @@ static const char acName1601_01[] = "IndexLatchEnable"; static const char acName1602[] = "StepGenIn1"; static const char acName1602_00[] = "Max SubIndex"; static const char acName1602_01[] = "CommandedPosition"; +static const char acName1602_02[] = "StepsPerMM"; static const char acName1603[] = "StepGenIn2"; static const char acName1603_00[] = "Max SubIndex"; static const char acName1603_01[] = "CommandedPosition"; +static const char acName1603_02[] = "StepsPerMM"; static const char acName1A00[] = "EncPos"; static const char acName1A00_00[] = "Max SubIndex"; static const char acName1A00_01[] = "EncPos"; @@ -83,9 +85,11 @@ static const char acName7001[] = "IndexLatchEnable"; static const char acName7002[] = "StepGenIn1"; static const char acName7002_00[] = "Max SubIndex"; static const char acName7002_01[] = "CommandedPosition"; +static const char acName7002_02[] = "StepsPerMM"; static const char acName7003[] = "StepGenIn2"; static const char acName7003_00[] = "Max SubIndex"; static const char acName7003_01[] = "CommandedPosition"; +static const char acName7003_02[] = "StepsPerMM"; const _objd SDO1000[] = { @@ -123,13 +127,15 @@ const _objd SDO1601[] = }; const _objd SDO1602[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 2, NULL}, {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_01, 0x70020140, NULL}, + {0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_02, 0x70020210, NULL}, }; const _objd SDO1603[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 1, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 2, NULL}, {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1603_01, 0x70030140, NULL}, + {0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1603_02, 0x70030210, NULL}, }; const _objd SDO1A00[] = { @@ -233,13 +239,15 @@ const _objd SDO7001[] = }; const _objd SDO7002[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 1, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 2, NULL}, {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition}, + {0x02, DTYPE_INTEGER16, 16, ATYPE_RO, acName7002_02, 0, &Obj.StepGenIn1.StepsPerMM}, }; const _objd SDO7003[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7003_00, 1, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7003_00, 2, NULL}, {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7003_01, 0, &Obj.StepGenIn2.CommandedPosition}, + {0x02, DTYPE_INTEGER16, 16, ATYPE_RO, acName7003_02, 0, &Obj.StepGenIn2.StepsPerMM}, }; const _objectlist SDOobjects[] = @@ -251,8 +259,8 @@ const _objectlist SDOobjects[] = {0x1018, OTYPE_RECORD, 4, 0, acName1018, SDO1018}, {0x1600, OTYPE_RECORD, 1, 0, acName1600, SDO1600}, {0x1601, OTYPE_RECORD, 1, 0, acName1601, SDO1601}, - {0x1602, OTYPE_RECORD, 1, 0, acName1602, SDO1602}, - {0x1603, OTYPE_RECORD, 1, 0, acName1603, SDO1603}, + {0x1602, OTYPE_RECORD, 2, 0, acName1602, SDO1602}, + {0x1603, OTYPE_RECORD, 2, 0, acName1603, SDO1603}, {0x1A00, OTYPE_RECORD, 1, 0, acName1A00, SDO1A00}, {0x1A01, OTYPE_RECORD, 1, 0, acName1A01, SDO1A01}, {0x1A02, OTYPE_RECORD, 1, 0, acName1A02, SDO1A02}, @@ -272,7 +280,7 @@ const _objectlist SDOobjects[] = {0x6006, OTYPE_RECORD, 1, 0, acName6006, SDO6006}, {0x7000, OTYPE_VAR, 0, 0, acName7000, SDO7000}, {0x7001, OTYPE_VAR, 0, 0, acName7001, SDO7001}, - {0x7002, OTYPE_RECORD, 1, 0, acName7002, SDO7002}, - {0x7003, OTYPE_RECORD, 1, 0, acName7003, SDO7003}, + {0x7002, OTYPE_RECORD, 2, 0, acName7002, SDO7002}, + {0x7003, OTYPE_RECORD, 2, 0, acName7003, SDO7003}, {0xffff, 0xff, 0xff, 0xff, NULL, NULL} }; diff --git a/Firmware/lib/soes/utypes.h b/Firmware/lib/soes/utypes.h index a85f8ac..f887e0d 100755 --- a/Firmware/lib/soes/utypes.h +++ b/Firmware/lib/soes/utypes.h @@ -34,10 +34,12 @@ typedef struct struct { double CommandedPosition; + int16_t StepsPerMM; } StepGenIn1; struct { double CommandedPosition; + int16_t StepsPerMM; } StepGenIn2; } _Objects; diff --git a/Firmware/src/StepGen.cpp b/Firmware/src/StepGen.cpp index 6957856..8f7a9c4 100755 --- a/Firmware/src/StepGen.cpp +++ b/Firmware/src/StepGen.cpp @@ -2,14 +2,13 @@ #include #include "StepGen.h" -StepGen::StepGen(TIM_TypeDef *Timer, uint8_t _timerChannel, uint8_t _stepPin, uint8_t _dirPin, void irq(void)) +StepGen::StepGen(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void)) { timerIsRunning = 0; timerStepPosition = 0; timerStepDirection = 0; timerStepPositionAtEnd = 0; timerNewEndStepPosition = 0; - timerNewCycleTime = 0; actualPosition = 0; requestedPosition = 0; stepsPerMM = 0; @@ -18,7 +17,6 @@ StepGen::StepGen(TIM_TypeDef *Timer, uint8_t _timerChannel, uint8_t _stepPin, ui stepPin = _stepPin; timerChan = _timerChannel; MyTim = new HardwareTimer(Timer); - MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin); MyTim->attachInterrupt(irq); pinMode(dirPin, OUTPUT); } @@ -52,18 +50,13 @@ void StepGen::handleStepper(void) } int32_t pulsesAtEndOfCycle = stepsPerMM * reqPos(); // From Turner.hal X:5000 Z:2000 ps/mm - sync0CycleTime = 1000; + // Will be picked up by the timer_CB and the timer is reloaded. + timerNewEndStepPosition = pulsesAtEndOfCycle; - if (timerIsRunning) - { - // Set variables, they will be picked up by the timer_CB and the timer is reloaded. - timerNewEndStepPosition = pulsesAtEndOfCycle; - timerNewCycleTime = sync0CycleTime; - } - if (!timerIsRunning) + if (!timerIsRunning) // no timer isn't running. start it here { // Start the timer - int32_t steps = pulsesAtEndOfCycle - timerStepPositionAtEnd; // Pulses to go + or - + int32_t steps = pulsesAtEndOfCycle - timerStepPosition; // Pulses to go + or - if (steps != 0) { uint8_t sgn = steps > 0 ? HIGH : LOW; @@ -83,7 +76,7 @@ void StepGen::handleStepper(void) void StepGen::timerCB() { timerStepPosition += timerStepDirection; // The step that was just completed - if (timerNewCycleTime != 0) // Are we going to reload? + if (timerNewEndStepPosition != 0) // Are we going to reload? { // Input for reload is timerNewEndStepPosition and timerNewEndTime // The timer has current position and current time and from this @@ -94,12 +87,11 @@ void StepGen::timerCB() { uint8_t sgn = steps > 0 ? HIGH : LOW; digitalWrite(dirPin, sgn); - double_t freqf = abs(steps) * (1e6 / double(timerNewCycleTime)); + double_t freqf = abs(steps) * (1e6 / double(sync0CycleTime)); uint32_t freq = uint32_t(freqf); timerStepDirection = steps > 0 ? 1 : -1; timerStepPositionAtEnd = timerNewEndStepPosition; timerNewEndStepPosition = 0; // Set to zero to not reload next time - timerNewCycleTime = 0; MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin); MyTim->setOverflow(freq, HERTZ_FORMAT); MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 % @@ -114,7 +106,7 @@ void StepGen::timerCB() } } -void StepGen::setScale(int32_t spm) +void StepGen::setScale(int16_t spm) { stepsPerMM = spm; } diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 9cab44b..2ac0178 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -27,13 +27,13 @@ void indexPulseEncoderCB2(void) #include "StepGen.h" void timerCallbackStep1(void); -StepGen Step1(TIM1, 4, PA11, PA12, timerCallbackStep1); +StepGen Step1(TIM1, 4, PA_11, PA12, timerCallbackStep1); void timerCallbackStep1(void) { Step1.timerCB(); } void timerCallbackStep2(void); -StepGen Step2(TIM8, 4, PC9, PC10, timerCallbackStep2); +StepGen Step2(TIM8, 4, PC_9, PC10, timerCallbackStep2); void timerCallbackStep2(void) { Step2.timerCB(); @@ -45,7 +45,9 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera Encoder1.setScale(Obj.EncPosScale); Step1.reqPos(Obj.StepGenIn1.CommandedPosition); + Step1.setScale(Obj.StepGenIn1.StepsPerMM); Step2.reqPos(Obj.StepGenIn2.CommandedPosition); + Step2.setScale(Obj.StepGenIn2.StepsPerMM); } void handleStepper(void) @@ -98,9 +100,6 @@ void setup(void) Serial1.begin(115200); rcc_config(); // probably breaks some timers. - Step1.setScale(1000); // 2000 /rev 4 mm/rev x 2.5 gear => 1250 /mm - Step2.setScale(500); // 2000 /rev 4 mm/rev => 500 /mm - ecat_slv_init(&config); }