334 lines
6.8 KiB
C
334 lines
6.8 KiB
C
|
/*
|
||
|
|
* mot.c
|
||
|
|
*
|
||
|
|
* Created on: May 2, 2026
|
||
|
|
* Author: user
|
||
|
|
*/
|
||
|
|
#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;
|
||
|
|
|
||
|
|
motMov_st m1ap(void){
|
||
|
|
static motMov_st m1st;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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(43,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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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],m2pwmch,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(43,FW,m4pwmch,0,tramp,RAMPRUN)==DONE){
|
||
|
|
m4st=motStop;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case motStop:
|
||
|
|
default:
|
||
|
|
SetMotPerc(M4,FW,0);
|
||
|
|
SetMotPerc(M4,BW,0);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
return m4st;
|
||
|
|
}
|