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 );