Last active
August 29, 2015 13:56
-
-
Save racerxdl/9026265 to your computer and use it in GitHub Desktop.
A Joystick Axis Pipe. This is the Arduino code that receives through a serial interface the commands and generate the PPM Pulses.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 () |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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++; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment