//*****************************************************************************
// File Name	: pid.c
// 
// Title		: test pid implementation
// Revision		: 1.1
// Notes		:
// Target MCU	: Atmel AVR series
// Editor Tabs	: 4
// 
// Revision History:
// When			Who			Description of change
// -----------	-----------	-----------------------
// 05-Feb-2003	pstang		Created the program
//*****************************************************************************

 
//----- Include Files ---------------------------------------------------------
#include <avr/io.h>		// include I/O definitions (port names, pin names, etc)
#include <avr/signal.h>	// include "signal" names (interrupt names)
#include <avr/interrupt.h>	// include interrupt support

#include "global.h"		// include our global settings
#include "uart.h"		// include uart function library
#include "rprintf.h"	// include printf function library
#include "timer.h"		// include timer function library (timing, PWM, etc)
#include "vt100.h"		// include VT100 terminal support
#include "a2d.h"		// include A2D converter support
#include "encoder.h"	// include encoder driver
#include "fixedpt.h"	// fixed-point math support
#include "cmdline.h"

// motor connection defines
#define MOTOR_BRAKE_PORT	PORTC
#define MOTOR_BRAKE_DDR		DDRC
#define MOTOR0_BRAKE		PC2
#define MOTOR1_BRAKE		PC3

#define MOTOR_PWM_PORT		PORTD
#define MOTOR_PWM_DDR		DDRD
#define MOTOR0_PWM			PD4
#define MOTOR1_PWM			PD5

#define MOTOR_DIR_PORT		PORTD
#define MOTOR_DIR_DDR		DDRD
#define MOTOR0_DIR			PD6
#define MOTOR1_DIR			PD7

// motor controller mode defines
#define CONTROLMODE_POS		0
#define CONTROLMODE_VEL		1

volatile s32 EncoderLastPos=0;
volatile u32 Counts=0;
volatile u32 Cycles=0;
volatile u08 TimerIncs=0;
volatile u08 ServiceFlag=FALSE;
volatile struct Controller_struct
{
	// controls coefficients
	s32 Kp;				// proportional gain
	s32 Ki;				// integral gain
	s32 Kd;				// derivative gain
	// state variables
	s32 error;			// error (input-feedback)
	s32 error_last;		// previous error
	s32 error_integ;	// integral of error
	s32 error_diff;		// derivative of error
	// limits
	s32 windupMax;		// integral wind-up limit
	s32 outputMax;		// motor output clamp (limit)
	// input and output
	s32 output;			// motor output
	s32 input;			// user input
	s32 feedback;		// motor feedback
	// counters
	u32 counts;
	// mode
	u08 mode;			// control mode
} Controller;

// motor command system
void motorCommand(void);
// motor command functions
void motorPosGo(void);
void motorGetCoeffs(void);
void motorSetCoeffs(void);
void motorSetMode(void);
void motorShowPID(void);
void motorHelp(void);

// motor control functions
void motorInit(void);
void motorSetPower(s32 power);

// PID controller algorithm
void pidService(void);
void timerService(void);

// testing functions
void motorTest(void);
void encoderTest(void);

//----- Begin Code ------------------------------------------------------------
int main(void)
{
	// initialize our libraries
	// initialize the UART (serial port)
	uartInit();
	// set the baud rate of the UART for our debug/reporting output
	uartSetBaudRate(115200);
	// initialize the timer system
	timerInit();
	// init A2D
	a2dInit();
	// initialize rprintf system
	rprintfInit(uartSendByte);
	// initialize vt100 library
	vt100Init();

	// clear the terminal screen
	vt100ClearScreen();
	
	// run the encoder test
	//encoderTest();

	// run the motor test
	//motorTest();

	// command system
	motorCommand();

	return 0;
}

void motorCommand(void)
{
	u08 c;

	// print welcome
	rprintf("Motor Controller V1.0\r\n");

	// initialize cmdline system
	cmdlineInit();

	// direct cmdline output to uart (serial port)
	cmdlineSetOutputFunc(uartSendByte);

	// add commands to the command database
	cmdlineAddCommand("go",			motorPosGo);
	cmdlineAddCommand("getcoeff",	motorGetCoeffs);
	cmdlineAddCommand("setcoeff",	motorSetCoeffs);
	cmdlineAddCommand("mode",		motorSetMode);
	cmdlineAddCommand("piddump",	motorShowPID);
	cmdlineAddCommand("help",		motorHelp);
	cmdlineAddCommand("enctest",	encoderTest);
	cmdlineAddCommand("motortest",	motorTest);
	// send a CR to cmdline input to stimulate a prompt
	cmdlineInputFunc('\r');


	// initialize Controller structure
	// mode
	Controller.mode			= CONTROLMODE_POS;
	// controls coefficients
	Controller.Kp			= 4000;
	Controller.Ki			= 0;
	Controller.Kd			= 4000;
	// limits
	Controller.windupMax	= 200<<8;
	Controller.outputMax	= 700;
	// state variables
	Controller.error		= 0;
	Controller.error_last	= 0;
	Controller.error_integ	= 0;
	Controller.error_diff	= 0;
	// input and output
	Controller.output		= 0;
	Controller.input		= 0;
	Controller.feedback		= 0;

	// initialize motor control lines
	motorInit();

	// set up LEDs
	outb(DDRB, 0xFF);
	// set up a2d converter
	a2dSetPrescaler(ADC_PRESCALE_DIV8);
	
	// initialize encoder
	encoderInit();

	// setup PWM on timer1
	timer1SetPrescaler(TIMER_CLK_DIV8);
	timer1PWMInit(10);
	timer1PWMAOn();
	timer1PWMBOn();
	// attach pid service
	timerAttach(TIMER1OVERFLOW_INT, pidService);

	// ***** this part currently unused *****
	// setup metering timer
	// count with 32.768KHz/8
	//timer2SetPrescaler(TIMERRTC_CLK_DIV8);
	timer2SetPrescaler(TIMERRTC_CLK_DIV1024);
	// switch to asynchronous input (32KHz crystal)
	//sbi(ASSR, AS2);
	// attach service
	//timerAttach(TIMER2OVERFLOW_INT, timerService);
	// **************************************
	
	while(1)
	{
		// pass characters received on the uart (serial port)
		// into the cmdline processor
		while(uartReceiveByte(&c)) cmdlineInputFunc(c);

		// run the cmdline execution functions
		cmdlineMainLoop();

		// print debug messages
		if(ServiceFlag)
		{
			rprintf("Cyc=");
			rprintfNum(10, 6, FALSE, ' ', Cycles);
			rprintf("  Pos=");
			rprintfNum(10, 7, TRUE, ' ', Controller.feedback);
			rprintf("  Ep=");
			rprintfNum(10, 7, TRUE, ' ', Controller.error);
			rprintf("  Ei=");
			rprintfNum(10, 7, TRUE, ' ', Controller.error_integ);
			rprintf("  Ed=");
			rprintfNum(10, 7, TRUE, ' ', Controller.error_diff);
			rprintf("  out=");
			rprintfNum(10, 7, TRUE, ' ', Controller.output);
			rprintfCRLF();
			ServiceFlag=FALSE;
		}
	}

}

void motorPosGo(void)
{
	Controller.input = cmdlineGetArgInt(1);
	rprintf("Set control input to: %d\r\n", Controller.input);
}

void motorGetCoeffs(void)
{
	rprintf("Control coeffs set to:\r\n");
	rprintfProgStrM("Kp="); rprintfNum(10, 6, TRUE, ' ', Controller.Kp); rprintfCRLF();
	rprintfProgStrM("Ki="); rprintfNum(10, 6, TRUE, ' ', Controller.Ki); rprintfCRLF();
	rprintfProgStrM("Kd="); rprintfNum(10, 6, TRUE, ' ', Controller.Kd); rprintfCRLF();
}

void motorSetCoeffs(void)
{
	Controller.Kp = cmdlineGetArgInt(1);
	Controller.Ki = cmdlineGetArgInt(2);
	Controller.Kd = cmdlineGetArgInt(3);

	motorGetCoeffs();
}

void motorSetMode(void)
{
	Controller.mode = cmdlineGetArgInt(1);
	rprintf("Control Mode set to: %d\r\n", Controller.mode);
}

void motorShowPID(void)
{
	if(cmdlineGetArgInt(1))
		timerAttach(TIMER2OVERFLOW_INT, timerService);
	else
		timerDetach(TIMER2OVERFLOW_INT);
}

void motorHelp(void)
{
	rprintfCRLF();

	rprintf("Available commands are:\r\n");
	rprintf("help                     - displays available commands\r\n");
	rprintf("go [x]                   - set controller input\r\n");
	rprintf("getcoeffs                - show controller coeffs\r\n");
	rprintf("setcoeffs [kp] [ki] [kd] - set controller coeffs\r\n");
	rprintf("mode [m]                 - set motor control mode\r\n");
	rprintf("                           m=0: position control\r\n");
	rprintf("                           m=1: velocity control\r\n");
	rprintf("enctest                  - encoder hardware test\r\n");
	rprintf("motortest                - motor hardware and driver test\r\n");

	rprintfCRLF();

}

void motorInit(void)
{
	// configure ports
	// initial state
	cbi(MOTOR_BRAKE_PORT, MOTOR0_BRAKE);
	cbi(MOTOR_BRAKE_PORT, MOTOR1_BRAKE);
	cbi(MOTOR_PWM_PORT,	MOTOR0_PWM);
	cbi(MOTOR_PWM_PORT,	MOTOR1_PWM);
	cbi(MOTOR_DIR_PORT, MOTOR0_DIR);
	cbi(MOTOR_DIR_PORT, MOTOR1_DIR);
	// I/O direction
	sbi(MOTOR_BRAKE_DDR, MOTOR0_BRAKE);
	sbi(MOTOR_BRAKE_DDR, MOTOR1_BRAKE);
	sbi(MOTOR_PWM_DDR, MOTOR0_PWM);
	sbi(MOTOR_PWM_DDR, MOTOR1_PWM);
	sbi(MOTOR_DIR_DDR, MOTOR0_DIR);
	sbi(MOTOR_DIR_DDR, MOTOR1_DIR);
}

void motorSetPower(s32 power)
{
	// set direction
	if(power > 0)
	{
		sbi(MOTOR_DIR_PORT, MOTOR0_DIR);
		sbi(MOTOR_DIR_PORT, MOTOR1_DIR);
	}
	else
	{
		cbi(MOTOR_DIR_PORT, MOTOR0_DIR);
		cbi(MOTOR_DIR_PORT, MOTOR1_DIR);
	}

	// clamp power level
	power = MIN(ABS(power),Controller.outputMax);

	// set power
	if(power < 15) power = 0;

	timer1PWMASet(power);
	timer1PWMBSet(power);

	// led debug
	outb(PORTB, (~power)>>6);
}

void pidService(void)
{
	s32 EncoderCurrentPos;
	
	// get setpoint
	// **** set by user via commands
	//Controller.input = 0;
	
	// get motor feedback
	if(Controller.mode == CONTROLMODE_POS)
	{
		// position feedback
		Controller.feedback = encoderGetPosition(0);
	}
	else
	{
		// velocity feedback
		EncoderCurrentPos = encoderGetPosition(0);
		Controller.feedback = EncoderCurrentPos-EncoderLastPos;
		EncoderLastPos = EncoderCurrentPos;
	}

	// calculate error
	Controller.error = Controller.input-Controller.feedback;
	// calculate integral error
	Controller.error_integ += Controller.error;
	// clamp integral error (anti-windup)
	//Controller.error_integ = MAX(Controller.error_integ, -Controller.windupMax);
	//Controller.error_integ = MIN(Controller.error_integ,  Controller.windupMax);
	// calculate differential error
	Controller.error_diff = Controller.error - Controller.error_last;
	Controller.error_last = Controller.error;
	// do PID
	Controller.output =	 ((Controller.Kp*Controller.error)>>8)
						+((Controller.Ki*Controller.error_integ)>>8)
						+((Controller.Kd*Controller.error_diff)>>8);
	// output value
	motorSetPower(Controller.output);
	// counters
	Controller.counts++;
}

void timerService(void)
{	
	if(!TimerIncs)
	{
		Cycles = Controller.counts;
		Controller.counts=0;
		ServiceFlag=TRUE;
		TimerIncs=3;
	}
	TimerIncs--;
}

void motorTest(void)
{
	s16 i;
	s16 dir = 1;
	u16 waitRamp = 1000;
	u16 waitConstant = 1000;

	// initialize motor control lines
	motorInit();
	
	// set maximum motor output
	Controller.outputMax = 255;
	
	// setup LEDs
	outb(DDRB, 0xFF);

	// setup PWM on timer1
	timer1SetPrescaler(TIMER_CLK_DIV64);
	// 8-bit PWM
	timer1PWMInit(8);
	// both PWM channels on
	timer1PWMAOn();
	timer1PWMBOn();

	// do speed triangle
	while(1)
	{
		// ramp speed up
		rprintfProgStrM("Ramp Up\r\n");
		for(i=0; i<256; i++)
		{
			motorSetPower(dir*i);
			timerPause(waitRamp>>8);
			outb(PORTB, ~i);
		}
		// constant speed
		rprintfProgStrM("Constant\r\n");
		timerPause(waitConstant);
		// ramp speed down
		rprintfProgStrM("Ramp Down\r\n");
		for(i=255; i>=0; i--)
		{
			motorSetPower(dir*i);
			timerPause(waitRamp>>8);
			outb(PORTB, ~i);
		}
		// stop
		rprintfProgStrM("Stop\r\n");
		timerPause(waitConstant);
		// toggle direction
		dir = -dir;
	}
}

void encoderTest(void)
{
	// initialize the encoders
	encoderInit();
	//cbi(GIMSK, INT1);

	// print a little intro message so we know things are working
	rprintf("\r\nWelcome to the encoder test!\r\n");

	// report encoder position continuously
	while(1)
	{
		rprintfProgStrM("Encoder0: ");
		// print encoder0 position
		// use base-10, 10 chars, signed, pad with spaces
		rprintfNum(10, 10, TRUE, ' ', encoderGetPosition(0));

		rprintfProgStrM("  Encoder1: ");
		// print encoder1 position
		// use base-10, 10 chars, signed, pad with spaces
		rprintfNum(10, 10, TRUE, ' ', encoderGetPosition(1));

		// print carriage return and line feed
		rprintfCRLF();

		// pause so we don't flood the host computer
		timerPause(25);
	}
}