Robotic F.A.C.E.: PIC C code
for the cheeks 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) == 12) {
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;
}
}