#include "NU32.h"
#include <stdio.h>

#define IKp 20
#define IKi 3
#define MKp 100
#define MKi 0
#define MKd 2400

static int Mode=0;
static int Pwm[2]={0};
static float I_eint[2]={0};
static float Motion_target[2]={0};
static float Motion_ep[2]={0};
static float Motion_eint[2]={0};
static float Motion_I[2]={0};

void ADC_setup();
void timer_setup();
void encoder_init();
void encoder_reset();
int I_read(int);
int deg1_read();
int deg2_read();
int encoder1_command(int);
int encoder2_command(int);

void __ISR(_TIMER_2_VECTOR,IPL4SOFT)Controller();
void __ISR(_TIMER_4_VECTOR,IPL5SOFT)MotionISR();

int main(){
	__builtin_disable_interrupts();
	Mode=3;
	ADC_setup();
	timer_setup();
	char buffer[100];
	NU32_Startup();
	encoder_init();
	encoder_reset();
	__builtin_enable_interrupts();
	while(1){
		NU32_ReadUART3(buffer,100);
		switch(buffer[0]){
			case'a':{
				__builtin_disable_interrupts();
				sprintf(buffer,"%d\r\n",I_read(0));
				NU32_WriteUART3(buffer);
				sprintf(buffer,"%d\r\n",I_read(1));
				NU32_WriteUART3(buffer);
				__builtin_enable_interrupts();
				break;
			}
			case'b':{
				__builtin_disable_interrupts();
				int n;
				NU32_ReadUART3(buffer,100);
				sscanf(buffer,"%d",&n);
				Pwm[0]=n;
				NU32_ReadUART3(buffer,100);
				sscanf(buffer,"%d",&n);
				Pwm[1]=n;
				Mode=1;
				__builtin_enable_interrupts();
				break;
			}
			case'c':{
				__builtin_disable_interrupts();
				sprintf(buffer,"%f\r\n",(((float)deg1_read())-32768)/(4*448)*360);
				NU32_WriteUART3(buffer);
				sprintf(buffer,"%f\r\n",(((float)deg2_read())-32768)/(4*448)*360);
				NU32_WriteUART3(buffer);
				__builtin_enable_interrupts();
				break;
			}
			case'd':{
				__builtin_disable_interrupts();
				encoder_reset();
				__builtin_enable_interrupts();
				break;
			}
			case'e':{
				__builtin_disable_interrupts();
				Motion_eint[0]=0;
				Motion_eint[1]=0;
				I_eint[0]=0;
				I_eint[1]=0;
				Motion_ep[0]=0;
				Motion_ep[1]=0;
				float i;
				NU32_ReadUART3(buffer,100);
				sscanf(buffer,"%f",&i);
				Motion_target[0]=i;
				NU32_ReadUART3(buffer,100);
				sscanf(buffer,"%f",&i);
				Motion_target[1]=i;
				Mode=2;
				__builtin_enable_interrupts();
				break;
			}
			case'f':{
				__builtin_disable_interrupts();
				Mode=3;
				__builtin_enable_interrupts();
				break;
			}
			case'g':{
				__builtin_disable_interrupts();
				Mode=4;
				__builtin_enable_interrupts();
				break;
			}
			case'h':{
				__builtin_disable_interrupts();
				Mode=0;
				__builtin_enable_interrupts();
				break;
			}
			default:{;}
		}
	}
	return 0;
}

void ADC_setup(){
	AD1PCFGbits.PCFG0=0;
	AD1PCFGbits.PCFG1=0;
	AD1CON3bits.ADCS=2;
	AD1CON1bits.ADON=1;
}

void timer_setup(){
	PR2=249;
	TMR2=0;
	T2CONbits.TCKPS=6;
	T2CONbits.ON=1;
	IPC2bits.T2IP=4;
	IPC2bits.T2IS=0;
	IFS0bits.T2IF=0;
	IEC0bits.T2IE=1;
	T3CONbits.TCKPS=0;
	PR3=3999;
	TMR3=0;
	T3CONbits.ON=1;
	PR4=6249;
	TMR4=0;
	T4CONbits.TCKPS=6;
	T4CONbits.ON=1;
	IPC4bits.T4IP=5;
	IPC4bits.T4IS=0;
	IFS0bits.T4IF=0;
	IEC0bits.T4IE=1;
	OC1CONbits.OCM=6;
	OC1CONbits.OCTSEL=1;
	OC1RS=0;
	OC1R=2000;
	OC1CONbits.ON=1;
	TRISDbits.TRISD6=0;
	OC5CONbits.OCM=6;
	OC5CONbits.OCTSEL=1;
	OC5RS=0;
	OC5R=2000;
	OC5CONbits.ON=1;
	TRISDbits.TRISD5=0;
}

void encoder_init(){
	SPI3CON=0;
	SPI3BUF;
	SPI3BRG=4;
	SPI3STATbits.SPIROV=0;
	SPI3CONbits.MSTEN=1;
	SPI3CONbits.MODE16=1;
	SPI3CONbits.MODE32=0;
	SPI3CONbits.SMP=1;
	SPI3CONbits.ON=1;
	SPI4CON=0;
	SPI4BUF;
	SPI4BRG=4;
	SPI4STATbits.SPIROV=0;
	SPI4CONbits.MSTEN=1;
	SPI4CONbits.MODE16=1;
	SPI4CONbits.MODE32=0;
	SPI4CONbits.SMP=1;
	SPI4CONbits.ON=1;
}

int I_read(int pin){
	AD1CHSbits.CH0SA=!pin;
	AD1CON1bits.SAMP=1;
	unsigned int time=_CP0_GET_COUNT();
	while(_CP0_GET_COUNT()-time<10){;}
	AD1CON1bits.SAMP=0;
	while(!AD1CON1bits.DONE){;}
	int i;
	if(pin==0){i=2.3201*ADC1BUF0-1169.3305;}
	else{i=2.3201*ADC1BUF0-1176.2908;}
	return i;
}

void encoder_reset(){
	encoder1_command(0);
	encoder1_command(0);
	encoder2_command(0);
	encoder2_command(0);
}

int deg1_read(){
  encoder1_command(1);
  return encoder1_command(1);
}

int deg2_read(){
  encoder2_command(1);
  return encoder2_command(1);
}

int encoder1_command(int read){
	SPI4BUF=read;
	while(!SPI4STATbits.SPIRBF){;}
	SPI4BUF;
	SPI4BUF=5;
	while(!SPI4STATbits.SPIRBF){;}
	return SPI4BUF;
}

int encoder2_command(int read){
	SPI3BUF=read;
	while(!SPI3STATbits.SPIRBF){;}
	SPI3BUF;
	SPI3BUF=5;
	while(!SPI3STATbits.SPIRBF){;}
	return SPI3BUF;
}

void __ISR(_TIMER_2_VECTOR,IPL4SOFT)Controller(){
	switch(Mode){
		case 0:{
			OC1RS=0;
			OC5RS=0;
			break;
		}
		case 1:{
			if(Pwm[0]<0){
				OC1RS=-40*Pwm[0];
				LATDbits.LATD6=0;
			}
			else{
				OC1RS=40*Pwm[0];
				LATDbits.LATD6=1;
			}
			if(Pwm[1]<0){
				OC5RS=-40*Pwm[1];
				LATDbits.LATD5=0;
			}
			else{
				OC5RS=40*Pwm[1];
				LATDbits.LATD5=1;
			}
			break;
		}
		case 2:{
			static float I_current[2]={0};
			static float I_err[2]={0};
			static float I_u[2]={0};
			I_current[0]=I_read(0);
			I_current[1]=I_read(1);
			I_err[0]=Motion_I[0]-I_current[0];
			I_err[1]=Motion_I[1]-I_current[1];
			I_eint[0]=I_eint[0]+I_err[0];
			I_eint[1]=I_eint[1]+I_err[1];
			if(I_eint[0]>1000){I_eint[0]=1000;}
			else if(I_eint[0]<-1000){I_eint[0]=-1000;}
			if(I_eint[1]>1000){I_eint[1]=1000;}
			else if(I_eint[1]<-1000){I_eint[1]=-1000;}
			I_u[0]=IKp*I_err[0]+IKi*I_eint[0];
			I_u[1]=IKp*I_err[1]+IKi*I_eint[1];
			if(I_u[0]>6000){I_u[0]=6000;}
			else if(I_u[0]<-6000){I_u[0]=-6000;}
			if(I_u[1]>6000){I_u[1]=6000;}
			else if(I_u[1]<-6000){I_u[1]=-6000;}
			if(I_u[0]<0){
				OC1RS=(int)(-0.66667*I_u[0]);
				LATDbits.LATD6=0;
			}
			else{
				OC1RS=(int)(0.66667*I_u[0]);
				LATDbits.LATD6=1;
			}
			if(I_u[1]<0){
				OC5RS=(int)(-0.66667*I_u[1]);
				LATDbits.LATD5=0;
			}
			else{
				OC5RS=(int)(0.66667*I_u[1]);
				LATDbits.LATD5=1;
			}
			break;
		}
		case 3:{
			static float I_current[2]={0};
			static float I_err[2]={0};
			static float I_u[2]={0};
			OC1RS=0;
			I_current[1]=I_read(1);
			I_err[1]=Motion_I[1]-I_current[1];
			I_eint[1]=I_eint[1]+I_err[1];
			if(I_eint[1]>1000){I_eint[1]=1000;}
			else if(I_eint[1]<-1000){I_eint[1]=-1000;}
			I_u[1]=IKp*I_err[1]+IKi*I_eint[1];
			if(I_u[1]>6000){I_u[1]=6000;}
			else if(I_u[1]<-6000){I_u[1]=-6000;}
			if(I_u[1]<0){
				OC5RS=(int)(-0.66667*I_u[1]);
				LATDbits.LATD5=0;
			}
			else{
				OC5RS=(int)(0.66667*I_u[1]);
				LATDbits.LATD5=1;
			}
			break;
		}
		case 4:{
			static int i=0;
			OC1RS=0;
			i=deg1_read()-32768;
			if(i<0&&i>=-400){
				OC5RS=(int)(-10*i);
				LATDbits.LATD5=0;
			}
			else if(i<-400){
				OC5RS=4000;
				LATDbits.LATD5=0;
			}
			else if(i>=0&&i<=400){
				OC5RS=(int)(10*i);
				LATDbits.LATD5=1;
			}
			else{
				OC5RS=4000;
				LATDbits.LATD5=1;
			}
			break;
		}
		default:{;}
	}
	IFS0bits.T2IF=0;
}

void __ISR(_TIMER_4_VECTOR,IPL5SOFT)MotionISR(){
	static float Motion_deg[2]={0};
	static float Motion_err[2]={0};
	static float Motion_edot[2]={0};
	if(Mode==2){
		Motion_deg[0]=(((float)deg1_read())-32768)/(4*448)*360;
		Motion_deg[1]=(((float)deg2_read())-32768)/(4*448)*360;
		Motion_err[0]=Motion_target[0]-Motion_deg[0];
		Motion_err[1]=Motion_target[1]-Motion_deg[1];
		Motion_eint[0]=Motion_eint[0]+Motion_err[0];
		Motion_eint[1]=Motion_eint[1]+Motion_err[1];
		if(Motion_eint[0]>1000){Motion_eint[0]=1000;}
		else if(Motion_eint[0]<-1000){Motion_eint[0]=-1000;}
		if(Motion_eint[1]>1000){Motion_eint[1]=1000;}
		else if(Motion_eint[1]<-1000){Motion_eint[1]=-1000;}
		Motion_edot[0]=Motion_err[0]-Motion_ep[0];
		Motion_edot[1]=Motion_err[1]-Motion_ep[1];
		Motion_I[0]=MKp*Motion_err[0]+MKi*Motion_eint[0]+MKd*Motion_edot[0];
		Motion_I[1]=MKp*Motion_err[1]+MKi*Motion_eint[1]+MKd*Motion_edot[1];
		if(Motion_I[0]>1000){Motion_I[0]=1000;}
		else if(Motion_I[0]<-1000){Motion_I[0]=-1000;}
		if(Motion_I[1]>1000){Motion_I[1]=1000;}
		else if(Motion_I[1]<-1000){Motion_I[1]=-1000;}
		Motion_ep[0]=Motion_err[0];
		Motion_ep[1]=Motion_err[1];
	}
	if(Mode==3){
		Motion_deg[0]=(((float)deg1_read())-32768)/(4*448)*360;
		Motion_deg[1]=(((float)deg2_read())-32768)/(4*448)*360;
		Motion_target[1]=Motion_deg[0];
		Motion_err[1]=Motion_target[1]-Motion_deg[1];
		Motion_eint[1]=Motion_eint[1]+Motion_err[1];
		if(Motion_eint[1]>1000){Motion_eint[1]=1000;}
		else if(Motion_eint[1]<-1000){Motion_eint[1]=-1000;}
		Motion_edot[1]=Motion_err[1]-Motion_ep[1];
		Motion_I[1]=MKp*Motion_err[1]+MKi*Motion_eint[1]+MKd*Motion_edot[1];
		if(Motion_I[1]>1000){Motion_I[1]=1000;}
		else if(Motion_I[1]<-1000){Motion_I[1]=-1000;}
		Motion_ep[1]=Motion_err[1];
	}
	IFS0bits.T4IF=0;
}