///////////////////////////////////////////////////////
//
// FileName: pidcount.c
// Author: kwanjee@andrew.cmu.edu
//
// Closed loop control test
// 10-bit PWM resolution
// In an ISR
// using usdigital chip, not HCTL-2016
//
////////////////////////////////////////////////////////

//Timing settings
#pragma CLOCK_FREQ 20000000

#include "16f877.h"
#include "sercomm.h"
#include "pwm.h"
#include "encusdig.h"

#define BLINK_PORT 	PORTB
#define BLINK_PIN  	0

#define	Kp 16
#define	Ki 8
#define	Kd 13
char vel; // desired velocity
char v; // instantaneous velocity
char e; // error in velocity
char pe; // previous error
char de; // change in error over time
char ie; // sum of error over time
char pie; // previous sum of error
char Yh, Yl; // PID output


char ah, al;
char bh, bl;
char xh, xl;
char mulcount;
char mulstat;
char modYl, modYh;
char sigY;

// calculates ah:al - bh:bl
void sub16(void) {
	xl = al - bl;
	if (~STATUS & 1) xh = 0xff;
	else xh = 0;
	xh = xh + ah;
	xh = xh - bh;
}

// calculates ah:al + bh:bl
void add16(void) {
	xl = al + bl;
	if (STATUS & 1) xh = 1;
	else xh = 0;
	xh = xh + ah + bh;
}

// al * bl = xh:xl, signed
void mult_8(void) {

	xh = al ^ bl;
	if (xh & 0x80) mulstat = 1;
	else mulstat = 0;

	if (al > 127) al = 0-al;
	if (bl > 127) bl = 0-bl;

asm {
	movlw	8
	movwf	_mulcount
	movf	_al,W
	movwf	_xl
	movf	_bl,W
	clrf	_xh
mul_loop
	bcf		STATUS,C
	btfsc	_xl,0
	addwf	_xh,F
	rrf		_xh,F
	rrf		_xl,F
	decfsz	_mulcount,F
	goto	mul_loop
}
	
	if (mulstat == 1) {
		xl = ~xl;
		xh = ~xh;
		xl = xl + 1;
		if (STATUS & 1) {
			xh = xh + 1;
		}
	}

	return;
}

char sat_add(char a, char b, char sub) {
	char res;
	
	if (sub) b = ~b + 1;

	res = a + b;

	if (!((a ^ b) & 0x80)) {
		if ((res ^ a) & 0x80) {
			if (res & 0x80) res = 0x7f;
			else res = 0x81;
		}
	}
	return res;
}

void interrupt(void) {
	set_bit(PORTB,0);

	if (INTCON & 0x04) {
		// timer0 overflow, time to update PID...
		
		// 16 bit signed arithmetic!!!!!

/*		bh = counth; bl = countl;
		enc_read();
		ah = enc_h; counth = ah;
		al = enc_l; countl = al;
		sub16();
		v = xl;*/

		v = enc_read();		
		enc_reset();
	
		// must saturate dv and d2v as well, 
		// else cannot handle large changes in vel
	
		pe = e;
		// e = vel - v;
		e = sat_add(vel,v,1);
	
		pie = ie;
		// ie = pie + e
		ie = sat_add(pie,e,0);
	
		//de = e - pe;
		de = sat_add(e,pe,1);
	
		//Y = Kp * e;
		al = Kp; bl = e; 
		mult_8();
		Yh = xh; Yl = xl;
	
		//Y = Y + Ki * ie;
		al = Ki; bl = ie;
		mult_8();
		ah = xh; al = xl;	
		bh = Yh; bl = Yl;
		add16();
		Yh = xh; Yl = xl;

		//Y = Y + Kd * de;
		al = Kd; bl = de;
		mult_8();
		ah = xh; al = xl;	
		bh = Yh; bl = Yl;
		add16();
		//results in xh:xl
		
		// saturate xh:xl to 10 bits magnitude
		// xh: chop off top 6 bits
		// if xh < 128, x is positive (sigY = 0)
		// else x is negative (sigY = 1, flip xl:xh)
		// at this point: xl:xh = magnitude. saturate it if necessary
		// if xh > 3, overflow, saturate
		// now, shift 2 bits right, setvel.
	
		if (xh < 128) {
			sigY = 0;
			modYh = xh;
			modYl = xl;
		}
		else {
			sigY = 1;
			modYh = ~xh;
			modYl = (~xl) + 1;
			if (STATUS & 1) {
				modYh++;
			}
		}
		if (modYh > 3) {
			modYh = 3;
			modYl = 255;
		}
	
		asm {
		rrf		_modYh,F
		rrf		_modYl,F
		rrf		_modYh,F
		rrf		_modYl,F
		rrf		_modYh,F
		}
	
		modYh = modYh & 0xc0;
		pwm_setveldir10(0,modYl,modYh,sigY);
	
		clear_bit(INTCON, T0IF);  //interrupt handled.
	}
	
	if (INTCON & 0x01) {
		// PORTB change, direction change...
		
		set_bit(PORTB,0);
		
		enc_tmp = ~enc_h;
		while (enc_tmp != enc_h) {
			enc_h = TMR1H;
			enc_l = TMR1L;
			enc_tmp = TMR1H;
		}
		
		if (!(PORTB & 0x02)) {
			enc_h = ~enc_h;
			enc_l = ~enc_l;
			enc_l++;
			if (enc_l == 0) enc_h++;
		}
		
		enc_prev_l = enc_l + enc_prev_l;
		// check carry...
		if (STATUS & 1) enc_h++;
		enc_prev_h = enc_h + enc_prev_h;
	
		TMR1L = 0;
		TMR1H = 0;
		
		clear_bit(PORTB,0);
		
		clear_bit(INTCON, RBIF);  //interrupt handled.
	}

	clear_bit(PORTB,0);	
}

void main(void) {
	char inp;
	char i;

	OPTION_REG = 0x05; // TMR0 @ 1:64, prescaler to timer0 (pid loop freq)

	TRISB = 0x00; // port B as outputs
    TRISD = 0xff; // port D as inputs
    
    PORTB = 0;

	// init...
	ser_init(); // init rs-232 comms
	pwm_init(); // initialize CCP module
	enc_init(); // initialize encoder counter chip	

	enc_reset();

	vel = 0;
	e = 0;
	de = 0;
	ie = 0;
	//counth = 0;
	//countl = 0;

	enable_interrupt(T0IE);
	enable_interrupt(GIE);

	putstring("\rUSD\r");
	
/*	ser_tx('>');
	vel = getdec();

	while (1) {
			delay_ms(1);
			putdec(v); ser_tx(' ');
			putdec(e); ser_tx(' ');
			putdec(de); ser_tx(' ');
			putdec(ie); ser_tx(' ');
			puthex(sigY); ser_tx(' ');
			puthex(modYl); puthex(modYh); ser_tx(' ');
			puthex(enc_h); puthex(enc_l); ser_tx('\r');
	}*/
	
	while (1) {
		ser_tx('>');
		inp = getdec();

//		vel = inp;

		for (i=0; i<100; i++) {
			if (i == 30) vel = inp;

			delay_ms(1);

			putdec(v); ser_tx(' ');
//			putdec(e); ser_tx(' ');
//			putdec(ie); ser_tx(' ');
//			putdec(de); ser_tx(' ');
//			puthex(xh); puthex(xl); ser_tx(' ');
//			puthex(sigY); ser_tx(' ');
//			puthex(modYl); puthex(modYh); ser_tx(' ');
			puthex(enc_h); puthex(enc_l); ser_tx(' ');
			puthex(enc_prev_h); puthex(enc_prev_l); ser_tx('\r');			
		}
	}
}

