Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
Code from "Paul Sumo 2", my winning entry in the LVBots sumo competition. This is an app for the Pololu Wixel that I compiled using the Wixel SDK.
#include <wixel.h>
#include <usb.h>
#include <usb_com.h>
#include <adc.h>
#include <stdio.h>
uint32 start_ms;
void resetTime()
{
start_ms = getMs();
}
uint32 elapsedTime()
{
return getMs() - start_ms;
}
uint8 buttonPressed()
{
setPort0PullType(HIGH);
setDigitalInput(00,PULLED);
setDigitalOutput(03,LOW);
return !isPinHigh(00);
}
void timer3Init()
{
// Start the timer in free-running mode and set the prescaler.
T3CTL = 0b01110000; // Prescaler 1:8, frequency = (24000 kHz)/8/256 = 11.7 kHz
//T3CTL = 0b01010000; // Use this line instead if you want 23.4 kHz (1:4)
// Set the duty cycles to zero.
T3CC0 = T3CC1 = 0;
// Enable PWM on both channels. We choose the mode where the channel
// goes high when the timer is at 0 and goes low when the timer value
// is equal to T3CCn.
T3CCTL0 = T3CCTL1 = 0b00100100;
// Configure Timer 3 to use Alternative 1 location, which is the default.
PERCFG &= ~(1<<5); // PERCFG.T3CFG = 0;
// Configure P1_3 and P1_4 to be controlled by a peripheral function (Timer 3)
// instead of being general purpose I/O.
P1SEL |= (1<<3) | (1<<4);
// After calling this function, you can set the duty cycles by simply writing
// to T3CC0 and T3CC1. A value of 255 results in a 100% duty cycle, and a
// value of N < 255 results in a duty cycle of N/256.
}
int16 position = 0;
int16 last_position = 0;
int16 diff = 0;
int16 pid = 0;
int8 rightSensor()
{
return !isPinHigh(10);
}
int8 leftSensor()
{
return !isPinHigh(11);
}
void setMotors(int16 left, int16 right)
{
if(left < 0)
{
T3CC1 = -left;
setDigitalOutput(16, 1);
}
else
{
T3CC1 = left;
setDigitalOutput(16, 0);
}
if(right < 0)
{
T3CC0 = -right;
setDigitalOutput(15, 1);
}
else
{
T3CC0 = right;
setDigitalOutput(15, 0);
}
}
typedef enum State
{
WAIT_BUTTON,
DELAY,
INITIAL_SPIN,
RUN,
SPIN,
ATTACK,
BACKUP
} State;
void updateState()
{
static State state = WAIT_BUTTON;
static uint32 state_start = 0;
static uint32 last_seen = 0;
static uint8 last_on_right = 0;
static int32 reverse;
static uint8 initial_random_turn = 0;
if(leftSensor() && !rightSensor())
last_on_right = 0;
if(rightSensor() && !leftSensor())
last_on_right = 1;
if(rightSensor() || leftSensor())
last_seen = getMs();
LED_RED(leftSensor());
LED_YELLOW(rightSensor());
switch(state)
{
case WAIT_BUTTON:
initial_random_turn = !initial_random_turn;
setMotors(0,0);
if(buttonPressed())
{
state_start = getMs();
state = DELAY;
}
break;
case DELAY:
setMotors(0,0);
if(getMs() - state_start > 5000)
{
state_start = getMs();
state = INITIAL_SPIN;
}
LED_YELLOW((getMs() - state_start) & 0x80);
LED_RED((getMs() - state_start) & 0x80);
break;
case INITIAL_SPIN:
if(getMs() - state_start > 80)
{
state_start = getMs();
state = RUN;
}
if(initial_random_turn)
setMotors(-255,255);
else
setMotors(255,-255);
break;
case RUN:
if(getMs() - state_start > 350)
{
state_start = getMs();
state = SPIN;
}
if(initial_random_turn)
setMotors(255,100);
else
setMotors(100,255);
break;
case SPIN:
if(last_on_right)
setMotors(255,-255);
else
setMotors(-255,255);
if(leftSensor() || rightSensor())
{
state = ATTACK;
state_start = getMs();
}
break;
case ATTACK:
if(getMs() - state_start > 750)
{
state_start = getMs();
state = BACKUP;
}
else if(leftSensor() && rightSensor())
setMotors(255,255);
else if(leftSensor())
setMotors(200,255);
else if(rightSensor())
setMotors(255,200);
else if(getMs() - last_seen > 50)
{
state_start = getMs();
state = SPIN;
}
break;
case BACKUP:
if(getMs() - state_start > 350)
{
state_start = getMs();
state = ATTACK;
}
else if(getMs() - state_start > 250)
setMotors(128,-128);
else
setMotors(-255,-64);
break;
}
}
void main()
{
systemInit();
usbInit();
timer3Init();
resetTime();
while(1)
{
boardService();
usbShowStatusWithGreenLed();
updateState();
usbComService();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment