//bugf84
//This is the driver program for the Cyber Spider.  It will use a 754410
//driver for the two motors, a TTT IRPD board for object detection and 
//will generate a PWM signal for the motor drivers.  Eventually it will
//use LDRs to sense light and dark.  Its designed to use the default IRQ
//handler (very slow) and a 4MHZ 16C84 (also very slow) so the PWM 
//resolution is only 4 bits (16 values in each direction).  Crappy, but
//more than adequate for this project!
//
//Version 1.0 Copyright Dennis Clark and TTT Enterprises February 2002

#include <16C84.H>

#fuses HS,NOWDT,NOPROTECT

#use delay(clock=4000000)
#use FAST_IO(A)				//don't keep setting TRIS register
#byte PORTB = 6				//writing bits to 754410 direction
#byte PORTA = 5				//IRPD on low bits, PWM on high bits
#define RTL 0x05			//rotate left
#define RTR 0x0A			//rotate right
#define REV 0x09			//go reverse
#define FWD 0x06			//go forward

const byte SLOW = 6;			//slow motor speed
const byte MED = 10;
const byte FAST = 15;
const byte WANDER = 1;			//Behavior priorities
const byte AVOID = 2;
const byte BUMPED = 3;
const byte EDGE = 4;

byte left;    				// current left PWM count
byte right;				// current right PWM count
byte PWMperiod;				// to get full 1KHz period

byte sleft;				//left PWM setting
byte sright;				//right PWM setting
const byte SPERIOD = 15;		//16 passes = 1KHz PWM

//Globals
byte dur;				//how long to go that way
byte priority;				//currently active priority


#int_rtcc                          	// This function is called every time
clock_isr() {                      	// the RTCC (timer0) overflows (255->0).
    if (left == 0) {
        output_bit(PIN_A2,0);
    }
    left--;
    if (right == 0) {
        output_bit(PIN_A3,0);
    }
    right--;
    if (PWMperiod == 0) {
        PWMperiod = SPERIOD;
        left = sleft;
        right = sright;
        output_bit(PIN_A2,1);
        output_bit(PIN_A3,1);
    } 
    PWMperiod--;			//C int maint makes this > 32us!
    set_rtcc(240);  			//255-239 =16, 16*2us = 32us
}

//behaviors

void do_wander() {
//Does just the wander routine

     if (priority == WANDER) {
        dur--;
        if (dur == 0)
            priority = 0;
    }
    if (priority < WANDER) {
        priority = WANDER;
        PORTB = FWD;
        sleft = MED;
        sright = MED;
        dur = 100;
    } 
}

void do_avoid() {
//Does the object avoidance

    byte irpd;				//IRPD inputs

    if (priority == AVOID) {
	dur--;
	if (dur == 0)
            priority = 0;
    }
    if (priority < AVOID) {
        irpd = PORTA & 0x03;
        if (irpd != 0) {
	    priority = AVOID;
            switch (irpd) {
	    case 3: // directly ahead
		PORTB = REV;
		sleft = FAST;
                sright = SLOW;
		dur = 200;
		break;
	    case 2: // something to the right
		PORTB = RTL;
		sleft = FAST;
		sright = FAST;
		dur = 50;
		break;
	    case 1: // something to the left
		PORTB = RTR;
		sleft = FAST;
		sright = FAST;
		dur = 50;
		break;
	    }
        } 
    }
}

main() {
// This starts it all off, set up the ports, IRQ and defaults
    byte i;

    set_tris_A(0x03);			//0,1 inputs, 2-4 outputs
    
    sleft=0;				//left legs speed 0-15
    sright = 0;				//right legs speed
    PWMperiod = SPERIOD;   		//overall period (about 1ms)

    // Lets wait for a few seconds first
    delay_ms(5000);

    //Set up PWM interrupt stuff
    setup_counters( RTCC_INTERNAL, RTCC_DIV_2);
    enable_interrupts(RTCC_ZERO);
    enable_interrupts(GLOBAL);
    set_rtcc(240);
    
    set_tris_B(0xF0);			//0-3 outputs, 4-7 inputs
    PORTB = FWD;
    sleft = 0;
    sright = 0;

    priority = 0;			// no initial behavior

    do {
        do_wander();
        do_avoid();
    } while (TRUE);

}