Robotic F.A.C.E.: PIC C code
for the face 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;

#int_rtcc clock_isr() {
	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 Trigger(int dir) {
	if (dir)
		return input(PIN_A3);
	return input(PIN_A4);
}

int Next(int curr, int dir) {
	if (dir) {
		return ((curr + 1) % 26);	
	}
	return ((curr + 25) % 26);	
}

void main() {
	char c;
	int first, comm, curr, dest, dir;
	
	// 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;

	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;

	Stop();

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

	while (Trigger(1))
		Move(0);
	Stop();

	first = 1;
	comm = 0;
	curr = 0;
	dest = 0;
	while (1) {
		if (kbhit()) {
			c = getc();
			if (comm) {
				if (c & 1) {
					dir = c & 2;
					dest = c >> 2;
				}
				output_low(PIN_B7);
				if (curr == dest)
					putchar(curr << 2 | 3);
				else
					putchar(curr << 2 | 2);
				output_high(PIN_B7);
				comm = 0;
			} else {
				if ((c & 15) == 8) {
					comm = 1;
					speed = (c >> 5) + 8;
				}
			}
		}

		if (!Trigger(0) && first) {
			curr = Next(curr, dir);
			first = 0;
		} else if (Trigger(0))
			first = 1;

		if (!Trigger(1) && !Trigger(0))
			curr = 0;

		if (curr == dest) {
			RED_LED_OFF;
			Stop();
		} else {
			RED_LED_ON;
			Move(dir);
		}
	}
}