Pages

Friday, July 1, 2011

Preliminary robot control script

I haven't updated in a while, mainly because I kept on changing and adding things about the code which controls the robot. This mostly includes ROS (Robot Operating System) functionality related to Android devices, as my plan is to mount my G2 to the robot for things like GPS and orientation polling, automatic navigation or webcam feeds. I'm coming along on that code, but for now I'll just share the bare-bones python code which lets a user control the robot with the arrow buttons on a keyboard.


import serial
import pygame
import time
import sys
pygame.init()
forward=False
left=False
right=False
back=False
q=False
speed=220
screen = pygame.display.set_mode((100,100))

#ser=serial.Serial("/dev/ttyUSB0")
ser=serial.Serial("COM12")

while True:
pygame.event.pump()
key=pygame.key.get_pressed()

forward=key[pygame.K_UP]
left=key[pygame.K_LEFT]
right=key[pygame.K_RIGHT]
back=key[pygame.K_DOWN]
q=key[pygame.K_q]

if forward and not ((left or right) or back):
LDirection="+"
RDirection="+"
LSpeed=chr(254)
RSpeed=chr(254)
elif back and not ((left or right) or forward):
LDirection="-"
RDirection="-"
LSpeed=chr(254)
RSpeed=chr(254)
elif left and not ((forward or right) or back):
LDirection="-"
RDirection="+"
LSpeed=chr(254)
RSpeed=chr(254)
elif right and not ((forward or left) or back):
LDirection="+"
RDirection="-"
LSpeed=chr(254)
RSpeed=chr(254)
elif (forward and left) and not (right or back):
LDirection="+"
RDirection="+"
LSpeed=chr(200)
RSpeed=chr(254)
elif (forward and right) and not (left or back):
LDirection="+"
RDirection="+"
LSpeed=chr(254)
RSpeed=chr(200)
elif (back and right) and not (left or forward):
LDirection="-"
RDirection="-"
LSpeed=chr(254)
RSpeed=chr(200)
elif (back and left) and not (right or forward):
LDirection="-"
RDirection="-"
LSpeed=chr(200)
RSpeed=chr(254)
else:
LDirection="-"
RDirection="-"
LSpeed=chr(0)
RSpeed=chr(0)
#checksum=ord(LDirection)+ord(LSpeed)+ord(RDirection)+ord(RSpeed)
string="s"+LDirection+LSpeed+RDirection+RSpeed+"0"+"e"
ser.write(string)
print string
if q:
pygame.quit()
sys.exit()
ser.close()
time.sleep(0.1)


As you can see, pygame is used for the keyboard input (which is probably not optimal, but I will later use the pygame window to display actual data). Checksum is not implemented yet, mainly because I'm lazy, and also because the robot has been working fine without it anyway. I'll still fix it at some point just for good practice.

No comments: