397 lines
8.3 KiB
C
397 lines
8.3 KiB
C
/*
|
|
* mot.c
|
|
*
|
|
* Created on: May 2, 2026
|
|
* Author: user
|
|
*/
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include "main.h"
|
|
#include "mot.h"
|
|
#include "pwm.h"
|
|
|
|
extern uint16_t apM1start;
|
|
extern uint16_t apM1stop;
|
|
extern uint16_t apM2start;
|
|
extern uint16_t apM2stop;
|
|
extern uint16_t apM3start;
|
|
extern uint16_t apM3stop;
|
|
extern uint16_t apM4start;
|
|
extern uint16_t apM4stop;
|
|
extern uint16_t chM1start;
|
|
extern uint16_t chM1stop;
|
|
extern uint16_t chM2start;
|
|
extern uint16_t chM2stop;
|
|
extern uint16_t chM3start;
|
|
extern uint16_t chM3stop;
|
|
extern uint16_t chM4start;
|
|
extern uint16_t chM4stop;
|
|
extern volatile uint16_t rtCiclo;
|
|
extern uint16_t m1pwmap;
|
|
extern uint16_t m1pwmch;
|
|
extern uint16_t m2pwmap;
|
|
extern uint16_t m2pwmch;
|
|
extern uint16_t m3pwmap;
|
|
extern uint16_t m3pwmch;
|
|
extern uint16_t m4pwmap;
|
|
extern uint16_t m4pwmch;
|
|
extern uint16_t mrampstart[4];
|
|
extern uint16_t tramp;
|
|
|
|
extern UART_HandleTypeDef huart1;
|
|
|
|
motMov_st m1ap(void){
|
|
static motMov_st m1st=motWaiting;
|
|
static motMov_st memm1st=motStop;
|
|
char s[100];
|
|
if(memm1st!=m1st){
|
|
memm1st=m1st;
|
|
sprintf(s,"\nm1ap=%d",m1st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
if(rtCiclo<apM1start)m1st=motWaiting;
|
|
switch(m1st){
|
|
case motWaiting:
|
|
SetMotPerc(M1,FW,0);
|
|
SetMotPerc(M1,BW,0);
|
|
if(rtCiclo>=apM1start){
|
|
(void)ramp(M1,BW,mrampstart[M1-1],m1pwmap,tramp,RAMPINIT);
|
|
m1st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M1,BW,mrampstart[M1-1],m1pwmap,tramp,RAMPRUN)==DONE){
|
|
m1st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=apM1stop){
|
|
(void)ramp(M1,BW,m1pwmap,0,tramp,RAMPINIT);
|
|
m1st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M1,BW,m1pwmap,0,tramp,RAMPRUN)==DONE){
|
|
m1st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M1,FW,0);
|
|
SetMotPerc(M1,BW,0);
|
|
break;
|
|
}
|
|
return m1st;
|
|
}
|
|
|
|
motMov_st m2ap(void){
|
|
static motMov_st m2st;
|
|
static motMov_st memm2st=motStop;
|
|
char s[100];
|
|
if(memm2st!=m2st){
|
|
memm2st=m2st;
|
|
sprintf(s,"\nm2ap=%d",m2st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
if(rtCiclo<apM2start)m2st=motWaiting;
|
|
switch(m2st){
|
|
case motWaiting:
|
|
SetMotPerc(M2,FW,0);
|
|
SetMotPerc(M2,BW,0);
|
|
if(rtCiclo>=apM2start){
|
|
(void)ramp(M2,BW,mrampstart[M2-1],m2pwmap,tramp,RAMPINIT);
|
|
m2st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M2,BW,mrampstart[M2-1],m2pwmap,tramp,RAMPRUN)==DONE){
|
|
m2st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=apM2stop){
|
|
(void)ramp(M2,BW,m2pwmap,0,tramp,RAMPINIT);
|
|
m2st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M2,BW,m2pwmap,0,tramp,RAMPRUN)==DONE){
|
|
m2st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M2,FW,0);
|
|
SetMotPerc(M2,BW,0);
|
|
break;
|
|
}
|
|
return m2st;
|
|
}
|
|
|
|
motMov_st m3ap(void){
|
|
static motMov_st m3st;
|
|
static motMov_st memm3st=motStop;
|
|
char s[100];
|
|
if(memm3st!=m3st){
|
|
memm3st=m3st;
|
|
sprintf(s,"\nm3ap=%d",m3st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
if(rtCiclo<apM3start)m3st=motWaiting;
|
|
switch(m3st){
|
|
case motWaiting:
|
|
SetMotPerc(M3,FW,0);
|
|
SetMotPerc(M3,BW,0);
|
|
if(rtCiclo>=apM3start){
|
|
(void)ramp(M3,BW,mrampstart[M3-1],m2pwmap,tramp,RAMPINIT);
|
|
m3st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M3,BW,mrampstart[M3-1],m3pwmap,tramp,RAMPRUN)==DONE){
|
|
m3st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=apM3stop){
|
|
(void)ramp(M3,BW,m3pwmap,0,tramp,RAMPINIT);
|
|
m3st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M3,BW,m3pwmap,0,tramp,RAMPRUN)==DONE){
|
|
m3st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M3,FW,0);
|
|
SetMotPerc(M3,BW,0);
|
|
break;
|
|
}
|
|
return m3st;
|
|
}
|
|
|
|
motMov_st m4ap(void){
|
|
static motMov_st m4st;
|
|
static motMov_st memm4st=motStop;
|
|
char s[100];
|
|
if(memm4st!=m4st){
|
|
memm4st=m4st;
|
|
sprintf(s,"\nm4ap=%d",m4st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
if(rtCiclo<apM4start)m4st=motWaiting;
|
|
switch(m4st){
|
|
case motWaiting:
|
|
SetMotPerc(M4,FW,0);
|
|
SetMotPerc(M4,BW,0);
|
|
if(rtCiclo>=apM4start){
|
|
(void)ramp(M4,BW,mrampstart[M4-1],m2pwmap,tramp,RAMPINIT);
|
|
m4st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M4,BW,mrampstart[M4-1],m4pwmap,tramp,RAMPRUN)==DONE){
|
|
m4st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=apM4stop){
|
|
(void)ramp(M4,BW,m4pwmap,0,tramp,RAMPINIT);
|
|
m4st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M4,BW,m4pwmap,0,tramp,RAMPRUN)==DONE){
|
|
m4st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M4,FW,0);
|
|
SetMotPerc(M4,BW,0);
|
|
break;
|
|
}
|
|
return m4st;
|
|
}
|
|
|
|
motMov_st m1ch(void){
|
|
static motMov_st m1st;
|
|
static motMov_st memm1st=motStop;
|
|
char s[100];
|
|
if(memm1st!=m1st){
|
|
memm1st=m1st;
|
|
sprintf(s,"\nm1ch=%d",m1st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
if(rtCiclo<chM1start)m1st=motWaiting;
|
|
switch(m1st){
|
|
case motWaiting:
|
|
SetMotPerc(M1,FW,0);
|
|
SetMotPerc(M1,BW,0);
|
|
if(rtCiclo>=chM1start){
|
|
(void)ramp(M1,FW,mrampstart[M1-1],m1pwmch,tramp,RAMPINIT);
|
|
m1st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M1,FW,mrampstart[M1-1],m1pwmch,tramp,RAMPRUN)==DONE){
|
|
m1st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=chM1stop){
|
|
(void)ramp(M1,FW,m1pwmch,0,tramp,RAMPINIT);
|
|
m1st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M1,FW,m1pwmch,0,tramp,RAMPRUN)==DONE){
|
|
m1st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M1,FW,0);
|
|
SetMotPerc(M1,BW,0);
|
|
break;
|
|
}
|
|
return m1st;
|
|
}
|
|
|
|
motMov_st m2ch(void){
|
|
static motMov_st m2st;
|
|
static motMov_st memm2st=motStop;
|
|
char s[100];
|
|
if(memm2st!=m2st){
|
|
memm2st=m2st;
|
|
sprintf(s,"\nm2ch=%d",m2st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
|
|
if(rtCiclo<chM2start)m2st=motWaiting;
|
|
switch(m2st){
|
|
case motWaiting:
|
|
SetMotPerc(M2,FW,0);
|
|
SetMotPerc(M2,BW,0);
|
|
if(rtCiclo>=chM2start){
|
|
(void)ramp(M2,FW,mrampstart[M2-1],m2pwmch,tramp,RAMPINIT);
|
|
m2st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M2,FW,mrampstart[M2-1],m2pwmch,tramp,RAMPRUN)==DONE){
|
|
m2st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=chM2stop){
|
|
(void)ramp(M2,FW,m2pwmch,0,tramp,RAMPINIT);
|
|
m2st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M2,FW,m2pwmch,0,tramp,RAMPRUN)==DONE){
|
|
m2st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M2,FW,0);
|
|
SetMotPerc(M2,BW,0);
|
|
break;
|
|
}
|
|
return m2st;
|
|
}
|
|
|
|
motMov_st m3ch(void){
|
|
static motMov_st m3st;
|
|
static motMov_st memm3st=motStop;
|
|
char s[100];
|
|
if(memm3st!=m3st){
|
|
memm3st=m3st;
|
|
sprintf(s,"\nm3ch=%d",m3st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
|
|
if(rtCiclo<chM3start)m3st=motWaiting;
|
|
switch(m3st){
|
|
case motWaiting:
|
|
SetMotPerc(M3,FW,0);
|
|
SetMotPerc(M3,BW,0);
|
|
if(rtCiclo>=chM3start){
|
|
(void)ramp(M3,FW,mrampstart[M3-1],m2pwmch,tramp,RAMPINIT);
|
|
m3st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M3,FW,mrampstart[M3-1],m3pwmch,tramp,RAMPRUN)==DONE){
|
|
m3st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=chM3stop){
|
|
(void)ramp(M3,FW,m3pwmch,0,tramp,RAMPINIT);
|
|
m3st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M3,FW,m3pwmch,0,tramp,RAMPRUN)==DONE){
|
|
m3st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M3,FW,0);
|
|
SetMotPerc(M3,BW,0);
|
|
break;
|
|
}
|
|
return m3st;
|
|
}
|
|
|
|
motMov_st m4ch(void){
|
|
static motMov_st m4st;
|
|
static motMov_st memm4st=motStop;
|
|
char s[100];
|
|
if(memm4st!=m4st){
|
|
memm4st=m4st;
|
|
sprintf(s,"\nm4ch=%d",m4st);
|
|
HAL_UART_Transmit(&huart1,(uint8_t*) s, strlen(s), HAL_MAX_DELAY);
|
|
}
|
|
|
|
if(rtCiclo<chM4start)m4st=motWaiting;
|
|
switch(m4st){
|
|
case motWaiting:
|
|
SetMotPerc(M4,FW,0);
|
|
SetMotPerc(M4,BW,0);
|
|
if(rtCiclo>=chM4start){
|
|
(void)ramp(M4,FW,mrampstart[M4-1],m4pwmch,tramp,RAMPINIT);
|
|
m4st=motStartRamp;
|
|
}
|
|
break;
|
|
case motStartRamp:
|
|
if(ramp(M4,FW,mrampstart[M4-1],m4pwmch,tramp,RAMPRUN)==DONE){
|
|
m4st=motWaitingTime;
|
|
}
|
|
break;
|
|
case motWaitingTime:
|
|
if(rtCiclo>=chM4stop){
|
|
(void)ramp(M4,FW,m4pwmch,0,tramp,RAMPINIT);
|
|
m4st=motStopRamp;
|
|
}
|
|
break;
|
|
case motStopRamp:
|
|
if(ramp(M4,FW,m4pwmch,0,tramp,RAMPRUN)==DONE){
|
|
m4st=motStop;
|
|
}
|
|
break;
|
|
case motStop:
|
|
default:
|
|
SetMotPerc(M4,FW,0);
|
|
SetMotPerc(M4,BW,0);
|
|
break;
|
|
}
|
|
return m4st;
|
|
}
|