//--------------------------------------------------------------------
//-- DollyScooter Balance controller PIC 16F877A 20 MHz
//-- Output pin for PWM signal = RC1 and RC2
//-- RA0,RA1,RA2 analog inputs (A_ACC, GYRO, STEER)  
//-- 
//--Timer 2: PWM
//--Timer 0: Angle integation time measurement 
//--Timer 1: Not used
//--
//-- Version 0.2
//-- 16.11.2011 P.Viljakainen---
//--------------------------------------------------------------------

#include <pic.h>
#include "lcd.h"
#include <stdlib.h>


__CONFIG( HS&DEBUGDIS&LVPDIS&BORDIS&PWRTEN&WDTDIS );
//__CONFIG (WDTDIS & LVPDIS & BORDIS & XT & PWRTEN) ;
//__CONFIG( HS&UNPROTECT&LVPDIS&BOREN&PWRTEN&WDTDIS );

bit st_risingedge;// start button rising edge memory bit
int time2;		// motor PWM update cycle time

int xraw;		//unsigned 16-bit integer 0...65535, pitch angle raw measurement
int graw;		//unsigned 16-bit integer 0...65535, gyro raw measurement
int sraw;		//unsigned 16-bit integer 0...65535, steering raw measurement
int dtmr;		//unsigned 16-bit integer 0...65535, integration time increment raw value
int lmotor;		//unsigned 16-bit integer 0...65535, left motor pwm setpoint
int rmotor;		//unsigned 16-bit integer 0...65535, right motor pwm setpoint
int sraw;		//unsigned 16-bit integer 0...65535, steering raw measurement
signed int turn;//signed 16-bit integer -32767...+32767, steering setpoint


float angle_deg;	// angle in degrees
float gyro_decps;	// gyro in degrees per second
float angle;		// filtered angle result in degrees
float motor;		// motor balance value
float xraaka;
float ramp;		// setpoint ramp-up variable
//------ interrupts handling -------------------------------------
/*
void interrupt tc_int(void)
		{			
		
		if(T0IF)				// Timer0 interrupt
			{	
				T0IF=0;			// clear interrupt bit				
				if(time2>0) --time2;						
			}	
		}
		
//----------------------------------------------------------------
		
*/

void task2(void);
void initialize(void);
void Control (void);
void UpdateMotors(void);
void ReadAnalog(void);
void Display (void);
void Oplogic (void);
						//  timer 0 interrupt, turnaround is 256* 1/20000000s = 51.2 us
							// 0,010s/51.2 us = 195 (appr)
							//control parameters 
#define KP 0.05	// proportional
#define KD 0.04	// derivative
#define KS 0.15	// steering


void main(void)
{	
	initialize();		
	while(1)	
	{	
	//if (time2==5) UpdateMotors();			// Change Duty cycle setpoints every 5th cycle (time2)		
	Control();							// Do control calcs every program cycle
	UpdateMotors();
	if (time2==500)Display();
	Oplogic ();
	
	}
}

void Oplogic()
{

if ((st_risingedge==0) && (RB2==1))
	{
	ramp=5000;
	st_risingedge=1;					// rising edge of start button found
	motor=0;
	}
if (RB2==0)								// reset rising edge memory	
	st_risingedge=0;
										//RB0=DMS
										//RB2=STOP
if ((RB0=1)&&(RB2=1))
	{
	RE0=0;								//RE0=SD
	RC3=1;
	RB4=1;								//monitor led
	}

else 
	{
	RE0=1;								//RE0=SD
	RC3=0;
	RB4=0;								//monitor led
	}										
//if ((angle_deg <2)&& (angle_deg > -2)&&(motor<40)&&(motor>-40))	//COND (RE2) OK?
if ((motor<40)&&(motor>-40))	//COND (RE2) OK?
	{
	RB5=1;
	RE2=1;
	}
else{
	RB5=0;
	RE2=0;
	}

}

void UpdateMotors()
{	
							
	CCPR1L=lmotor>>2;				// set DUTY CYCLE motor 1 (On-time, 8 MSB's)
	CCP1X=(lmotor>>8 && 0x01);			// 2 LSB's in CCP1CON vector CCP1CON(5:4)
	CCP1Y=(lmotor>>8 && 0x02)>>1;			// 10 bits total

	CCPR2L=rmotor>>2;				// set DUTY CYCLE motor 2 (On-time, 8 MSB's)
	CCP2X=(rmotor>>8 && 0x01);			// 2 LSB's in CCP1CON vector CCP1CON(5:4)
	CCP2Y=(rmotor>>8 && 0x02)>>1;			// 10 bits total
	
}


void Control()
{
	ReadAnalog();					// analog reading
	angle_deg=-225.11+0.440*(float)xraaka;		// Scaling angle to degrees
	gyro_decps=-200.07+0.391*(float)graw;		// scaling angular speed to degrees per second

	dtmr=TMR0;					// itegration time timer 1 count value
	TMR0=0x00;					// reset Timer 0 for next time
	//RD1^=1;
	

	angle = 0.7 * (angle + gyro_decps * (float)dtmr * 0.0000128);
	angle += 0.3 * (angle_deg);			// angle measurement integration and filtration, one tick= 1/20 MHz*4

							// add limiters here

	motor += ((KP * angle) + (KD * gyro_decps)); 	// po angle ei angle_dec add PD algorithm here
	motor=motor*(ramp/10000);
	if (motor <= -351) //max 511, limit 151 for fully loaded battery
		{ 
		motor = -311; 
		}
	else if (motor >= 311) 
		{ 
		motor = 311;
		}		

	lmotor= rmotor= 511 +(int)motor;
	turn = (-511+sraw )* KS;		// calculate steering amount
	if (turn < -100) //limit turning speed
		{ 
		turn = -100; 
		}
	if (turn > 100) 
		{ 
		turn = 100;
		}
	lmotor += (int)turn;					// offset motor setpoints
	rmotor -= (int)turn;
	
	if (lmotor > 980) { lmotor = 980; }		// limit min and max
	else if (lmotor <= 40) { lmotor = 40; }
	if (rmotor > 980) { rmotor = 980; }
	else if (rmotor <= 40) { rmotor = 40; }
	
	if (ramp<10000)
		ramp++;
	else
		ramp=10000;

	if(time2<500)
	{
	time2++;							// increase counter
	}
	else
	{
	time2=1;							// reset t2
	}
}

void ReadAnalog()
{
	int z;						// Delay loop variable
								
	ADCS0=0;					// select Fosc/32
	ADCS1=1;					// select Fosc/32
	CHS0=0;						// channel select 0
	CHS1=0;
	CHS2=0;	
	for(z=0;z<15;z++)				// sampling C charges 			
	ADGO=1;						// initiate conversion on the selected channel
	while(ADGO)continue;							
	xraaka=((ADRESH<<8)+(ADRESL));  			// X accelerometer voltage, float
	

	ADIF=0;	
	ADCS0=0;
	ADCS1=1;		
	CHS0=1;						// channel select 1
	CHS1=0;
	CHS2=0;
	for(z=0;z<15;z++)				// sampling C charges	
	ADGO=1;						// initiate conversion on the selected channel
	while(ADGO)continue;							
	graw=((ADRESH<<8)+(ADRESL));  			// GYRO voltage, float


	ADIF=0;	
	ADCS0=0;
	ADCS1=1;		
	CHS0=0;						// channel select 2
	CHS1=1;
	CHS2=0;
	for(z=0;z<15;z++)				// sampling C charges 	
	ADGO=1;						// initiate conversion on the selected channel
	while(ADGO)continue;							
	sraw=((ADRESH<<8)+(ADRESL));  			// Steering voltage, unsigned int

}

void Display()
{
	lcd_line1();
	//lcd_cmd(0x01);					//ylarivi 0x00-0x13 alarivi 0x40-0x53
	lcd_string("angle = ");	
	lcd_16number((int)angle);
	lcd_goto (0x0E);
	lcd_16number(motor);
	lcd_line2();
	lcd_string("ohjaus = ");
	lcd_16number(turn);
	//lcd_16number(ADRESL);
	//lcd_string("xraaka = ");	
	//lcd_16number((int)xraaka);
	//lcd_goto (0x4E);
	//lcd_16number((int)graw);

RE1^=1;
}
void initialize()
{

		ADCON1 = 0xB2;
		//ADCON1 = 0x83;				// 10-bit, right justified A,VREF,A,A,A
		TRISA=0b00111;				//RA0 & RA1  & RA2 = input muut output	
		TRISB=0b11001101;			// RB0=DMS, RB1=DSP(ENA), RB2=STOPputteja		
		RBPU=0;						// Enable RB weak pull-up
		TRISD = 0b00000000;				//D-portti outputeiksi	
		TRISC = 0b10000000;	
			
		PORTD=0x0;
			
		TRISE0=0;				//RE0=output(SD),RE1=SPARE input, RE2=output(COND)
		TRISE1=0;
		TRISE2=0;	
		//CVREN=0;
		//CMCON=7;	
		ADON=1;					// turn on the A2D conversion module

		lcd_init();				// Display module initialization
		delay();
		//RBIE=0;
		//GIE=0;					// enable global interrupts
		//PEIE=0;					// enable peripheral interrupts
		
		//INTCON=0x0;
		T0CS = 0;				// Timer0 increments on instruction clock
		//TMR0IE = 1;				// Enable interrupt on TMR0 overflow
		PSA=0;
		PS0=1;					// Prescaler bits 1:64
		PS1=0;
		PS2=1;
 		
		T2CON= 0b00000100;			//Timer 2 control register prescale 1:1(xxxxxx00);1:4(xxxxxx01), postscale 1:1						
		
		CCP1CON= 0b00001100;			//set up for PWM and 2 LSB (CCP1CON(5:4), CCP1X, CCP1Y)
		CCP2CON= 0b00001100;
		PR2= 0b11111111;			// Set up PERIOD, constant =255, with TMR2 Prescaler 1:4= 4.88 kHz (1:16 = 1.22 kHz) PWWM frequency
		//CCPR1L=0;				// Set up Duty cycle initial value 8 MSB
		
		//T1CON= 0b00001001;			//Timer 1 control register prescale 1:1, turn timer on
		//TMR1H=0x00;
		//TMR1L=0x00;	
		motor=0;
		ramp=10000;					// setpoint and ramp-up initial value

		
}