/*
** Eric's RC to PWM
**
** RCPWM2.c
** Written by Eric Lundquist 05/30/2003
** (c)2003 Eric Lundquist - Free for personal non-commerical use.  If you are going to use this code for
** a commercial product, you must first obtain permission.
**
** RC pulse decoder to 1KHz PWM written for a 4MHz STK200.
** Program Description:  This program will decode the 2 RC pulse streams to a resolution
** of 50us.  Since a servo pulse goes from 1.0ms to 2.0ms, we break it down into 20
** segments.  The first 10 detected pulse lengths map to reverse at speeds between 0-100% 
** in 10% increments.  The next 10 detected pulse lengths map to forward at speeds 
** between 0-100%.
** The pulse length is determined in the timer interrupt routine.  The main program
** loops at a leisurely 1KHz rate looking at the servo pulse speed and setting the 
** motor drive signals accordingly.  I chose the 1KHz rate because I am using the
** Tecel D100 motor controller.  The documentation claims that the controller will
** melt down if we use a higher pwm rate.
** 
** Motor Control
**
** PortC
** 0-
** 1-Motor 1 Fwd
** 2-
** 3-Motor 1 Rev
** 4-
** 5-Motor 0 Fwd
** 6-
** 7-Motor 0 Rev
**
** PortD
** 0-
** 1-Channel a r/c input
** 2-
** 3-channel b r/c input
** 4-
** 5-
** 6-
** 7-
**
*/

#include <stdlib.h>
#include <avr/signal.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/timer.h>
#include <avr/sleep.h>

/* Various and Sundry Defines */
#define ENABLE_BIT_DEFINITIONS
#define CPUCLK 4000000L /* 4Mhz Clock Rate */
#define TCNT0_INIT (255 - 25) /* Timer Up Counter Value. 25 ticks=50us */
#define TRUE 1  /* Useful boolean definitions */
#define FALSE 0
#define MS1 20 /* 1ms clock ticks = 50 * 20 */
#define MS5 100 /* 5ms clock ticks = 50 * 20 * 5 */
#define MS30 600 /* 30ms clock ticks = 50us * 20 * 30 */

unsigned char direction0 = 0;	/* 0=Fwd 1=Rev */
unsigned char enabled0 = 1;	/* 0=motor off 1=motor on */
unsigned char speed0 = 0;	/* 0-10 => 0-100% */
unsigned char direction1 = 0;	/* 0=Fwd 1=Rev */
unsigned char speed1 = 0;	/* 0-10 => 0-100% */
unsigned char enabled1 = 1;	/* 0=motor off 1=motor on */

/* RC Channel A */
unsigned char rc_a_ticks = 0;	/* Number of 50us ticks for last received pulse (between 1-20) */
unsigned int rc_a_counter = 0;	/* Current in progress rc pulse counter */
/* RC Channel B */
unsigned char rc_b_ticks = 0;	/* Number of 50us ticks for last received pulse (between 1-20) */
unsigned int rc_b_counter = 0;	/* Current in progress rc pulse counter */

unsigned int ms_ticks = 0;	/* millisecond ticks.  Just rolls over. */
unsigned char ms_counter = 0;	/* millisecond in progress counter */

/* Map number of rc ticks to pwm direction/speed */
/* Notice that we start low speed at 20%.  10% is just too slow and just */
/* makes the motor growl. */
char pwm_table[20] = {-10, -10, -9, -8, -7, -6, -5, -4, -3, 	/* reverse speeds */
				0, 0, 				/* center deadband */
				3, 4, 5, 6, 7, 8, 9, 10, 10};	/* forward speeds */

/* Timer0 Overflow Interrupt Handler */
/* The Timer0 counter gets incremented every 4M/8 seconds. This */
/* works out to 0.000002 seconds or 2us per count. When it reaches 255, it */
/* generates a timer0 overflow interrupt and executes this handler. */
/* Every time we get an interrupt, we reset the up counter. */
/* We want this thing to fire every 50us.  */
SIGNAL (SIG_OVERFLOW0)
{
	unsigned char currval;

        outb (TCNT0, TCNT0_INIT);        /* Reset Up Counter */

	currval = inb(PIND);	/* Read rc input */

	if ((currval & 0x02)) {	/* Check channel A */
		/* Pulse high */
		rc_a_counter++;	/* Bump in progress counter */
		if (rc_a_counter > MS5) { /* Check for timeout */
			/* If we have a high for greater than 5ms then */
			/* we definitely don't have a valid rc signal. */
			/* Better zero things out and try again. */
			rc_a_counter = 0;
			rc_a_ticks = 0;
		}
	} else {
		/* Pulse low */
		if (rc_a_counter > 0) {	/* if transition pulse (high->low) */
			if (rc_a_counter > MS30) { /* check for timeout */
				/* The rc receiver guarantees a pulse every 20-30ms. */
				/* If we haven't received a transition by then, we */
				/* must assume that we are not talking to the radio */
				/* and shut things down for safety. */
				rc_a_counter = 0;  /* timeout, ignore this pulse */
				rc_a_ticks = 0;	/* stop */
			}
			/* Not sure why we have to subtract 16 instead of 20 */
			/* This was required to center the 1.5ms pulse */
			rc_a_ticks = rc_a_counter - 16;	/* Subtract (about) 1ms and move to curr */
			/* Sanity check */
			if (rc_a_ticks < 1) {
				rc_a_ticks = 1;
			} else if (rc_a_ticks > MS1) {
				rc_a_ticks = MS1;
			}
			rc_a_counter = 0;	/* reset in progress counter */
		}
	}
	if ((currval & 0x08)) {	/* Check channel B */
		/* Pulse high */
		rc_b_counter++;	/* Bump in progress counter */
		if (rc_b_counter > MS5) { /* check for timeout */
			/* If we have a high for greater than 5ms then */
			/* we definitely don't have a valid rc signal. */
			/* Better zero things out and try again. */
			rc_b_counter = 0;
			rc_b_ticks = 0;
		}
	} else {
		/* Pulse low */
		if (rc_b_counter > 0) {	/* if transition pulse (high->low) */
			if (rc_b_counter > MS30) { /* check for timeout */
				/* The rc receiver guarantees a pulse every 20-30ms. */
				/* If we haven't received a transition by then, we */
				/* must assume that we are not talking to the radio */
				/* and shut things down for safety. */
				rc_b_counter = 0; /* timeout, ignore this pulse */
				rc_b_ticks = 0;	/* stop */
			}
			/* Not sure why we have to subtract 16 instead of 20 */
			/* This was required to center the 1.5ms pulse */
			rc_b_ticks = rc_b_counter - 16;	/* subtract (about) 1ms and move to curr */
			/* sanity check */
			if (rc_b_ticks < 1) {
				rc_b_ticks = 1;
			} else if (rc_b_ticks > MS1) {
				rc_b_ticks = MS1;
			}
			rc_b_counter = 0;	/* reset in progress counter */
		}
	}
	/* Process millisecond timer */
	ms_counter++;
	if (ms_counter >= MS1) {  /* 50us * 20 = 1ms */
		ms_ticks++;	/* Bump global ms */
		ms_counter = 0;	/* reset ms in progress counter */
	}

}

/* Turn Motor0 Off */
void motor0_off() {
	unsigned char currval;
	unsigned char outpins;

    currval = inb(PORTC);
	outpins = currval;
	outpins |= 0xa0;	/* Turn pins 5 & 7 on */
	if (currval != outpins) {
		/* Set if changed */
		outb(PORTC, outpins);
	}
}

void motor0_on() {
	unsigned char currval;
	unsigned char outpins;

    currval = inb(PORTC);
	outpins = currval;
	if (direction0 == 0) {
		outpins &= 0xdf; /* Turn pin 5 off */
		outpins |= 0x80; /* Turn pin 7 on */
	} else {
		outpins &= 0x7f; /* Turn pin 7 off */
		outpins |= 0x20; /* Turn pin 5 on */
	}
	if (currval != outpins) {
		/* Set if changed */
		outb(PORTC, outpins);
	}
}

/* Turn Motor1 Off */
void motor1_off() {
	unsigned char currval;
	unsigned char outpins;

	currval = inb(PORTC);
	outpins = currval;
	outpins |= 0x0a; /* Turn pins 1 & 3 on */
	if (currval != outpins) {
		/* Set if changed */
		outb(PORTC, outpins);
	}
}

/* Turn Motor1 on */
void motor1_on() {
	unsigned char currval;
	unsigned char outpins;

    currval = inb(PORTC);
	outpins = currval;
	if (direction1 == 1) {
		outpins &= 0xfd; /* Turn pin 1 off */
		outpins |= 0x08; /* Turn pin 3 on */
	} else {
		outpins &= 0xf7; /* Turn pin 3 off */
		outpins |= 0x02; /* Turn pin 1 on */
	}
	if (currval != outpins) {
		/* Set if changed */
		outb(PORTC, outpins);
	}
}

		
int main()
{
	unsigned int prev_tick; /* Used to detect ms change */
	unsigned char counter0 = 0; /* counter for pwm pulse 0-9 */
	unsigned char counter1 = 0; /* counter for pwm pulse 0-9 */
	unsigned char tmp;	/* used for debugging */

    outb (MCUCR, (1<<SE));  /* Enable "sleep" instruction  */

    outb (TCNT0, TCNT0_INIT);       /* Init up counter */
    outb (TCCR0, (1<<CS01));        /* Clock 0 prescale CK/8 */
    outb (TIMSK, (1<<TOIE0));       /* Enable timer 0 overflow interrupts */
	outb (TIFR, (1<<TOV0));		/* Clear any pending interrupts */
    outb (DDRB, 0xff);      /* Port B direction */
	outb (DDRC, 0xff);	/* Port C direction */
	outb (PORTC, 0xaa);	/* Fwd/Rev disabled */
	outb (DDRD, 0x00);	/* Port D all inputs */
	outb (PORTD, 0xff); 	/* Enable pullups */

	prev_tick = ms_ticks;	/* Init previous clock tick value */
	sei();		/* Enable interrupts */

	/* Main Loop */
	while (1) {
		if (prev_tick != ms_ticks) {
			/* We are on a ms boundary */
			outb(PORTB, ~(speed1 + (speed0<<4)));	/* Wink the lights */
			
			/* convert rc speed to pwm speed */
			if (rc_a_ticks == 0) {
				speed0 = 0;
			} else if (pwm_table[rc_a_ticks-1] < 0) {
				direction0 = 1; /* reverse */
				speed0 = -pwm_table[rc_a_ticks-1];
			} else {
				direction0 = 0; /* fwd */
				speed0 = pwm_table[rc_a_ticks-1];
			}

			if (rc_b_ticks == 0) {
				speed1 = 0;
			} else if (pwm_table[rc_b_ticks-1] < 0) {
				direction1 = 1;	/* reverse */
				speed1 = -pwm_table[rc_b_ticks-1];
			} else {
				direction1 = 0;	/* fwd */
				speed1 = pwm_table[rc_b_ticks-1];
			}

			/* PWM cycle is 10 ms.  Every 1 ms we decide to */
			/* turn the motor off or on depending on the speed. */
			/* So a speed of 1 translates into 1ms on and 9ms off. */
			/* A speed of 7 is 7ms on and 3 ms off. */

			/* Decide if we turn motor 0 off/on */
			if (enabled0) {
				counter0++;
				if (counter0 > 10) {
					counter0 = 0;
				}
				if (counter0 >= speed0) {
					motor0_off();
				} else {
					motor0_on();
				}
			}

			/* Decide if we turn motor 1 off/on */
			if (enabled1) {
				counter1++;
				if (counter1 > 10) {
					counter1 = 0;
				}
				if (counter1 >= speed1) {
					motor1_off();
				} else {
					motor1_on();
				}
			}
			prev_tick = ms_ticks;
		}
	}
		
}