/*
 * 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;
}