Firebird control via joystick

firebird_joystick.py

# Author: Akshar Prabhu Desai
# Date: 20/04/2011
# Objective: To interface FB5 with joystick
#

import pygame
import inspect
import math
import time
exec(open("functionList.py").read());
# Define some colors
black	= (   0,   0,   0)
white	= ( 255, 255, 255)
blue	 = (  50,  50, 255)
yellow   = ( 255,250,205)
green	= (   0, 255,   0)
dkgreen  = (   0, 100,   0)
red	  = ( 255,   0,   0)
purple   = (0xBF,0x0F,0xB5)
brown	= (0x55,0x33,0x00)
transparent	= (111,111,111)

	
pygame.init()
init("COM6"); #OPen Xbee
def test():
	print("ISR:"+str(time.time()));
	
#onInterrupt("left",test);



WHITE = 100;

def draw_sensor_board(screen,left,center,right):
	
	font = pygame.font.Font(None, 37)
	text = font.render("White Line Sensors",1, red)
	textRect = text.get_rect()
	screen.blit(text,(180,10))
	
	left_center = [200,100]; # center of circle representing sensor board
	center_center = [300,100];
	right_center = [400,100];
	
	if(left<WHITE):
		pygame.draw.circle(screen, white ,left_center , 20);
	else:
		pygame.draw.circle(screen, red , left_center , 20);
		
	if(center<WHITE):
		pygame.draw.circle(screen, white ,center_center , 20);
	else:
		pygame.draw.circle(screen, red , center_center , 20);
		
	if(right<WHITE):
		pygame.draw.circle(screen, white ,right_center , 20);
	else:
		pygame.draw.circle(screen, red , right_center , 20);
		
		
	

		
screen = pygame.display.set_mode((800, 800))
 
# Current position
x_coord=0
y_coord=0
 
# Count the joysticks the computer has
joystick_count=pygame.joystick.get_count()
if joystick_count == 0:
	# No joysticks!
	print ("Error, I didn't find any joysticks.")
else:
	# Use joystick #0 and initialize it
	my_joystick = pygame.joystick.Joystick(0)
	my_joystick.init()
			 
clock = pygame.time.Clock()

tick =0;
done=False
while done==False:
 
	for event in pygame.event.get():
		if event.type == pygame.QUIT:
			done=True
			continue
	# As long as there is a joystick
	if joystick_count != 0:
		stop();
		print(tick)
		tick+=1
		##GET ANALOG VALUES OF JOYSTICK##
		vert_axis_pos= my_joystick.get_axis(1)
		hor_axis_pos= my_joystick.get_axis(0)
		
		#ROUNDED OFF VALUES OF GAME CONTROLLER#
		vert_axis_pos= math.floor(vert_axis_pos*10)
		hor_axis_pos= math.floor(hor_axis_pos*10)
		
		#print(str(hor_axis_pos)+","+str(vert_axis_pos));
		if(hor_axis_pos == -1):
			hor_axis_pos=0;
		if(vert_axis_pos == -1):
			vert_axis_pos=0;
			
		set_speed = 0; #The speed with which the bot will move, turn etc
		
		direction = -1
		if(vert_axis_pos<0):
			direction = 1; 
		set_speed = math.fabs(vert_axis_pos)*25;
		
		setVelocity(set_speed,set_speed);
		#print("speed"+str(set_speed));
		
		
		
		if(hor_axis_pos>0):
			setVelocity(set_speed,0);
			#print("right turn\n");
		elif(hor_axis_pos<0):
			setVelocity(0,set_speed);
			#print("left turn\n");
		else:
			setVelocity(set_speed,set_speed);
		
		if(direction==1):
			forward();
		else:
			backward();
		
		if(set_speed>0):
			print(time.time());
		
		##### GET WHITELINE SENSORS#######
		left = getLeftWLS();
		center = getCenterWLS();
		right = getRightWS();
		##### END ########################
		
		#### RENDER A DASHBOARD ####
		draw_sensor_board(screen,left,center,right);
		
		
		
 
	 
	
 
	  
	
	
	
	pygame.display.flip()
	clock.tick()
	 
pygame.quit ()