diff --git a/Pcb-1-lan9252/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml b/Pcb-1-lan9252/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml index aa46cd5..2397c98 100755 --- a/Pcb-1-lan9252/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml +++ b/Pcb-1-lan9252/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml @@ -123,6 +123,30 @@ + + DT1602 + 48 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + CommandedPosition + UDINT + 32 + 16 + + ro + + + DT1A00 48 @@ -243,6 +267,30 @@ + + DT1A05 + 48 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + ActualPosition + UDINT + 32 + 16 + + ro + + + DT1C00ARR USINT @@ -278,15 +326,15 @@ DT1C12ARR UINT - 32 + 48 1 - 2 + 3 DT1C12 - 48 + 64 0 Max SubIndex @@ -300,7 +348,7 @@ Elements DT1C12ARR - 32 + 48 16 ro @@ -310,15 +358,15 @@ DT1C13ARR UINT - 80 + 96 1 - 5 + 6 DT1C13 - 96 + 112 0 Max SubIndex @@ -332,13 +380,63 @@ Elements DT1C13ARR - 80 + 96 16 ro + + DT6005 + 48 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + ActualPosition + DINT + 32 + 16 + + ro + T + + + + + DT7002 + 48 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + CommandedPosition + DINT + 32 + 16 + + ro + R + + + UDINT 32 @@ -506,6 +604,29 @@ ro + + #x1602 + StepGenIn1 + DT1602 + 48 + + + Max SubIndex + + 1 + + + + CommandedPosition + + #x70020120 + + + + + ro + + #x1A00 EncPos @@ -621,6 +742,29 @@ ro + + #x1A05 + StepgenOut1 + DT1A05 + 48 + + + Max SubIndex + + 1 + + + + ActualPosition + + #x60050120 + + + + + ro + + #x1C00 Sync Manager Communication Type @@ -666,12 +810,12 @@ #x1C12 Sync Manager 2 PDO Assignment DT1C12 - 48 + 64 Max SubIndex - 2 + 3 @@ -686,6 +830,12 @@ #x1601 + + PDO Mapping + + #x1602 + + ro @@ -695,12 +845,12 @@ #x1C13 Sync Manager 3 PDO Assignment DT1C13 - 96 + 112 Max SubIndex - 5 + 6 @@ -733,6 +883,12 @@ #x1A04 + + PDO Mapping + + #x1A05 + + ro @@ -803,6 +959,29 @@ T + + #x6005 + StepgenOut1 + DT6005 + 48 + + + Max SubIndex + + 1 + + + + ActualPosition + + 0 + + + + + ro + + #x7000 EncPosScale @@ -829,6 +1008,29 @@ R + + #x7002 + StepGenIn1 + DT7002 + 48 + + + Max SubIndex + + 1 + + + + CommandedPosition + + 0 + + + + + ro + + @@ -861,6 +1063,17 @@ UDINT + + #x1602 + StepGenIn1 + + #x7002 + #x1 + 32 + CommandedPosition + DINT + + #x1A00 EncPos @@ -916,6 +1129,17 @@ UDINT + + #x1A05 + StepgenOut1 + + #x6005 + #x1 + 32 + ActualPosition + DINT + + diff --git a/Pcb-1-lan9252/Firmware/lib/soes/ecat_options.h b/Pcb-1-lan9252/Firmware/lib/soes/ecat_options.h index e9ec844..20a7dba 100755 --- a/Pcb-1-lan9252/Firmware/lib/soes/ecat_options.h +++ b/Pcb-1-lan9252/Firmware/lib/soes/ecat_options.h @@ -33,8 +33,8 @@ #define SM3_smc 0x20 #define SM3_act 1 -#define MAX_MAPPINGS_SM2 2 -#define MAX_MAPPINGS_SM3 5 +#define MAX_MAPPINGS_SM2 3 +#define MAX_MAPPINGS_SM3 6 #define MAX_RXPDO_SIZE 512 #define MAX_TXPDO_SIZE 512 diff --git a/Pcb-1-lan9252/Firmware/lib/soes/esi.json b/Pcb-1-lan9252/Firmware/lib/soes/esi.json index aac4565..b30c7ad 100755 --- a/Pcb-1-lan9252/Firmware/lib/soes/esi.json +++ b/Pcb-1-lan9252/Firmware/lib/soes/esi.json @@ -104,6 +104,26 @@ "dtype": "UNSIGNED32", "value": "0", "data": "&Obj.IndexStatus" + }, + "6005": { + "otype": "RECORD", + "name": "StepgenOut1", + "access": "RO", + "items": [ + { + "name": "Max SubIndex" + }, + { + "name": "ActualPosition", + "dtype": "INTEGER32", + "data": "&Obj.StepgenOut1.ActualPosition", + "value": "0", + "access": "RO" + } + ], + "pdo_mappings": [ + "txpdo" + ] } }, "rxpdo": { @@ -129,6 +149,26 @@ "value": "0", "data": "&Obj.IndexLatchEnable" }, + "7002": { + "otype": "RECORD", + "name": "StepGenIn1", + "access": "RO", + "items": [ + { + "name": "Max SubIndex" + }, + { + "name": "CommandedPosition", + "dtype": "INTEGER32", + "data": "&Obj.StepGenIn1.CommandedPosition", + "value": "0", + "access": "RO" + } + ], + "pdo_mappings": [ + "rxpdo" + ] + }, "60664": { "otype": "VAR", "name": "ActualPosition", diff --git a/Pcb-1-lan9252/Firmware/lib/soes/objectlist.c b/Pcb-1-lan9252/Firmware/lib/soes/objectlist.c index 7c71c3c..45e6fd6 100755 --- a/Pcb-1-lan9252/Firmware/lib/soes/objectlist.c +++ b/Pcb-1-lan9252/Firmware/lib/soes/objectlist.c @@ -19,6 +19,9 @@ static const char acName1600_01[] = "EncPosScale"; static const char acName1601[] = "IndexLatchEnable"; static const char acName1601_00[] = "Max SubIndex"; 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 acName1A00[] = "EncPos"; static const char acName1A00_00[] = "Max SubIndex"; static const char acName1A00_01[] = "EncPos"; @@ -34,6 +37,9 @@ static const char acName1A03_01[] = "IndexByte"; static const char acName1A04[] = "IndexStatus"; static const char acName1A04_00[] = "Max SubIndex"; static const char acName1A04_01[] = "IndexStatus"; +static const char acName1A05[] = "StepgenOut1"; +static const char acName1A05_00[] = "Max SubIndex"; +static const char acName1A05_01[] = "ActualPosition"; static const char acName1C00[] = "Sync Manager Communication Type"; static const char acName1C00_00[] = "Max SubIndex"; static const char acName1C00_01[] = "Communications Type SM0"; @@ -44,6 +50,7 @@ static const char acName1C12[] = "Sync Manager 2 PDO Assignment"; static const char acName1C12_00[] = "Max SubIndex"; static const char acName1C12_01[] = "PDO Mapping"; static const char acName1C12_02[] = "PDO Mapping"; +static const char acName1C12_03[] = "PDO Mapping"; static const char acName1C13[] = "Sync Manager 3 PDO Assignment"; static const char acName1C13_00[] = "Max SubIndex"; static const char acName1C13_01[] = "PDO Mapping"; @@ -51,13 +58,20 @@ static const char acName1C13_02[] = "PDO Mapping"; static const char acName1C13_03[] = "PDO Mapping"; static const char acName1C13_04[] = "PDO Mapping"; static const char acName1C13_05[] = "PDO Mapping"; +static const char acName1C13_06[] = "PDO Mapping"; static const char acName6000[] = "EncPos"; static const char acName6001[] = "EncFrequency"; static const char acName6002[] = "DiffT"; static const char acName6003[] = "IndexByte"; static const char acName6004[] = "IndexStatus"; +static const char acName6005[] = "StepgenOut1"; +static const char acName6005_00[] = "Max SubIndex"; +static const char acName6005_01[] = "ActualPosition"; static const char acName7000[] = "EncPosScale"; static const char acName7001[] = "IndexLatchEnable"; +static const char acName7002[] = "StepGenIn1"; +static const char acName7002_00[] = "Max SubIndex"; +static const char acName7002_01[] = "CommandedPosition"; const _objd SDO1000[] = { @@ -93,6 +107,11 @@ const _objd SDO1601[] = {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1601_00, 1, NULL}, {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1601_01, 0x70010020, NULL}, }; +const _objd SDO1602[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL}, + {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1602_01, 0x70020120, NULL}, +}; const _objd SDO1A00[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A00_00, 1, NULL}, @@ -118,6 +137,11 @@ const _objd SDO1A04[] = {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A04_00, 1, NULL}, {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A04_01, 0x60040020, NULL}, }; +const _objd SDO1A05[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A05_00, 1, NULL}, + {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A05_01, 0x60050120, NULL}, +}; const _objd SDO1C00[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_00, 4, NULL}, @@ -128,18 +152,20 @@ const _objd SDO1C00[] = }; const _objd SDO1C12[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C12_00, 2, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C12_00, 3, NULL}, {0x01, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_01, 0x1600, NULL}, {0x02, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_02, 0x1601, NULL}, + {0x03, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_03, 0x1602, NULL}, }; const _objd SDO1C13[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 5, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 6, NULL}, {0x01, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_01, 0x1A00, NULL}, {0x02, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_02, 0x1A01, NULL}, {0x03, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_03, 0x1A02, NULL}, {0x04, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_04, 0x1A03, NULL}, {0x05, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_05, 0x1A04, NULL}, + {0x06, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_06, 0x1A05, NULL}, }; const _objd SDO6000[] = { @@ -161,6 +187,11 @@ const _objd SDO6004[] = { {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_TXPDO, acName6004, 0, &Obj.IndexStatus}, }; +const _objd SDO6005[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6005_00, 1, NULL}, + {0x01, DTYPE_INTEGER32, 32, ATYPE_RO, acName6005_01, 0, &Obj.StepgenOut1.ActualPosition}, +}; const _objd SDO7000[] = { {0x0, DTYPE_INTEGER32, 32, ATYPE_RO | ATYPE_RXPDO, acName7000, 0, &Obj.EncPosScale}, @@ -169,6 +200,11 @@ const _objd SDO7001[] = { {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_RXPDO, acName7001, 0, &Obj.IndexLatchEnable}, }; +const _objd SDO7002[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 1, NULL}, + {0x01, DTYPE_INTEGER32, 32, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition}, +}; const _objectlist SDOobjects[] = { @@ -179,20 +215,24 @@ 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}, {0x1A00, OTYPE_RECORD, 1, 0, acName1A00, SDO1A00}, {0x1A01, OTYPE_RECORD, 1, 0, acName1A01, SDO1A01}, {0x1A02, OTYPE_RECORD, 1, 0, acName1A02, SDO1A02}, {0x1A03, OTYPE_RECORD, 1, 0, acName1A03, SDO1A03}, {0x1A04, OTYPE_RECORD, 1, 0, acName1A04, SDO1A04}, + {0x1A05, OTYPE_RECORD, 1, 0, acName1A05, SDO1A05}, {0x1C00, OTYPE_ARRAY, 4, 0, acName1C00, SDO1C00}, - {0x1C12, OTYPE_ARRAY, 2, 0, acName1C12, SDO1C12}, - {0x1C13, OTYPE_ARRAY, 5, 0, acName1C13, SDO1C13}, + {0x1C12, OTYPE_ARRAY, 3, 0, acName1C12, SDO1C12}, + {0x1C13, OTYPE_ARRAY, 6, 0, acName1C13, SDO1C13}, {0x6000, OTYPE_VAR, 0, 0, acName6000, SDO6000}, {0x6001, OTYPE_VAR, 0, 0, acName6001, SDO6001}, {0x6002, OTYPE_VAR, 0, 0, acName6002, SDO6002}, {0x6003, OTYPE_VAR, 0, 0, acName6003, SDO6003}, {0x6004, OTYPE_VAR, 0, 0, acName6004, SDO6004}, + {0x6005, OTYPE_RECORD, 1, 0, acName6005, SDO6005}, {0x7000, OTYPE_VAR, 0, 0, acName7000, SDO7000}, {0x7001, OTYPE_VAR, 0, 0, acName7001, SDO7001}, + {0x7002, OTYPE_RECORD, 1, 0, acName7002, SDO7002}, {0xffff, 0xff, 0xff, 0xff, NULL, NULL} }; diff --git a/Pcb-1-lan9252/Firmware/lib/soes/utypes.h b/Pcb-1-lan9252/Firmware/lib/soes/utypes.h index aef373d..de80673 100755 --- a/Pcb-1-lan9252/Firmware/lib/soes/utypes.h +++ b/Pcb-1-lan9252/Firmware/lib/soes/utypes.h @@ -18,11 +18,19 @@ typedef struct uint32_t DiffT; uint32_t IndexByte; uint32_t IndexStatus; + struct + { + int32_t ActualPosition; + } StepgenOut1; /* Outputs */ int32_t EncPosScale; uint32_t IndexLatchEnable; + struct + { + int32_t CommandedPosition; + } StepGenIn1; } _Objects; diff --git a/Pcb-1-lan9252/Firmware/src/Stepper.cpp b/Pcb-1-lan9252/Firmware/src/Stepper.cpp index fe99446..518bf0a 100755 --- a/Pcb-1-lan9252/Firmware/src/Stepper.cpp +++ b/Pcb-1-lan9252/Firmware/src/Stepper.cpp @@ -71,7 +71,7 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim) PE9 ------> TIM1_CH1 PA8 ------< TIM1_CH1 */ - GPIO_InitStruct.Pin = GPIO_PIN_8; // 9; + GPIO_InitStruct.Pin = GPIO_PIN_11; // 9; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; @@ -97,7 +97,7 @@ void StepperSetup(void) // htim1.Instance->ARR = 1; // htim1.Instance->CCR1 = 1; - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); TIM_TypeDef *TIMM = TIM1; #define CLOCK_FREQ (168000000-2000000) diff --git a/Pcb-1-lan9252/Firmware/src/main.cpp b/Pcb-1-lan9252/Firmware/src/main.cpp index 315921c..9ecbc50 100755 --- a/Pcb-1-lan9252/Firmware/src/main.cpp +++ b/Pcb-1-lan9252/Firmware/src/main.cpp @@ -17,13 +17,11 @@ int64_t unwrap_encoder(uint16_t in, int64_t *prev); Encoder EncoderInit; Encoder *encP = &EncoderInit; -// #include "Stepper.h" +#include "Stepper.h" #define INDEX_PIN PA2 HardwareSerial Serial1(PA10, PA9); _Objects Obj; -volatile uint32_t sync0s = 0; - void indexPulse(void); double PosScaleRes = 1.0; uint32_t CurPosScale = 1; @@ -32,6 +30,8 @@ volatile uint8_t indexPulseFired = 0; uint32_t nFires = 0; volatile uint8_t pleaseZeroTheCounter = 0; +uint32_t sync0CycleTime = 0; + void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { if (Obj.IndexLatchEnable && !OldLatchCEnable) // Should only happen first time IndexCEnable is set @@ -57,7 +57,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation nFires++; PreviousEncoderCounterValue = 0; } - Obj.DiffT = sync0s; + Obj.DiffT = sync0CycleTime; int64_t pos = unwrap_encoder(TIM2->CNT, &PreviousEncoderCounterValue); double CurPos = pos * PosScaleRes; @@ -81,7 +81,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation void ESC_interrupt_enable(uint32_t mask); void ESC_interrupt_disable(uint32_t mask); -uint16_t dc_checker (void); +uint16_t dc_checker(void); static esc_cfg_t config = { @@ -108,6 +108,7 @@ void setup(void) Serial1.begin(115200); rcc_config(); + StepperSetup(); // Set starting count value EncoderInit.SetCount(Tim2, 0); // EncoderInit.SetCount(Tim3, 0); @@ -155,9 +156,27 @@ void indexPulse(void) } } -void countSync0(void) +volatile int32_t actualPosition = 0; +volatile int32_t requestedPosition; +volatile uint32_t pulsesToGo = 0; +volatile byte forwardDirection = 0; // 1 if going forward +#define STEPPER_DIR PA12 +//#define STEPPER_STEP PA11 // Set in StepperSetup + +void sync0Handler(void) { - sync0s++; + // Update the actual position + actualPosition += pulsesToGo; + Obj.StepgenOut1.ActualPosition = actualPosition; + // Get new end position + requestedPosition = Obj.StepGenIn1.CommandedPosition; + // Get the diff and the direction + pulsesToGo = requestedPosition - actualPosition; + forwardDirection = pulsesToGo > 0 ? 1 : 0; + // Set direction pin + digitalWrite(STEPPER_DIR, forwardDirection); // I think one should really wait a bit when changed + // Make the pulses using hardware timer + makePulses(sync0CycleTime/1000, pulsesToGo); } void ESC_interrupt_enable(uint32_t mask) @@ -170,7 +189,7 @@ void ESC_interrupt_enable(uint32_t mask) { ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); - attachInterrupt(digitalPinToInterrupt(PC3), countSync0, RISING); + attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING); // Set LAN9252 interrupt pin driver as push-pull active high uint32_t bits = 0x00000111; @@ -200,10 +219,12 @@ void ESC_interrupt_disable(uint32_t mask) } } -// Setup of DC -uint16_t dc_checker (void) +extern "C" uint32_t ESC_SYNC0cycletime(void); +// Setup of DC +uint16_t dc_checker(void) { - // Indicate we run DC + // Indicate we run DC ESCvar.dcsync = 1; + sync0CycleTime = ESC_SYNC0cycletime(); return 0; }