racerxdl / axispipe.py

import serial import pygame import struct import os import sys import time class TextPrint: def __init__(self): self.reset() self.font = pygame.font.Font(None, 30) def show(self, screen, textString): textBitmap = self.font.render(textString, True, (0,0,0)) screen.blit(textBitmap, [self.x, self.y]) self.y += self.line_height def reset(self): self.x = 20 self.y = 20 self.line_height = 25 def indent(self): self.x += 20 def unindent(self): self.x -= 20 class AxisPipe: def __init__(self, port): self.port = port self.serial = serial.Serial(port,38400, timeout=0.01) self.axis = [ 0,0,0,0 ] self.aux = [ 0,0 ] def UpdateAxis(self, axis, value): if self.axis[axis] != value & 0xFFFF: print "Updating Axis %s with %s" %(axis,value) self.axis[axis] = value & 0xFFFF self.serial.write(struct.pack("BBB",axis,value&0xFF,value/0xFF)) def UpdateAux(self, aux, value): if self.aux[aux] != value & 0xFFFF: print "Updating Aux %s with %s" %(aux,value) self.aux[aux] = value & 0xFFFF self.serial.write(struct.pack("BBB",aux+7,value&0xFF,value/0xFF)) def SetLed(self, val): self.serial.write(struct.pack("BBB",4,val&0xFF,0)) def ReadData(self): data = self.serial.readline() if len(data) > 1: print "Response: %s" %data.replace("\n","") def TurnOn(self): self.serial.write("\x05\x00\x00") self.UpdateAxis(0,1000); self.UpdateAxis(1,1000); self.UpdateAxis(2,1000); self.UpdateAxis(3,1000); self.UpdateAux(0,1000); self.UpdateAux(1,1000); def TurnOff(self): self.serial.write("\x06\x00\x00") def KeepAlive(self): self.serial.write("\x09\x00\x00") def Close(self): self.TurnOff() self.serial.close() pygame.init() screen = pygame.display.set_mode([540,300]) pygame.display.set_caption("Controller") done = False ax = AxisPipe("/dev/rfcomm21") led = 0 leddir = False clock = pygame.time.Clock() pygame.joystick.init() textPrint = TextPrint() def FixDeadBand(val): if val >= 1580 or val <= 1420: return val else: return 1500 ax.UpdateAxis(0,1000); ax.UpdateAxis(1,1000); ax.UpdateAxis(2,1000); ax.UpdateAxis(3,1000); ax.UpdateAux(0,1000); ax.UpdateAux(1,1000); while done==False: for event in pygame.event.get(): if event.type == pygame.QUIT: done=True elif event.type == pygame.JOYBUTTONUP: if event.button == 7: print "Turning ON the controller" ax.TurnOn() elif event.button == 6: print "Turning OFF the controller" ax.TurnOff() elif event.button == 0: # Button A ax.UpdateAux(0,int(joystick.get_axis(5)*500+1500)) elif event.button == 1: # Button B ax.UpdateAux(1,int(joystick.get_axis(5)*500+1500)) elif event.type == pygame.KEYUP: if event.key == 100: ax.UpdateAxis(1,2000) elif event.key == 97: ax.UpdateAxis(1,1000) else: ax.UpdateAxis(1,1500) screen.fill((255,255,255)) textPrint.reset() if pygame.joystick.get_count() > 0: joystick = pygame.joystick.Joystick(1) joystick.init() textPrint.show(screen, "Joystick {}".format(0) ) textPrint.indent() name = joystick.get_name() textPrint.show(screen, "Joystick name: {}".format(name) ) if joystick.get_numaxes() >= 6: ax.UpdateAxis(0,int(joystick.get_axis(4)*500+1500)) ax.UpdateAxis(1,FixDeadBand(int(joystick.get_axis(2)*250+1500))) ax.UpdateAxis(2,FixDeadBand(int(-joystick.get_axis(3)*250+1500))) ax.UpdateAxis(3,FixDeadBand(int(joystick.get_axis(0)*500+1500))) textPrint.indent() textPrint.show(screen, "CH1: {}".format(ax.axis[0])) textPrint.show(screen, "CH2: {}".format(ax.axis[1])) textPrint.show(screen, "CH3: {}".format(ax.axis[2])) textPrint.show(screen, "CH4: {}".format(ax.axis[3])) textPrint.show(screen, "AUX0: {}".format(ax.aux[0])) textPrint.show(screen, "AUX1: {}".format(ax.aux[1])) textPrint.unindent() else: textPrint.show(screen, "No Joystick Available" ) ax.UpdateAxis(0,0); ax.UpdateAxis(1,0); ax.UpdateAxis(2,0); ax.UpdateAxis(3,0); ax.ReadData() if leddir: if led >= 255: led = 255 leddir = False else: led += 5 else: if led <= 0: led = 0 leddir = True else: led -= 5 #ax.SetLed(led); textPrint.show(screen, "LED: {}".format(led) ) textPrint.unindent() textPrint.unindent() pygame.display.flip() ax.KeepAlive() clock.tick(240) # We need to update the KeepAlive more than 70 times per sec ax.Close() pygame.quit ()

#include <Servo.h> Servo ch[6]; unsigned char buff[3]; unsigned short count; unsigned short kcount = 0; void TurnOn() { ch[0].attach(3); ch[1].attach(5); ch[2].attach(6); ch[3].attach(9); ch[4].attach(8); ch[5].attach(10); digitalWrite(13, HIGH); Serial.println("Controller ON"); } void TurnOff() { for(int i=0;i<6;i++) ch[i].detach(); digitalWrite(3, LOW); digitalWrite(5, LOW); digitalWrite(6, LOW); digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, LOW); digitalWrite(13, LOW); Serial.println("Controller OFF"); } void KeepAlive() { TCCR2B = 0x00; TCNT2 = 0x00; TIFR2 = 0x00; TIMSK2 = 0x01; TCCR2A = 0x00; TCCR2B |= (1<< CS12) | (1<< CS10); kcount = 0; } ISR(TIMER2_OVF_vect) { //We didnt receive the signal yet. So we must shutdown the output kcount++; if(kcount == 255) TurnOff(); TCCR2B = 0x00; TCNT2 = 0x00; TIFR2 = 0x00; TIMSK2 = 0x01; TCCR2A = 0x00; TCCR2B |= (1<< CS12) | (1<< CS10); } void setup() { Serial.begin(38400); Serial.println("Inititializing controllers"); pinMode(13, OUTPUT); pinMode(3, OUTPUT); pinMode(5, OUTPUT); pinMode(6, OUTPUT); pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(10, OUTPUT); digitalWrite(3, LOW); digitalWrite(5, LOW); digitalWrite(6, LOW); digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, LOW); count = 0; Serial.println("AxisPipe Started!"); KeepAlive(); } void loop() { short val; if(count == 3) { count = 0; val = buff[1] + buff[2] * 256; switch(buff[0]) { case 0: case 1: case 2: case 3: ch[buff[0]].writeMicroseconds(val); break; case 4: break; // Nothing on 328 case 5: TurnOn(); break; case 6: TurnOff(); break; case 7: case 8: // Aux Outputs ch[buff[0]-3].writeMicroseconds(val); break; case 9: break; default: Serial.println("Unknown CMD"); } KeepAlive(); } if (Serial.available() > 0) { buff[count] = Serial.read(); count++; } }