Basic RoboSumo template code

//
// RoboSumo template code for dsPIC30F4011
// Written by Ted Burke
// Last updated 16-1-2013
//
// This code sets up:
//	analog inputs for sensors
//	digital outputs for motors
//	PWM for servo / speed control
//	UART for printf debugging
//	A simple state machine
//
// This program assumes that the distance sensor is
// mounted on the right side of the robot facing
// straight forward. Colour sensors are mounted on
// the front left and front right corners, as well
// as one colour sensor in the centre of the back.
// At start signal, the robot spins left until the
// right edge of the opponent is detected, at which
// point the robot charges forwards until either the
// opponent disappears, or the edge of the arena is
// detected.
//
// State 0: Stay still. On start signal RC13==0 -> state 1
// State 1: Spin left. If ds>threshold -> state 2
// State 2: Go forward. If white or ds<threshold -> state 1
//
// RC13: Start signal input (active low)
// RD0, RD2: Left motor
// RD1, RD3: Right motor
// AN0: Front left colour sensor (flcs)
// AN1: Front right colour sensor (frcs)
// AN2: Back colour sensor (bcs)
// AN3: Distance sensor (ds)
//

#include <xc.h>
#include <stdio.h>
#include <libpic30.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

// Function prototypes
unsigned int read_analog_channel(int n);

int main()
{
	// Variables for sensor readings and state
	int flcs, frcs, bcs, ds, state;
	int cs_threshold = 512, ds_threshold = 512;
	
	// Configure all 4 port D pins as digital outputs
	TRISD = 0;

	// Configure AN0-AN8 as analog inputs
	ADCON3bits.ADCS = 15;  // Tad = 266ns, conversion time is 12*Tad
	ADCON1bits.ADON = 1;   // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER = 0.33ns * 64 * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDCx
	//
	PWMCON1 = 0x00FF;     // Enable all PWM pairs in complementary mode
	PTCONbits.PTCKPS = 3; // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 9375;         // 20ms PWM period (15-bit period value)
	PDC1 = 1406;          // 1.5ms pulse width on PWM channel 1
	PDC2 = 1406;          // 1.5ms pulse width on PWM channel 2
	PDC3 = 1406;          // 1.5ms pulse width on PWM channel 3
	PTMR = 0;             // Clear 15-bit PWM timer counter
	PTCONbits.PTEN = 1;   // Enable PWM time base

	// Setup UART
	U1BRG = 48;            // 38400 baud @ 30 MIPS
	U1MODEbits.UARTEN = 1; // Enable UART
	
	state = 0;
	while(1)
	{
		// Read sensors
		flcs = read_analog_channel(0);
		frcs = read_analog_channel(1);
		bcs = read_analog_channel(2);
		ds = read_analog_channel(3);
		printf("fls=%04d frs=%04d bcs=%04d ds=%04d\n", flcs, frcs, bcs, ds);
		
		if (state == 0)
		{
			// Stay still until start signal
			LATD = 0b0000;
			if (_RC13 == 0) state = 1;
		}
		if (state == 1)
		{
			// spin left until opponent is detected
			LATD = 0b1001;
			if (ds > ds_threshold) state = 2;
		}
		else if (state == 2)
		{
			// Charge forwards until white or opponent disappears
			LATD = 0b1100;
			if (ds < ds_threshold) state = 1;
			if (flcs < cs_threshold || frcs < cs_threshold) state = 1;
		}
	}

	return 0;
}

// This function reads a single sample from the specified
// analog input. It should take less than 5us when the
// microcontroller is running at 30 MIPS.
// The dsPIC30F4011 has a 10-bit ADC, so the value
// returned is between 0 and 1023 inclusive.
unsigned int read_analog_channel(int channel)
{
	ADCHS = channel;          // Select the requested channel
	ADCON1bits.SAMP = 1;      // Start sampling
	__delay32(30);            // 1us delay @ 30 MIPS
	ADCON1bits.SAMP = 0;      // Start Converting
	while (!ADCON1bits.DONE); // Should take 12 * Tad = 3.2us
	return ADCBUF0;
}

This is my build script for the above program, using the XC16 compiler:

xc16-gcc main.c -mcpu=30F4011 -Wl,--script=p30F4011.gld
if errorlevel 0 xc16-bin2hex a.out
Advertisements
This entry was posted in Uncategorized. Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s