diff --git a/stm32-stepper-root/stepper_h7/CM7/Core/Src/Stepper/stepper.c b/stm32-stepper-root/stepper_h7/CM7/Core/Src/Stepper/stepper.c
index 194b5de..cc8675e 100644
--- a/stm32-stepper-root/stepper_h7/CM7/Core/Src/Stepper/stepper.c
+++ b/stm32-stepper-root/stepper_h7/CM7/Core/Src/Stepper/stepper.c
@@ -23,9 +23,16 @@ static L6474_Handle_t stepperHandle = NULL;
*/
int state = 0;
+float posmax = 29.5;
+float posmin = 0.0f;
+float posref = 0.0f;
+// eingestellter Step Mode: 1, 2, 4, 8 oder 16
+int microsteps = 16;
+
+// 50 Vollschritte pro mm, bei 1/16 Schritt also 800 Schritte pro mm
int mm_to_step(float mm){
- return (int)(800 * mm);
+ return (int)(50 * microsteps * mm);
}
static void* StepLibraryMalloc( unsigned int size ){
@@ -71,6 +78,9 @@ static int StepperHasFault(L6474_Handle_t s){
// Pulsfrequenz einstellen, nur aufrufen wenn gerade keine Pulse laufen
static void StepTimerSetSpeed(unsigned int hz){
+ // hz gilt fuer 1/16 Schritt, bei anderem Step Mode gleiche mm/min halten
+ hz = hz * microsteps / 16;
+
uint32_t arr = (STEP_TICK_HZ / hz) - 1;
stepHz = hz;
@@ -114,15 +124,45 @@ void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim){
portYIELD_FROM_ISR(woken);
}
+// Name des Config-Parameters in die Nummer fuer config_stepper wandeln, 0 wenn unbekannt
+static int config_param_id(const char* name){
+ if(strcmp(name, "torque") == 0){
+ return 1;
+ }else if(strcmp(name, "throvercurr") == 0){
+ return 2;
+ }else if(strcmp(name, "powerena") == 0){
+ return 3;
+ }else if(strcmp(name, "stepmode") == 0){
+ return 4;
+ }else if(strcmp(name, "timeoff") == 0){
+ return 5;
+ }else if(strcmp(name, "timeon") == 0){
+ return 6;
+ }else if(strcmp(name, "timefast") == 0){
+ return 7;
+ }else if(strcmp(name, "mmperturn") == 0){
+ return 8;
+ }else if(strcmp(name, "posmax") == 0){
+ return 9;
+ }else if(strcmp(name, "posmin") == 0){
+ return 10;
+ }else if(strcmp(name, "posref") == 0){
+ return 11;
+ }else if(strcmp(name, "stepsperturn") == 0){
+ return 12;
+ }
+ return 0;
+}
+
static int config_stepper(L6474_Handle_t s, int param, float value){
/*
- * param 1: torque
- * param 2: throvercurr
- * param 3: powerena
- * param 4: stepmode
- * param 5: timeoff
- * param 6: timeon
- * param 7: timefast
+ * param 1: torque (mA)
+ * param 2: throvercurr (mA)
+ * param 3: powerena (0 oder 1)
+ * param 4: stepmode (1, 2, 4, 8, 16)
+ * param 5: timeoff (us)
+ * param 6: timeon (us)
+ * param 7: timefast (us)
* param 8: mmperturn
* param 9: posmax
* param 10: posmin
@@ -130,32 +170,171 @@ static int config_stepper(L6474_Handle_t s, int param, float value){
* param 12: stepsperturn
*/
+ int reg = 0;
+ int result = 0;
+
switch(param){
case 1:
- L6474_SetProperty(s, L6474_PROP_TORQUE, L6474_EncodePhaseCurrent(value));
+ result = L6474_SetProperty(s, L6474_PROP_TORQUE, L6474_EncodePhaseCurrent(value));
+ break;
case 2:
- L6474_SetProperty(s, L6474_PROP_OCDTH, value);
- //TODO value muss noch in Enum L6474x_OCD_TH_t gewandelt werden
+ // 375 mA pro Stufe, Register 0 = 375 mA
+ if(value < 375 || value > 6000){
+ return 1;
+ }
+ reg = (int)(value / 375) - 1;
+ result = L6474_SetProperty(s, L6474_PROP_OCDTH, reg);
+ break;
case 3:
if(value == 1){
- HAL_GPIO_WritePin(STEP_RSTN_GPIO_Port, STEP_RSTN_Pin, GPIO_PIN_SET);
+ result = L6474_SetPowerOutputs(s, 1);
}else{
- HAL_GPIO_WritePin(STEP_RSTN_GPIO_Port, STEP_RSTN_Pin, GPIO_PIN_RESET);
+ result = L6474_SetPowerOutputs(s, 0);
}
+ break;
case 4:
+ if(value == 1){
+ result = L6474_SetStepMode(s, smFULL);
+ }else if(value == 2){
+ result = L6474_SetStepMode(s, smHALF);
+ }else if(value == 4){
+ result = L6474_SetStepMode(s, smMICRO4);
+ }else if(value == 8){
+ result = L6474_SetStepMode(s, smMICRO8);
+ }else if(value == 16){
+ result = L6474_SetStepMode(s, smMICRO16);
+ }else{
+ return 1;
+ }
+
+ if(result == errcNONE){
+ microsteps = (int)value;
+ // Position stimmt nicht mehr, neue Referenzfahrt noetig
+ if(state == 2){
+ state = 1;
+ }
+ }
+ break;
case 5:
+ // 0.5 us pro Stufe, Register 0 = 0.5 us
+ if(value < 0.5f || value > 64){
+ return 1;
+ }
+ reg = (int)(value * 2) - 1;
+ result = L6474_SetProperty(s, L6474_PROP_TOFF, reg);
+ break;
case 6:
+ // 0.5 us pro Stufe, Register 0 = 0.5 us
+ if(value < 0.5f || value > 64){
+ return 1;
+ }
+ reg = (int)(value * 2) - 1;
+ result = L6474_SetProperty(s, L6474_PROP_TON, reg);
+ break;
case 7:
- case 8:
+ // 2 us pro Stufe, Register 0 = 2 us
+ // obere 4 Bit: TOFF_FAST (wird gesetzt), untere 4 Bit: FAST_STEP (bleibt)
+ if(value < 2 || value > 32){
+ return 1;
+ }
+ L6474_GetProperty(s, L6474_PROP_TFAST, ®);
+ reg = (((int)(value / 2) - 1) << 4) | (reg & 0x0F);
+ result = L6474_SetProperty(s, L6474_PROP_TFAST, reg);
+ break;
case 9:
+ posmax = value;
+ result = errcNONE;
+ break;
case 10:
+ posmin = value;
+ result = errcNONE;
+ break;
case 11:
- case 12:
+ posref = value;
+ result = errcNONE;
+ break;
default:
+ //TODO param 8 bis 12
return 1;
}
+ if(result != errcNONE){
+ return 1;
+ }
+ return 0;
+}
+static int config_stepper_get(L6474_Handle_t s, int param, float* value){
+ //gleiche param Nummern und Einheiten wie bei config_stepper
+
+ int reg = 0;
+ int result = 0;
+ L6474x_State_t driverState;
+ L6474x_StepMode_t mode;
+
+ switch(param){
+ case 1:
+ result = L6474_GetProperty(s, L6474_PROP_TORQUE, ®);
+ *value = (reg + 1) * 31.25f;
+ break;
+ case 2:
+ result = L6474_GetProperty(s, L6474_PROP_OCDTH, ®);
+ *value = (reg + 1) * 375;
+ break;
+ case 3:
+ result = L6474_GetState(s, &driverState);
+ if(driverState == stENABLED){
+ *value = 1;
+ }else{
+ *value = 0;
+ }
+ break;
+ case 4:
+ result = L6474_GetStepMode(s, &mode);
+ if(mode == smFULL){
+ *value = 1;
+ }else if(mode == smHALF){
+ *value = 2;
+ }else if(mode == smMICRO4){
+ *value = 4;
+ }else if(mode == smMICRO8){
+ *value = 8;
+ }else{
+ *value = 16;
+ }
+ break;
+ case 5:
+ result = L6474_GetProperty(s, L6474_PROP_TOFF, ®);
+ *value = (reg + 1) * 0.5f;
+ break;
+ case 6:
+ result = L6474_GetProperty(s, L6474_PROP_TON, ®);
+ *value = (reg + 1) * 0.5f;
+ break;
+ case 7:
+ result = L6474_GetProperty(s, L6474_PROP_TFAST, ®);
+ *value = ((reg >> 4) + 1) * 2;
+ break;
+ case 9:
+ *value = posmax;
+ result = errcNONE;
+ break;
+ case 10:
+ *value = posmin;
+ result = errcNONE;
+ case 11:
+ *value = posref;
+ result = errcNONE;
+ break;
+ default:
+ //TODO param 8 bis 12
+ return 1;
+ }
+
+ if(result != errcNONE){
+ return 1;
+ }
+ return 0;
}
static int StepLibraryStep( void* pPWM, int dir, unsigned int numPulses ){
@@ -227,6 +406,7 @@ int reset_stepper(L6474_Handle_t s){
L6474_BaseParameter_t b;
L6474_SetBaseParameter(&b);
b.stepMode = smMICRO16;
+ microsteps = 16;
b.OcdTh = ocdth1125mA;
L6474_EncodePhaseCurrentParameter(&b, 700.0f);
@@ -287,7 +467,7 @@ int stepper_reference(L6474_Handle_t s, int flag){
}
- L6474_SetAbsolutePosition(s, 0);
+ L6474_SetAbsolutePosition(s, mm_to_step(posref));
return StepperHasFault(s);
@@ -302,7 +482,7 @@ int stepper_pos(L6474_Handle_t s){
if(L6474_GetAbsolutePosition(s, &position) != errcNONE){
return 1;
}
- printf("%d", (position/800));
+ printf("%d", (position/(50 * microsteps)));
return StepperHasFault(s);
}
@@ -456,7 +636,7 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx )
}
float target_mm = (float)atof(argv[1]);
- if (target_mm < 0.0f || target_mm > 100.0f) {
+ if (target_mm < posmin || target_mm > posmax) {
printf("position out of range\r\n");
printf("FAIL\r\n");
return -1;
@@ -474,113 +654,130 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx )
return 0;
}
else if (strcmp(argv[0], "status") == 0)
- {
- L6474x_State_t currentState;
- L6474_Status_t stepperStatus;
- int moving = 0;
+ {
+ L6474x_State_t currentState;
+ L6474_Status_t stepperStatus;
+ int moving = 0;
- // 1. get state of state machine
- if (L6474_GetState(h, ¤tState) != errcNONE)
- {
- printf("failed to get state\r\nFAIL");
- return -1;
- }
+ // 1. get state of state machine
+ if (L6474_GetState(h, ¤tState) != errcNONE)
+ {
+ printf("failed to get state\r\nFAIL");
+ return -1;
+ }
- // 2.get driver-state
- if (L6474_GetStatus(h, &stepperStatus) != errcNONE)
- {
- printf("failed to get status\r\nFAIL");
- return -1;
- }
+ // 2.get driver-state
+ if (L6474_GetStatus(h, &stepperStatus) != errcNONE)
+ {
+ printf("failed to get status\r\nFAIL");
+ return -1;
+ }
- unsigned int errorBits = ((stepperStatus.WRONG_CMD & 0x01) << 8) |
- ((stepperStatus.UVLO & 0x01) << 7) |
- ((stepperStatus.TH_WARN & 0x01) << 6) |
- ((stepperStatus.TH_SD & 0x01) << 5) |
- ((stepperStatus.ONGOING & 0x01) << 4) |
- ((stepperStatus.OCD & 0x01) << 3) |
- ((stepperStatus.NOTPERF_CMD & 0x01) << 2) |
- ((stepperStatus.HIGHZ & 0x01) << 1) |
- ((stepperStatus.DIR & 0x01) << 0);
+ unsigned int errorBits = ((stepperStatus.WRONG_CMD & 0x01) << 8) |
+ ((stepperStatus.UVLO & 0x01) << 7) |
+ ((stepperStatus.TH_WARN & 0x01) << 6) |
+ ((stepperStatus.TH_SD & 0x01) << 5) |
+ ((stepperStatus.ONGOING & 0x01) << 4) |
+ ((stepperStatus.OCD & 0x01) << 3) |
+ ((stepperStatus.NOTPERF_CMD & 0x01) << 2) |
+ ((stepperStatus.HIGHZ & 0x01) << 1) |
+ ((stepperStatus.DIR & 0x01) << 0);
- // 3. check for moving motor aka async pending operation (should be always 0 with no async mode)
- L6474_IsMoving(h, &moving);
+ // 3. check for moving motor aka async pending operation (should be always 0 with no async mode)
+ L6474_IsMoving(h, &moving);
- printf("0x%X\r\n", (unsigned int)currentState); //print state in first line
- printf("0x%X\r\n", errorBits); //print error flags in second line
- printf("internal error bits in binary: ");
- print_binary_8bit(errorBits);
- printf("%d\r\n", moving); //print async pending op in third line
- printf("OK");
+ //TEST CODE START
- return 0;
- }
+ unsigned int mappedState = 0x0;
+
+ // check for error (would print 0x8)
+ if (errorBits != 0) {
+ mappedState = 0x8; // scsFLT
+ }
+ else {
+ switch (currentState) {
+ case stRESET:
+ mappedState = 0x1; //map stRESET to scsREF?
+ break;
+
+ case stDISABLED:
+ mappedState = 0x2; //map stDISABLED to scsDIS (driver ready, otuput off)?
+ break;
+
+ case stENABLED:
+ mappedState = 0x4; //map stENABLED to scsENA --> driver active + output on
+ break;
+
+ default:
+ mappedState = 0x0; //entsprciht scsInit
+ break;
+ }
+ }
+
+ printf("0x%X\r\n", mappedState); //print state machine state in first line
+
+ //TEST CODE END
+
+
+ //printf("0x%X\r\n", (unsigned int)currentState); //print state in first line
+ printf("0x%X\r\n", errorBits); //print error flags in second line
+ printf("internal error bits in binary: ");
+ print_binary_8bit(errorBits);
+
+ printf("%d\r\n", moving); //print async pending op in third line
+ printf("OK");
+
+ return 0;
+ }
else if (strcmp(argv[0], "config") == 0)
{
- float value = atof(argv[2]);
if ( argc < 2 ){
-
- printf("Too few arguments\r\n");
+ printf("missing parameter name for config command\r\n");
printf("FAIL\r\n");
- return 1;
+ return -1;
+ }
+ int param = config_param_id(argv[1]);
+
+ if(param == 0){
+ printf("unknown config parameter\r\n");
+ printf("FAIL\r\n");
+ return -1;
}
- switch(atoi(argv[1])){
- case 1:
- config_stepper(h, 1, value);
- printf("OK\r\n");
- return 0;
- case 2:
- config_stepper(h, 2, value);
- printf("OK\r\n");
- return 0;
- case 3:
- config_stepper(h, 3, value);
- printf("OK\r\n");
- return 0;
- case 4:
- config_stepper(h, 4, value);
- printf("OK\r\n");
- return 0;
- case 5:
- config_stepper(h, 5, value);
- printf("OK\r\n");
- return 0;
- case 6:
- config_stepper(h, 6, value);
- printf("OK\r\n");
- return 0;
- case 7:
- config_stepper(h, 7, value);
- printf("OK\r\n");
- return 0;
- case 8:
- config_stepper(h, 8, value);
- printf("OK\r\n");
- return 0;
- case 9:
- config_stepper(h, 9, value);
- printf("OK\r\n");
- return 0;
- case 10:
- config_stepper(h, 10, value);
- printf("OK\r\n");
- return 0;
- case 11:
- config_stepper(h, 11, value);
- printf("OK\r\n");
- return 0;
- case 12:
- config_stepper(h, 12, value);
- printf("OK\r\n");
- return 0;
- default:
+
+ // Ohne Flag: Wert lesen -> config
+ if ( argc == 2 ){
+ float value = 0.0f;
+
+ if(config_stepper_get(h, param, &value) != 0){
printf("FAIL\r\n");
- return 1;
+ return -1;
+ }
+
+ printf("%.2f\r\n", value);
+ printf("OK\r\n");
+ return 0;
}
+
+ // Mit Flag: Wert schreiben -> config -v
+ if ( argc >= 4 && strcmp(argv[2], "-v") == 0 ){
+ float value = (float)atof(argv[3]);
+
+ if(config_stepper(h, param, value) != 0){
+ printf("FAIL\r\n");
+ return -1;
+ }
+
+ printf("OK\r\n");
+ return 0;
+ }
+
+ printf("usage: config [-v ]\r\n");
+ printf("FAIL\r\n");
+ return -1;
}
else
diff --git a/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c b/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c
index 7616e67..a93d133 100644
--- a/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c
+++ b/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c
@@ -183,20 +183,20 @@ static int CapabilityFunc( int argc, char** argv, void* ctx )
0, // has stepper refrun timeout
0, // has stepper refrun skip
0, // has stepper refrun stay enabled
- 0, // has stepper reset
+ 1, // has stepper reset
1, // has stepper position
- 0, // has stepper config
- 0, // has stepper config torque
- 0, // has stepper config throvercurr
- 0, // has stepper config powerena
- 0, // has stepper config stepmode
- 0, // has stepper config timeoff
- 0, // has stepper config timeon
- 0, // has stepper config timefast
+ 1, // has stepper config
+ 2, // has stepper config torque
+ 1, // has stepper config throvercurr
+ 1, // has stepper config powerena
+ 1, // has stepper config stepmode
+ 1, // has stepper config timeoff
+ 1, // has stepper config timeon
+ 1, // has stepper config timefast
0, // has stepper config mmperturn
- 0, // has stepper config posmax
- 0, // has stepper config posmin
- 0, // has stepper config posref
+ 1, // has stepper config posmax
+ 1, // has stepper config posmin
+ 1, // has stepper config posref
0, // has stepper config stepsperturn
0 // has stepper cancel
);