diff --git a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml index f3e06b9..26eb77a 100755 --- a/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml +++ b/Firmware/lib/soes/MetalMusings_EaserCAT_2000.xml @@ -125,7 +125,7 @@ DT1602 - 144 + 80 0 Max SubIndex @@ -146,12 +146,26 @@ ro + + + DT1603 + 80 - 2 - CommandedVelocity + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + CommandedPosition ULINT 64 - 80 + 16 ro @@ -301,6 +315,30 @@ + + DT1A06 + 80 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + ActualPosition + ULINT + 64 + 16 + + ro + + + DT1C00ARR USINT @@ -336,15 +374,15 @@ DT1C12ARR UINT - 48 + 64 1 - 3 + 4 DT1C12 - 64 + 80 0 Max SubIndex @@ -358,7 +396,7 @@ Elements DT1C12ARR - 48 + 64 16 ro @@ -368,15 +406,15 @@ DT1C13ARR UINT - 96 + 112 1 - 6 + 7 DT1C13 - 112 + 128 0 Max SubIndex @@ -390,7 +428,7 @@ Elements DT1C13ARR - 96 + 112 16 ro @@ -422,9 +460,34 @@ + + DT6006 + 80 + + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + ActualPosition + LREAL + 64 + 16 + + ro + T + + + DT7002 - 144 + 80 0 Max SubIndex @@ -446,12 +509,26 @@ R + + + DT7003 + 80 - 2 - CommandedVelocity + 0 + Max SubIndex + USINT + 8 + 0 + + ro + + + + 1 + CommandedPosition LREAL 64 - 80 + 16 ro R @@ -633,12 +710,12 @@ #x1602 StepGenIn1 DT1602 - 144 + 80 Max SubIndex - 2 + 1 @@ -647,10 +724,27 @@ #x70020140 + + + ro + + + + #x1603 + StepGenIn2 + DT1603 + 80 + - CommandedVelocity + Max SubIndex - #x70020240 + 1 + + + + CommandedPosition + + #x70030140 @@ -796,6 +890,29 @@ ro + + #x1A06 + StepGenOut2 + DT1A06 + 80 + + + Max SubIndex + + 1 + + + + ActualPosition + + #x60060140 + + + + + ro + + #x1C00 Sync Manager Communication Type @@ -841,12 +958,12 @@ #x1C12 Sync Manager 2 PDO Assignment DT1C12 - 64 + 80 Max SubIndex - 3 + 4 @@ -867,6 +984,12 @@ #x1602 + + PDO Mapping + + #x1603 + + ro @@ -876,12 +999,12 @@ #x1C13 Sync Manager 3 PDO Assignment DT1C13 - 112 + 128 Max SubIndex - 6 + 7 @@ -920,6 +1043,12 @@ #x1A05 + + PDO Mapping + + #x1A06 + + ro @@ -1013,6 +1142,29 @@ ro + + #x6006 + StepGenOut2 + DT6006 + 80 + + + Max SubIndex + + 1 + + + + ActualPosition + + 0 + + + + + ro + + #x7000 EncPosScale @@ -1043,12 +1195,12 @@ #x7002 StepGenIn1 DT7002 - 144 + 80 Max SubIndex - 2 + 1 @@ -1057,8 +1209,25 @@ 0 + + + ro + + + + #x7003 + StepGenIn2 + DT7003 + 80 + - CommandedVelocity + Max SubIndex + + 1 + + + + CommandedPosition 0 @@ -1110,11 +1279,15 @@ CommandedPosition LREAL + + + #x1603 + StepGenIn2 - #x7002 - #x2 + #x7003 + #x1 64 - CommandedVelocity + CommandedPosition LREAL @@ -1184,6 +1357,17 @@ LREAL + + #x1A06 + StepGenOut2 + + #x6006 + #x1 + 64 + ActualPosition + LREAL + + diff --git a/Firmware/lib/soes/ecat_options.h b/Firmware/lib/soes/ecat_options.h index 29f42a8..30684ae 100755 --- a/Firmware/lib/soes/ecat_options.h +++ b/Firmware/lib/soes/ecat_options.h @@ -34,7 +34,7 @@ #define SM3_act 1 #define MAX_MAPPINGS_SM2 4 -#define MAX_MAPPINGS_SM3 6 +#define MAX_MAPPINGS_SM3 7 #define MAX_RXPDO_SIZE 512 #define MAX_TXPDO_SIZE 512 diff --git a/Firmware/lib/soes/esi.json b/Firmware/lib/soes/esi.json index 1bb3c75..bd9e5a0 100755 --- a/Firmware/lib/soes/esi.json +++ b/Firmware/lib/soes/esi.json @@ -124,6 +124,26 @@ "pdo_mappings": [ "txpdo" ] + }, + "6006": { + "otype": "RECORD", + "name": "StepGenOut2", + "access": "RO", + "items": [ + { + "name": "Max SubIndex" + }, + { + "name": "ActualPosition", + "dtype": "REAL64", + "data": "&Obj.StepGenOut2.ActualPosition", + "value": "0", + "access": "RO" + } + ], + "pdo_mappings": [ + "txpdo" + ] } }, "rxpdo": { @@ -163,13 +183,26 @@ "data": "&Obj.StepGenIn1.CommandedPosition", "value": "0", "access": "RO" + } + ], + "pdo_mappings": [ + "rxpdo" + ] + }, + "7003": { + "otype": "RECORD", + "name": "StepGenIn2", + "access": "RO", + "items": [ + { + "name": "Max SubIndex" }, { - "name": "CommandedVelocity", + "name": "CommandedPosition", "dtype": "REAL64", + "data": "&Obj.StepGenIn2.CommandedPosition", "value": "0", - "access": "RO", - "data": "&Obj.StepGenIn1.CommandedVelocity" + "access": "RO" } ], "pdo_mappings": [ diff --git a/Firmware/lib/soes/objectlist.c b/Firmware/lib/soes/objectlist.c index d3e0869..93d4d8d 100755 --- a/Firmware/lib/soes/objectlist.c +++ b/Firmware/lib/soes/objectlist.c @@ -22,7 +22,9 @@ 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[] = "CommandedVelocity"; +static const char acName1603[] = "StepGenIn2"; +static const char acName1603_00[] = "Max SubIndex"; +static const char acName1603_01[] = "CommandedPosition"; static const char acName1A00[] = "EncPos"; static const char acName1A00_00[] = "Max SubIndex"; static const char acName1A00_01[] = "EncPos"; @@ -41,6 +43,9 @@ 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 acName1A06[] = "StepGenOut2"; +static const char acName1A06_00[] = "Max SubIndex"; +static const char acName1A06_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"; @@ -52,6 +57,7 @@ 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 acName1C12_04[] = "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"; @@ -60,6 +66,7 @@ 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 acName1C13_07[] = "PDO Mapping"; static const char acName6000[] = "EncPos"; static const char acName6001[] = "EncFrequency"; static const char acName6002[] = "DiffT"; @@ -68,12 +75,17 @@ 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 acName6006[] = "StepGenOut2"; +static const char acName6006_00[] = "Max SubIndex"; +static const char acName6006_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"; -static const char acName7002_02[] = "CommandedVelocity"; +static const char acName7003[] = "StepGenIn2"; +static const char acName7003_00[] = "Max SubIndex"; +static const char acName7003_01[] = "CommandedPosition"; const _objd SDO1000[] = { @@ -111,9 +123,13 @@ const _objd SDO1601[] = }; const _objd SDO1602[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 2, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL}, {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_01, 0x70020140, NULL}, - {0x02, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1602_02, 0x70020240, NULL}, +}; +const _objd SDO1603[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 1, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1603_01, 0x70030140, NULL}, }; const _objd SDO1A00[] = { @@ -145,6 +161,11 @@ const _objd SDO1A05[] = {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A05_00, 1, NULL}, {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A05_01, 0x60050140, NULL}, }; +const _objd SDO1A06[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A06_00, 1, NULL}, + {0x01, DTYPE_UNSIGNED64, 64, ATYPE_RO, acName1A06_01, 0x60060140, NULL}, +}; const _objd SDO1C00[] = { {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_00, 4, NULL}, @@ -155,20 +176,22 @@ const _objd SDO1C00[] = }; const _objd SDO1C12[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C12_00, 3, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C12_00, 4, 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}, + {0x04, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_04, 0x1603, NULL}, }; const _objd SDO1C13[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 6, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 7, 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}, + {0x07, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_07, 0x1A06, NULL}, }; const _objd SDO6000[] = { @@ -195,6 +218,11 @@ const _objd SDO6005[] = {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6005_00, 1, NULL}, {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName6005_01, 0, &Obj.StepGenOut1.ActualPosition}, }; +const _objd SDO6006[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName6006_00, 1, NULL}, + {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName6006_01, 0, &Obj.StepGenOut2.ActualPosition}, +}; const _objd SDO7000[] = { {0x0, DTYPE_INTEGER32, 32, ATYPE_RO | ATYPE_RXPDO, acName7000, 0, &Obj.EncPosScale}, @@ -205,9 +233,13 @@ const _objd SDO7001[] = }; const _objd SDO7002[] = { - {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 2, NULL}, + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7002_00, 1, NULL}, {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7002_01, 0, &Obj.StepGenIn1.CommandedPosition}, - {0x02, DTYPE_REAL64, 64, ATYPE_RO, acName7002_02, 0, &Obj.StepGenIn1.CommandedVelocity}, +}; +const _objd SDO7003[] = +{ + {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName7003_00, 1, NULL}, + {0x01, DTYPE_REAL64, 64, ATYPE_RO, acName7003_01, 0, &Obj.StepGenIn2.CommandedPosition}, }; const _objectlist SDOobjects[] = @@ -219,24 +251,28 @@ 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, 2, 0, acName1602, SDO1602}, + {0x1602, OTYPE_RECORD, 1, 0, acName1602, SDO1602}, + {0x1603, OTYPE_RECORD, 1, 0, acName1603, SDO1603}, {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}, + {0x1A06, OTYPE_RECORD, 1, 0, acName1A06, SDO1A06}, {0x1C00, OTYPE_ARRAY, 4, 0, acName1C00, SDO1C00}, - {0x1C12, OTYPE_ARRAY, 3, 0, acName1C12, SDO1C12}, - {0x1C13, OTYPE_ARRAY, 6, 0, acName1C13, SDO1C13}, + {0x1C12, OTYPE_ARRAY, 4, 0, acName1C12, SDO1C12}, + {0x1C13, OTYPE_ARRAY, 7, 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}, + {0x6006, OTYPE_RECORD, 1, 0, acName6006, SDO6006}, {0x7000, OTYPE_VAR, 0, 0, acName7000, SDO7000}, {0x7001, OTYPE_VAR, 0, 0, acName7001, SDO7001}, - {0x7002, OTYPE_RECORD, 2, 0, acName7002, SDO7002}, + {0x7002, OTYPE_RECORD, 1, 0, acName7002, SDO7002}, + {0x7003, OTYPE_RECORD, 1, 0, acName7003, SDO7003}, {0xffff, 0xff, 0xff, 0xff, NULL, NULL} }; diff --git a/Firmware/lib/soes/utypes.h b/Firmware/lib/soes/utypes.h index 09b8193..a85f8ac 100755 --- a/Firmware/lib/soes/utypes.h +++ b/Firmware/lib/soes/utypes.h @@ -22,6 +22,10 @@ typedef struct { double ActualPosition; } StepGenOut1; + struct + { + double ActualPosition; + } StepGenOut2; /* Outputs */ @@ -30,8 +34,11 @@ typedef struct struct { double CommandedPosition; - double CommandedVelocity; } StepGenIn1; + struct + { + double CommandedPosition; + } StepGenIn2; } _Objects; diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 192f0a4..7f9dccc 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -32,14 +32,12 @@ void timerCallbackStep1(void) { Step1.timerCB(); } -#if 0 void timerCallbackStep2(void); StepGen Step2(TIM8, 4, PC9, PC10, timerCallbackStep2); void timerCallbackStep2(void) { Step2.timerCB(); } -#endif void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { @@ -47,11 +45,13 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera Encoder1.setScale(Obj.EncPosScale); Step1.cmdPos(Obj.StepGenIn1.CommandedPosition); + Step2.cmdPos(Obj.StepGenIn2.CommandedPosition); } void handleStepper(void) { Step1.handleStepper(); + Step2.handleStepper(); } void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation @@ -62,6 +62,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation Obj.IndexByte = Encoder1.getIndexState(); Obj.StepGenOut1.ActualPosition = Step1.actPos(); + Obj.StepGenOut2.ActualPosition = Step2.actPos(); Obj.DiffT = 10000 * Step1.reqPos(); // Debug } @@ -97,7 +98,8 @@ void setup(void) Serial1.begin(115200); rcc_config(); - Step1.setScale(500); + Step1.setScale(1250); // 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); } @@ -164,7 +166,6 @@ uint16_t dc_checker(void) { // Indicate we run DC ESCvar.dcsync = 0; - //Step1.setCycleTime(ESC_SYNC0cycletime() / 1000); // nsec to usec StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000; return 0; }