diff --git a/stm32-stepper-root/stepper_h7/CM7/Core/Inc/main.h b/stm32-stepper-root/stepper_h7/CM7/Core/Inc/main.h index 1c009c6..9b27dfa 100644 --- a/stm32-stepper-root/stepper_h7/CM7/Core/Inc/main.h +++ b/stm32-stepper-root/stepper_h7/CM7/Core/Inc/main.h @@ -108,11 +108,15 @@ void Error_Handler(void); extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim4; +extern TIM_HandleTypeDef htim3; +extern TIM_HandleTypeDef htim12; extern SPI_HandleTypeDef hspi1; void green_led(); void red_led(); void blink_yellow_led(); +void yellow_led_start(); +void yellow_led_stop(); /* USER CODE END Private defines */ #ifdef __cplusplus 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 80cebf7..59355af 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 @@ -8,10 +8,12 @@ #define STEP_CHUNK 60000u #define STEP_HZ_MOVE 4000u // 300 mm/min bei 800 Schritten/mm #define STEP_HZ_REFERENCE 500u // 37,5 mm/min bei 800 Schritten/mm +#define STEP_POLL_MS 50u // Treiberstatus-Abfrage waehrend der Fahrt static SemaphoreHandle_t stepDone; static volatile unsigned int stepsLeft = 0; static unsigned int stepHz = STEP_HZ_MOVE; +static L6474_Handle_t stepperHandle = NULL; /* * States: @@ -56,6 +58,18 @@ void StepLibraryDelay(unsigned int ms){ vTaskDelay(pdMS_TO_TICKS(ms)); } +// 1 wenn der Treiber nicht fahrbereit ist. L6474_GetStatus selbst meldet nur SPI-Fehler, +// die Flags muessen hier ausgewertet werden. HIGHZ bleibt nach OCD/UVLO/TH_SD stehen. +// Der Aufruf aus StepLibraryStep setzt LIBL6474_HAS_LOCKING 0 voraus. +static int StepperHasFault(L6474_Handle_t s){ + L6474_Status_t status; + + if(L6474_GetStatus(s, &status) != errcNONE){ + return 1; + } + return status.HIGHZ || status.UVLO || status.TH_SD || status.OCD; +} + // Pulsfrequenz einstellen, nur aufrufen wenn gerade keine Pulse laufen static void StepTimerSetSpeed(unsigned int hz){ uint32_t arr = (STEP_TICK_HZ / hz) - 1; @@ -68,7 +82,13 @@ static void StepTimerSetSpeed(unsigned int hz){ // TIM1 auf die naechsten n Pulse stellen und starten, das oeffnet das Gate von TIM4 static void StepTimerStartChunk(void){ - unsigned int n = (stepsLeft > STEP_CHUNK) ? STEP_CHUNK : stepsLeft; + unsigned int n; + + if(stepsLeft > STEP_CHUNK){ + n = STEP_CHUNK; + }else{ + n = stepsLeft; + } stepsLeft -= n; TIM1->CNT = 0; @@ -111,11 +131,15 @@ static int StepLibraryStep( void* pPWM, int dir, unsigned int numPulses ){ // Erwartete Fahrzeit plus 1 s Reserve uint32_t timeoutMs = (uint32_t)(((uint64_t)numPulses * 1000u) / stepHz) + 1000u; + TickType_t start = xTaskGetTickCount(); - if(xSemaphoreTake(stepDone, pdMS_TO_TICKS(timeoutMs)) != pdTRUE){ - TIM1->CR1 &= ~TIM_CR1_CEN; - stepsLeft = 0; - return 1; + // Bei Fehler oder Timeout Pulse sofort stoppen + while(xSemaphoreTake(stepDone, pdMS_TO_TICKS(STEP_POLL_MS)) != pdTRUE){ + if(StepperHasFault(stepperHandle) || (xTaskGetTickCount() - start) >= pdMS_TO_TICKS(timeoutMs)){ + stepsLeft = 0; // zuerst, damit der Interrupt keinen neuen Chunk startet + TIM1->CR1 &= ~TIM_CR1_CEN; + return 1; + } } return 0; } @@ -147,6 +171,7 @@ L6474_Handle_t init_stepper(void* pIO, void* pGPO, void* pPWM){ //p.stepAsync = StepTimerAsync; //p.cancelStep = StepTimerCancelAsync; L6474_Handle_t s = L6474_CreateInstance(&p, pIO, pGPO, pPWM); + stepperHandle = s; return s; } @@ -165,17 +190,25 @@ int reset_stepper(L6474_Handle_t s){ result |= L6474_ResetStandBy(s); result |= L6474_Initialize(s, &b); result |= L6474_SetPowerOutputs(s, 0); + yellow_led_stop(); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET); + return result; } int stepper_reference(L6474_Handle_t s, int flag){ - L6474_SetPowerOutputs(s, 1); + if(L6474_SetPowerOutputs(s, 1) != errcNONE){ + return 1; + } + yellow_led_start(); //flag: 0 keiner, 1 -t, 2 -e, 3 -s if(HAL_GPIO_ReadPin(REFERENCE_MARK_GPIO_Port, REFERENCE_MARK_Pin) == GPIO_PIN_RESET){ - L6474_StepIncremental(s, mm_to_step(5)); + if(L6474_StepIncremental(s, mm_to_step(5)) != errcNONE){ + return 1; + } int button_pressed = 0; @@ -194,18 +227,14 @@ int stepper_reference(L6474_Handle_t s, int flag){ if(flag == 3){ L6474_SetAbsolutePosition(s, 0); - L6474_Status_t status; - if(L6474_GetStatus(s, &status) != errcNONE){ - return 1; - } - - - return 0; + return StepperHasFault(s); } int button_pressed = 0; while(!button_pressed){ - L6474_StepIncremental(s, -1); + if(L6474_StepIncremental(s, -1) != errcNONE){ + return 1; + } if(HAL_GPIO_ReadPin(REFERENCE_MARK_GPIO_Port, REFERENCE_MARK_Pin) == GPIO_PIN_RESET){ button_pressed=1; } @@ -214,7 +243,7 @@ int stepper_reference(L6474_Handle_t s, int flag){ } L6474_SetAbsolutePosition(s, 0); - return 0; + return StepperHasFault(s); } @@ -224,9 +253,11 @@ void stepper_softstart(L6474_Handle_t s, unsigned int duration){ int stepper_pos(L6474_Handle_t s){ int position = 0; - L6474_GetAbsolutePosition(s, &position); + if(L6474_GetAbsolutePosition(s, &position) != errcNONE){ + return 1; + } printf("Stepper position: %d", position); - return 0; + return StepperHasFault(s); } int stepper_status(L6474_Handle_t s){ @@ -245,16 +276,12 @@ int stepper_move(L6474_Handle_t s, int pos){ int pos_diff = pos - position; - if(L6474_StepIncremental(s, pos_diff) != errcNONE){ + // StepIncremental lehnt 0 Schritte als Fehler ab + if(pos_diff != 0 && L6474_StepIncremental(s, pos_diff) != errcNONE){ return 1; } - L6474_Status_t status; - if(L6474_GetStatus(s, &status) != errcNONE){ - return 1; - } - - return 0; + return StepperHasFault(s); } @@ -289,20 +316,32 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx ) return -1; } - L6474_StepIncremental(h, atoi(argv[1])); + int steps = atoi(argv[1]); - printf("%d steps", (int)atoi(argv[1])); + if((steps != 0 && L6474_StepIncremental(h, steps) != errcNONE) || StepperHasFault(h)){ + state = 0; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); + printf("FAIL\r\n"); + return -1; + } + + printf("%d steps", steps); } else if ( strcmp(argv[0], "reset") == 0 ) { if(reset_stepper(h)){ state = 0; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); + yellow_led_stop(); printf("FAIL\r\n"); return -1; } state = 1; + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET); printf("OK\r\n"); return 0; } @@ -320,6 +359,9 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx ) if(refResult){ state = 0; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); + yellow_led_stop(); printf("FAIL\r\n"); return -1; } @@ -338,6 +380,9 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx ) if(stepper_pos(h)){ state = 0; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); + yellow_led_stop(); printf("FAIL\r\n"); return -1; } @@ -369,6 +414,9 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx ) if(stepper_move(h, mm_to_step(target_mm))==1){ state = 0; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); + yellow_led_stop(); printf("FAIL\r\n"); return -1; } 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 39bd802..7616e67 100644 --- a/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c +++ b/stm32-stepper-root/stepper_h7/CM7/Core/Src/main.c @@ -63,6 +63,11 @@ TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim4; +//Für LEDS +TIM_HandleTypeDef htim3; +TIM_HandleTypeDef htim12; + + UART_HandleTypeDef huart3; @@ -119,8 +124,11 @@ void green_led(){ } } +static TaskHandle_t yellowLedTask = NULL; + void blink_yellow_led(){ while(1){ + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_SET); vTaskDelay(pdMS_TO_TICKS(500)); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET); @@ -128,8 +136,26 @@ void blink_yellow_led(){ } } +void yellow_led_start(void) +{ + if (yellowLedTask == NULL) { + xTaskCreate(blink_yellow_led, "blink_yellow_led", 1024, NULL, 1, &yellowLedTask); + } +} + +void yellow_led_stop(void) +{ + if (yellowLedTask != NULL) { + vTaskDelete(yellowLedTask); + yellowLedTask = NULL; + HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET); + } + +} + void red_led(){ while(1){ + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOE, GPIO_PIN_1, GPIO_PIN_RESET); @@ -137,6 +163,7 @@ void red_led(){ } + // -------------------------------------------------------------------------------------------------------------------- static int CapabilityFunc( int argc, char** argv, void* ctx ) // -------------------------------------------------------------------------------------------------------------------- @@ -145,9 +172,9 @@ static int CapabilityFunc( int argc, char** argv, void* ctx ) (void)argv; (void)ctx; printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\nOK", - 0, // has spindle - 0, // has spindle status - 0, // has stepper + 1, // has spindle + 1, // has spindle status + 1, // has stepper 0, // has stepper move relative 0, // has stepper move speed 0, // has stepper move async @@ -157,7 +184,7 @@ static int CapabilityFunc( int argc, char** argv, void* ctx ) 0, // has stepper refrun skip 0, // has stepper refrun stay enabled 0, // has stepper reset - 0, // has stepper position + 1, // has stepper position 0, // has stepper config 0, // has stepper config torque 0, // has stepper config throvercurr