Robotic F.A.C.E.: PIC C code
for the mouth iRX board.

//////////////////////////////////////////////////////////////////
//                                                              //
// This code written by Mark Newman (newman@media.mit.edu),     //
// with a little help from Stefan Marti (stefanm@media.mit.edu) //
// for the Robotic F.A.C.E. project                             //
//                                                              //
// Copyright (C) 2003 MIT Media Laboratory                      //
//                                                              //
//////////////////////////////////////////////////////////////////

#include <16F84.H>
#include "Math32.c"

// Configure PIC to use: HS clock, no Watchdog Timer, 
// no code protection, enable Power Up Timer
//
#fuses HS,NOWDT,NOPROTECT,PUT

// Tell compiler clock is 10MHz.  This is required for delay_ms()
// and for all serial I/O (such as printf(...).  These functions
// use software delay loops, so the compiler needs to know the
// processor speed.
//
#use DELAY(clock=10000000)

// Declare that we'll manually establish the data direction of
// each I/O pin on port B.
//
#use fast_io(B)
#use fast_io(A)

// Standard definitions for the irx2_1 board
//
#define RS232_XMT       PIN_B1  // (output) RS232 serial transmit
#define RED_LED         PIN_B2  // (output) Red LED (low true)
#define IR_LED          PIN_B3  // (output) Infrared LED (low true)
#define IR_SENSOR       PIN_B4  // (input) IR sensor (Sharp IS1U30)
#define RS232_RCV       PIN_B5  // (input) RS232 serial receive

// Macros to simplify I/O operations
//
#define RED_LED_ON      output_low(RED_LED)
#define RED_LED_OFF     output_high(RED_LED)
#define IR_LED_ON       output_low(IR_LED)
#define IR_LED_OFF      output_high(IR_LED)
#define IR_RECEIVED	(!input(IR_SENSOR))

// Inform printf() and friends of the desired baud rate 
// and which pins to use for serial I/O.
//
#use rs232(baud=9600, xmit=RS232_XMT, rcv=RS232_RCV)

int motor, pwm, speed;
char ms[4];
char Time0[4];
char Time1[4];

#int_rtcc clock_isr() {
	inc32(ms);
	pwm++;
	if (pwm == speed)
		pwm = 0;
	if (motor && pwm < 8)
		output_high(PIN_A2);
	else
		output_low(PIN_A2);
}

void Move(int dir) {
//	output_high(PIN_A2);
	motor = 1;
	if (dir)
		output_low(PIN_A1);	
	else
		output_high(PIN_A1);	
}

void Stop() {
//	output_low(PIN_A2);
	motor = 0;
}

int AtEnd(int dir) {
	if (dir)
		return input(PIN_A4);
	return input(PIN_A3);
}

void MoveToEnd(int dir) {
	Move(dir);
	while (!AtEnd(dir)) {}
	Stop();
}

int Comm() {
	char c;
	if (kbhit()) {
		c = getc();
		if ((c & 15) == 4) {
			speed = (c >> 5) + 8;
			return 1;
		}
	}
	return 0;
}

void main() {
	char c, curr, dest;
	char target[4];
	char delay[4];

	// since we've declared #use fast_io(B) (above), we MUST 
 	// include a call to set_tris_b() at startup.
 	// 
 	set_tris_b(0b00110000);
 	set_tris_a(0b00011000);

	output_high(PIN_B7);

	speed = 8;
	pwm = 0;
	motor = 0;
	zero32(ms);
	set_rtcc(0);
	setup_counters(RTCC_INTERNAL, RTCC_DIV_32);
	enable_interrupts(INT_RTCC);  
	enable_interrupts(GLOBAL);

	RED_LED_ON;                    // reality check at startup
	delay_ms(200);
	RED_LED_OFF;

	delay_ms(200);	
	c = getc();                    // Serial check at startup
	RED_LED_ON;
	delay_ms(200);
	RED_LED_OFF;

	MoveToEnd(1);
	MoveToEnd(0);
	
	zero32(ms);
	MoveToEnd(1);
	move32(Time1, ms);
	
	zero32(ms);
	MoveToEnd(0);
	move32(Time0, ms);

	RED_LED_ON;

	curr = 0;
	while (1) {
		while(!Comm()) {}
		dest = getc();
		if (dest == 2) {
			output_low(PIN_B7);
			putchar(3);
			output_high(PIN_B7);
		} else if ((dest & 5) == 4) {
			output_low(PIN_B7);
			putchar(2);
			output_high(PIN_B7);
			RED_LED_ON;
			if (dest & 8)
				curr = 255;
			else
				curr = 1;
			Move(dest & 8);
			while (!AtEnd(dest & 8)) {
				if (Comm()) {
					c = getc();
					output_low(PIN_B7);
					putchar(2);
					output_high(PIN_B7);
				}
			}
		} else if (dest & 1) {
			output_low(PIN_B7);
			putchar(2);
			output_high(PIN_B7);
			zero32(ms);			// Zero timer
			RED_LED_ON;
			if (dest < curr) {
				curr = curr - dest;
				move32(target, Time0);
				Move(0);
			} else if (dest > curr) {
				curr = dest - curr;
				move32(target, Time1);
				Move(1);
			} else
				curr = 0;
			mult32byte(target, curr);	// Time move will take
			move32(delay, target);
			mult32byte(delay, speed << 5);	// Aditional time due to PWM
			add32(target, delay);		// Total delay time
			while (lt32(ms, target)) {
				if (Comm()) {
					c = getc();
					output_low(PIN_B7);
					putchar(2);
					output_high(PIN_B7);
				}
			}
			curr = dest;
		}
		Stop();
		RED_LED_OFF;
	}
}