/*
last update 5/20/2007
Current test program for Solarbotics Sumovore robot  
with GM2 motors (output shafts shortened by 0.08 inches)
WheelWatcher WW-02 modules
ATmega8 FOSC=8MHz

sjames_remington at yahoo dot com

*/
#include <stdint.h>
#include <ctype.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdio.h>
#include <avr/pgmspace.h>
#include <stdlib.h>

#define F_CPU 8000000UL
/* UART baud rate and input buffer size */
#define UART_BAUD  9600
#define RX_BUFSIZE 20

#define SPDSCALE 18750		//convert wheel tick period to rpm*10 (range ~ 50-500)

#define MAXPWM 220
#define MINPWM 15
#define MINRPM 10
#define MAXRPM 50


/*
 * globals MUST be declared volatile to avoid gcc bugs
 */

volatile unsigned long next_ran=1; 		 			//random number gen seed
volatile unsigned int ticks=0,tocks=0,seconds=0;	//time

volatile signed int enc_pos_l=0,enc_pos_r=0,enc_per_l=0,enc_per_r=0;		//current position and rotational periods
volatile signed int pwm_l,pwm_r;			//PWM settings
volatile signed int req_rpm_l,req_rpm_r;	//rpm*10 target values, stall timers

// Ks scaled up by 10
volatile signed int Kp=0,Kd=0;
volatile signed int per_l[128],per_r[128],dump_index_l=0,dump_index_r=0,dump_period=0;

//putchar and getchar for uart

extern int uart_putchar(char c, FILE *stream);
extern int uart_getchar(FILE *stream);
FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);


/*
 *  Function prototypes
 *
 */
 
unsigned char p_limit(int pwm);
void	movrel (int left, int right, int pwm);
void 	set_speed (int left, int right);
void 	set_pwm (void);
void	Delay_us ( uint16_t microseconds );
void	Delay_ms ( uint16_t milliseconds );
void	countdown (void);
unsigned int ADCIN( uint8_t channel );
unsigned int lrand (void);

/*
 * Timer0 overflow interrupt handler. Counts system clock/8/250 -- provides global ticks at 4 kHz
 */

ISR(TIMER0_OVF_vect)
{
	static unsigned int pcount = 4000,tcount=200;	//4000 ticks per second, 20 tocks per sec

	TCNT0 = 6; 	ticks++; 	//reload counter; 250 to overflow
	if (pcount-- == 0) {  	//decrement tick counter and update runtime seconds
	pcount=4000;
    seconds++;
	}
	if (tcount-- == 0) {	//do stall timeout check and bump PWM on that wheel if necessary
	tocks++;tcount=200;
	}
}

/*

 INT0 (PORTD.2) interrupt service routine. Interrupt occurs on falling edge of INT0

 -Assumes encoder0 PD2=clock, PB6=dir, right wheel
 -Increment or decrement encoder 0 position accordingly
 -low pass filter? 
 
*/

ISR (INT0_vect) {

	static unsigned int last_tick=0;
	static unsigned int last_err=0;
	volatile signed int t,err,p_curr;

	t=ticks-last_tick;
	last_tick=ticks;
	if(t>0) enc_per_r=(t+7*enc_per_r)>>3;		//low pass?
	if( (dump_period>0) && (dump_index_r<128) ) {
		per_r[dump_index_r]=enc_per_r; 
		dump_index_r++;
		}
	if (PINB & _BV(6)) enc_pos_r--; else enc_pos_r++;	//Inverted on right
}
/*

 INT1 (PORTD.3) interrupt service routine. Interrupt occurs on falling edge of INT1
 -Assumes encoder1 PD3=clock, PB7=dir, left wheel
 -Increment or decrement encoder 1 position accordingly
 -low pass?

*/

ISR (INT1_vect) {

	static unsigned int last_tick=0;
	static signed int last_err=0;
	volatile signed int t,err,p_curr;
	t=ticks-last_tick;
	last_tick=ticks;
	if(t>0) enc_per_l=(t+7*enc_per_l)>>3;	//low pass?
	if( (dump_period>0) && (dump_index_l<128) ) {
		per_l[dump_index_l]=enc_per_l; 
		dump_index_l++;
		}
	if (PINB & _BV(7)) enc_pos_l++;	else enc_pos_l--;

}

void timer_init(void) {

// set up Timer0 to provide 0.25 ms ticks

	TCCR0  = (1<<CS01);  //clock/8
	TIMSK |= _BV(TOIE0); // enable int on overflow
	TCNT0=6;	//250 counts to overflow

// set up Timer1 to provide fast PWM 8-bit output for motors
// initially output high, then cleared on compare match.

	TCCR1A = ( 1 << WGM10 ) | ( 1 << COM1A1 ) | ( 1 << COM1B1 );
	TCCR1B = ( 3 << CS10  ) | ( 1 << WGM12  );  //cs10=3, clock/64 = 488 Hz PWM frequency

//	cs12=1 -> clock/256, cs11=1 -> clock/8

// set up Timer2 for fast PWM 8-bit output for PB3 (green LED)
// initially output high, then cleared on compare match.
    TCCR2 = ( 3 << WGM20  ) | ( 1 << COM21 ) | ( 1 << CS21  );  //clock/8 = 3.9 kHz
}

#include "uart_mega8.c"
	
int main( void )
{
	unsigned char cycle,err,buf[20];
	int i,j,mono_speed=0,arg1,arg2;

	// PB 1245=motor output pins, 3 = green led. 6,7=encoder direction input 
	DDRB = (1<<PB1)|(1<<PB2)|(1<<PB3)|(1<<PB4)|(1<<PB5);
	// PD4 - PD6 = red leds, PD2,3=INT0,1 input
	DDRD = (1<<PD4)|(1<<PD5)|(1<<PD6);

// 	enable external interrupts 0 and 1 for falling edge triggering

	MCUCR = _BV(ISC11) | _BV(ISC01);
	GICR = _BV(INT1) | _BV(INT0);

	timer_init();
	set_speed(0,0);

	uart_init();

	stdout = stdin = &uart_str;			// associate uart data stream with stdout, stdin
	Delay_ms(200);						//let uart stabilize
	printf("Mazer lowpass 1/8\n");

	sei();

	for (i=0; i<128; i++) per_r[i]=per_l[i]=0;

//	countdown();

	cycle=0;
	set_speed(0,0);

	for (;;) {

// get command of form "c arg1 arg2"
// where c is a single character command with up to two arguments


	printf("%d>",seconds);  //prompt for command, print running seconds counter
	fgets(buf, sizeof buf - 1, stdin);

	err=0;						//initialize error flag
	
	switch (tolower(buf[0]))
		{
		default:
		err=1;
		printf("?: %s\n", buf);
		break;

//"m", set motor speed. currently one argument for both left and right

		case 'm':  //motor speed, range 0 to +/-255 (0 off)
		if (sscanf(buf, "%*s %d", &arg1) > 0) {  //if argument on hand,
			printf("Spd %d\n", arg1);
			set_speed(arg1,arg1);
			mono_speed=arg1;
			}
		else err=2;  //no argument
		break;

//"k", set Kp and Kd

		case 'k':  
		if (sscanf(buf, "%*s %d %d", &arg1, &arg2) > 0) {  //if arguments on hand,
			printf("Ks %d %d\n", arg1, arg2);
			Kp=arg1; Kd=arg2;
			}
		else err=2;  //no argument
		break;
//"s", (status) print current speeds, positions and pwms
		case 's':  
		printf("%d %d %d %d %d %d\n",1875/enc_per_l,OCR1A,enc_pos_l,1875/enc_per_r,OCR1B,enc_pos_r);
		break;
//"h", halt, stop motors
		case 'h':  
		set_speed(0,0);	
		printf("Halt: %d %d\n",enc_pos_l,enc_pos_r);
		break;
//"p", dump 128 stripe periods
		case 'p':
		dump_period=1;
		while ( (per_r[127]==0) && (per_l[127]==0) );
		printf("period dump speed %d pwm_l,r: %d %d\n",mono_speed,pwm_l,pwm_r);
		for (i=0; i<128; i++) printf("%d %d %d\n",i,per_l[i],per_r[i]);
		dump_period=0; 
		dump_index_l=dump_index_r=0; //reset for next dump

		for(i=0; i<128; i++) {per_l[i]=0;per_r[i]=0;}
		break;

		} //end switch

	} //end for

	while (1) {
	cycle++;
	OCR2=cycle;  //green led intensity cycles while holding 
	Delay_ms(20);
	}

//end of main
}

//limit PWM to [MINPWM,MAXPWM]

unsigned char p_limit(int pwm)
{
	volatile unsigned char ret;
	ret=pwm;
	if(pwm>MAXPWM) ret=MAXPWM;
	if(pwm<MINPWM) ret=MINPWM;
	return ret;
}

/*
 set speeds for PID rpm regulation. 
 
 1875/(period in ticks per stripe) = rpm
 Use empirically derived quadratic approximation to estimate initial pwm setting.

input: rpm left, rpm right

post: req_rpm_l and req_rpm_r (rpm x 10, 50-500, with sign)
post: pwm_l and pwm_r (signed integers)

 */


void set_speed (int rpm_l, int rpm_r) {

	volatile signed int t;

	if(rpm_l != 0) {
	req_rpm_l=10*rpm_l;				//scale up by 10 for PID loop
	t=abs(rpm_l);
	pwm_l=p_limit(35-t+t*t/9);
	if(rpm_l < 0) pwm_l=-pwm_l;
	} 
	else {pwm_l=0; req_rpm_l=0;}

	if(rpm_r !=0 ) {
 	req_rpm_r=10*rpm_r;
 	t=abs(rpm_r);
	pwm_r=p_limit(35-t+t*t/9);	//quadratic fit to pwm versus rpm curve
	if(rpm_r < 0) pwm_r=-pwm_r;
	} 
	else {pwm_r=0; req_rpm_r=0;}

	set_pwm ();		//set initial guess at appropriate PWM settings

	printf("req rpm, pwm: %d %d %d %d\n",rpm_l, rpm_r, pwm_l,pwm_r);

}
/*
	setup Timer1 for PWM
*/
	void set_pwm (void) {

	if (pwm_l == 0) TCCR1A &= ~(_BV(COM1A1)|_BV(COM1A0)); //disconnect PWM channel A from PORTB (PB1 should be low)
	else	{
	TCCR1A |=  _BV(COM1A1);
	TCCR1A &= ~_BV(COM1A0);		//set COM1A1=1, COM1A0=0. Result: OC1A=1 on upcount, =0 on compare match
		if (pwm_l>0) {OCR1A =  pwm_l; PORTB |=  (1<<4);} 	// left forward
		else 		 {OCR1A = -pwm_l; PORTB &= ~(1<<4);} 	// left reverse
	}

	if (pwm_r == 0) TCCR1A &= ~(_BV(COM1B1)|_BV(COM1B0)); //disconnect PWM channel B from PORTB (PB2 should be low)
	else {
	TCCR1A |=  _BV(COM1B1);
	TCCR1A &= ~_BV(COM1B0);		//set COM1B1=1, COM1B0=0. Result: OC1B=1 on upcount, =0 on compare match
		if (pwm_r > 0)	{OCR1B =  pwm_r; PORTB |=  (1<<5);}	//right forward
		else 			{OCR1B = -pwm_r; PORTB &= ~(1<<5);}	//right reverse
	}
}


void countdown(void)
{
    /* start with a five second countdown */

//    PORTD |= ( 1 << PD2 ); /* turn on L1 */
//    Delay_ms( 1000 ); 

//    PORTD |= ( 1 << PD3 ); /* turn on L2 */
//    Delay_ms( 1000 );

    PORTD |= ( 1 << PD4 ); /* turn on L3 */
    Delay_ms( 1000 );

    PORTD |= ( 1 << PD5 ); /* turn on L4 */
    Delay_ms( 1000 );

    PORTD |= ( 1 << PD6 ); /* turn on L5 */
    Delay_ms( 1000 );


    PORTD &= ~( ( 1 << PD4 )
              | ( 1 << PD5 )
              | ( 1 << PD6 ) );  //LEDs off
}


/*
 *  Delay_us
 *
 *  wait in a loop for the specified number of microseconds.
 *
 */

void Delay_us( uint16_t microseconds )
{
    register uint16_t loop_count;
    /* 8mhz clock, 4 instructions per loop_count  */
    loop_count = microseconds<<1;

    __asm__ volatile (
        "1: sbiw %0,1" "\n\t"
        "brne 1b"
        : "=w" ( loop_count )
        : "0"  ( loop_count )
    );
}


/* Delay_ms
 *
 *  wait in a loop for the specified number of milliseconds.
 *
 */

void Delay_ms( uint16_t milliseconds )
{
    uint16_t i;

    for ( i = 0; i < milliseconds; ++i )
    {
        Delay_us( 1000 );
    }
}
/*
* lrand-return a random integer between 0 and 65535
*/
unsigned int lrand(void) {
	next_ran = next_ran*1103515245UL + 12345UL;
	return ((next_ran/65536UL) & 0xFFFF);
}