RoboBuggy

/*

  Makes Arduino receive input from xbee and parse data.

 The circuit:

 * RX is digital pin 2 (connect to TX of XBee)

 * TX is digital pin 3 (connect to RX of XBee)

 */

//#include <SoftwareSerial.h>

#include <Servo.h> 

Servo myservo;  // create servo object to control a servo 

//SoftwareSerial xbee(2, 3); // RX, TX

unsigned long timer = 0L;

char data;

String message;

char intbuf[32];

int brake = 0;

int steeringAngle = 135;

int pingPong = 1;

int startfound = 0;

int midfound = 0;

int endfound = 1;

void setup()  {

  Serial.begin(9600);

  //Serial.println( "Arduino started sending bytes via XBee" );

  Serial1.begin(9600);

  pinMode(4, OUTPUT);

  pinMode(5, OUTPUT);

  pinMode(8, OUTPUT);

  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 

  myservo.write(133);

}

void loop()  {

  // receive and parse message from xbee

  if(Serial1.available() > 0) { 

    timer = millis();

    // read message from xbee

    data = Serial1.read();

    // parse message data

    if (data == 'A') {

      startfound = 1;

      midfound = 0;

      endfound = 0;

      message = "";

    }

    else if (data == 'B') {

      startfound = 0;

      midfound = 1;

      endfound = 0;

    }

    else if (data == 'C') {

      startfound = 0;

      midfound = 0;

      endfound = 1;

    }

    else if (startfound) {

      message = message + data;

    }

    else if (midfound) {

      if(data == '1')

        brake = 1;

      else

        brake = 0;

      message.toCharArray(intbuf, sizeof(intbuf));

      steeringAngle = atoi(intbuf);

    }  

    // flop external LED everytime message is recieved

    if ( pingPong == 0 ) {

      digitalWrite(4, LOW);

    } 

    else {

      digitalWrite(4, HIGH);

    }

    pingPong = 1 - pingPong;

  } // end receive message

  // brake if it has been greater than 3 seconds since we last got a message

  if( (millis() - timer) > 5000L ) {

    digitalWrite(8, 0);

    digitalWrite(5, HIGH);

    exit(1);

  }

  if((millis() - timer) > 2000L) {

    digitalWrite(12, HIGH);

  }

  else {

    digitalWrite(12, LOW);

  }

  // make brake LED light up if brakes should be down

  if( brake == 0 ) {

    digitalWrite(5, HIGH);

  } 

  else {

    digitalWrite(5, LOW);

  }

  // send parsed signal to brakes and servo

  digitalWrite(8, brake);

  // sets the servo position

  myservo.write(steeringAngle);

}

import pygame

import serial

# Define some colors

BLACK    = (   0,   0,   0)

WHITE    = ( 255, 255, 255)

# This is a simple class that will help us print to the screen

# It has nothing to do with the joysticks, just outputing the

# information.

class TextPrint:

    def __init__(self):

        self.reset()

        self.font = pygame.font.Font(None, 20)

    def printt(self, screen, textString):

        textBitmap = self.font.render(textString, True, BLACK)

        screen.blit(textBitmap, [self.x, self.y])

        self.y += self.line_height

    def reset(self):

        self.x = 10

        self.y = 10

        self.line_height = 15

    def indent(self):

        self.x += 10

    def unindent(self):

        self.x -= 10

pygame.init()

# Set the width and height of the screen [width,height]

size = [500, 700]

screen = pygame.display.set_mode(size)

pygame.display.set_caption("My Game")

#Loop until the user clicks the close button.

done = False

# Used to manage how fast the screen updates

clock = pygame.time.Clock()

# Initialize the joysticks

pygame.joystick.init()

# Get ready to print

textPrint = TextPrint()

# Set starting values

steeringAngle = 0

brake = 1

# Open serial port

ser = serial.Serial('COM8', 9600, timeout=5)

# -------- Main Program Loop -----------

while done==False:

    # EVENT PROCESSING STEP

    for event in pygame.event.get(): # User did something

        if event.type == pygame.QUIT: # If user clicked close

            done=True # Flag that we are done so we exit this loop

        # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION

        if event.type == pygame.JOYBUTTONDOWN:

            print("Joystick button pressed.")

        if event.type == pygame.JOYBUTTONUP:

            print("Joystick button released.")

    # DRAWING STEP

    # First, clear the screen to white. Don't put other drawing commands

    # above this, or they will be erased with this command.

    screen.fill(WHITE)

    textPrint.reset()

    # Get count of joysticks

    joystick_count = pygame.joystick.get_count()

    textPrint.printt(screen, "Number of joysticks: {}".format(joystick_count) )

    textPrint.indent()

    # For each joystick:

    for i in range(joystick_count):

        joystick = pygame.joystick.Joystick(i)

        joystick.init()

        textPrint.printt(screen, "Joystick {}".format(i) )

        textPrint.indent()

        # Get the name from the OS for the controller/joystick

        name = joystick.get_name()

        textPrint.printt(screen, "Joystick name: {}".format(name) )

        # Usually axis run in pairs, up/down for one, and left/right for

        # the other.

        axes = joystick.get_numaxes()

        textPrint.printt(screen, "Number of axes: {}".format(axes) )

        textPrint.indent()

        for i in range( axes ):

            axis = joystick.get_axis( i )

            textPrint.printt(screen, "Axis {} value: {:>6.3f}".format(i, axis) )

            if i == 0:

                steeringAngle = axis

        textPrint.unindent()

        buttons = joystick.get_numbuttons()

        textPrint.printt(screen, "Number of buttons: {}".format(buttons) )

        textPrint.indent()

        for i in range( buttons ):

            button = joystick.get_button( i )

            textPrint.printt(screen, "Button {:>2} value: {}".format(i,button) )

            if button == 1:

                if i == 1:

                    brake = 0;

                elif i == 3:

                    brake = 1;

        textPrint.unindent()

        # Hat switch. All or nothing for direction, not like joysticks.

        # Value comes back in an array.

        hats = joystick.get_numhats()

        textPrint.printt(screen, "Number of hats: {}".format(hats) )

        textPrint.indent()

        for i in range( hats ):

            hat = joystick.get_hat( i )

            textPrint.printt(screen, "Hat {} value: {}".format(i, str(hat)) )

        textPrint.unindent()

        textPrint.unindent()

    # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT

    # Go ahead and update the screen with what we've drawn.

    pygame.display.flip()

    sAngle = (-1.0*(steeringAngle - 1))/2.0

    sAngle = (sAngle*60)+102

    # sAngle = (sAngle*range of motion)+minimum value

    sAngle = int(sAngle)

    if sAngle > 180:

        sAngle = 180

    # send data through xbee

    currentpacket = 'A'+str(sAngle)+'B'+str(brake)+'C'

    print currentpacket

    ser.write('A')

    ser.write(str(sAngle))

    ser.write('B')

    ser.write(str(brake))

    ser.write('C')

    # Limit to 20 frames per second

    #clock.tick(20)

    clock.tick(8)

ser.close()

# Close the window and quit.

# If you forget this line, the program will 'hang'

# on exit if running from IDLE.

pygame.quit ()