This commit is contained in:
fewikay committed 2026-11-06 14:26:37 +01:00
1 parent fef883dca4
commit 6a6a1589ee
3 files changed
+110 -31

No files matched your search

@@ -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;
}
@@ -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