喜歡 0
分支 0

最後活躍 1 month ago

A Joystick Axis Pipe. This is the Arduino code that receives through a serial interface the commands and generate the PPM Pulses.

axispipe.py 原始檔案
1import serial
2import pygame
3import struct
4import os
5import sys
6import time
7class TextPrint:
8 def __init__(self):
9 self.reset()
10 self.font = pygame.font.Font(None, 30)
11
12 def show(self, screen, textString):
13 textBitmap = self.font.render(textString, True, (0,0,0))
14 screen.blit(textBitmap, [self.x, self.y])
15 self.y += self.line_height
16
17 def reset(self):
18 self.x = 20
19 self.y = 20
20 self.line_height = 25
21
22 def indent(self):
23 self.x += 20
24
25 def unindent(self):
26 self.x -= 20
27
28class AxisPipe:
29 def __init__(self, port):
30 self.port = port
31 self.serial = serial.Serial(port,38400, timeout=0.01)
32 self.axis = [ 0,0,0,0 ]
33 self.aux = [ 0,0 ]
34
35 def UpdateAxis(self, axis, value):
36 if self.axis[axis] != value & 0xFFFF:
37 print "Updating Axis %s with %s" %(axis,value)
38 self.axis[axis] = value & 0xFFFF
39 self.serial.write(struct.pack("BBB",axis,value&0xFF,value/0xFF))
40
41 def UpdateAux(self, aux, value):
42 if self.aux[aux] != value & 0xFFFF:
43 print "Updating Aux %s with %s" %(aux,value)
44 self.aux[aux] = value & 0xFFFF
45 self.serial.write(struct.pack("BBB",aux+7,value&0xFF,value/0xFF))
46
47 def SetLed(self, val):
48 self.serial.write(struct.pack("BBB",4,val&0xFF,0))
49
50 def ReadData(self):
51 data = self.serial.readline()
52 if len(data) > 1:
53 print "Response: %s" %data.replace("\n","")
54
55 def TurnOn(self):
56 self.serial.write("\x05\x00\x00")
57 self.UpdateAxis(0,1000);
58 self.UpdateAxis(1,1000);
59 self.UpdateAxis(2,1000);
60 self.UpdateAxis(3,1000);
61
62 self.UpdateAux(0,1000);
63 self.UpdateAux(1,1000);
64
65 def TurnOff(self):
66 self.serial.write("\x06\x00\x00")
67
68 def KeepAlive(self):
69 self.serial.write("\x09\x00\x00")
70
71 def Close(self):
72 self.TurnOff()
73 self.serial.close()
74
75pygame.init()
76
77screen = pygame.display.set_mode([540,300])
78pygame.display.set_caption("Controller")
79
80done = False
81ax = AxisPipe("/dev/rfcomm21")
82led = 0
83leddir = False
84clock = pygame.time.Clock()
85
86
87pygame.joystick.init()
88textPrint = TextPrint()
89
90def FixDeadBand(val):
91 if val >= 1580 or val <= 1420:
92 return val
93 else:
94 return 1500
95
96ax.UpdateAxis(0,1000);
97ax.UpdateAxis(1,1000);
98ax.UpdateAxis(2,1000);
99ax.UpdateAxis(3,1000);
100
101ax.UpdateAux(0,1000);
102ax.UpdateAux(1,1000);
103
104while done==False:
105 for event in pygame.event.get():
106 if event.type == pygame.QUIT:
107 done=True
108 elif event.type == pygame.JOYBUTTONUP:
109 if event.button == 7:
110 print "Turning ON the controller"
111 ax.TurnOn()
112 elif event.button == 6:
113 print "Turning OFF the controller"
114 ax.TurnOff()
115 elif event.button == 0: # Button A
116 ax.UpdateAux(0,int(joystick.get_axis(5)*500+1500))
117 elif event.button == 1: # Button B
118 ax.UpdateAux(1,int(joystick.get_axis(5)*500+1500))
119 elif event.type == pygame.KEYUP:
120 if event.key == 100:
121 ax.UpdateAxis(1,2000)
122 elif event.key == 97:
123 ax.UpdateAxis(1,1000)
124 else:
125 ax.UpdateAxis(1,1500)
126
127 screen.fill((255,255,255))
128
129 textPrint.reset()
130
131 if pygame.joystick.get_count() > 0:
132 joystick = pygame.joystick.Joystick(1)
133 joystick.init()
134
135 textPrint.show(screen, "Joystick {}".format(0) )
136 textPrint.indent()
137 name = joystick.get_name()
138 textPrint.show(screen, "Joystick name: {}".format(name) )
139 if joystick.get_numaxes() >= 6:
140 ax.UpdateAxis(0,int(joystick.get_axis(4)*500+1500))
141 ax.UpdateAxis(1,FixDeadBand(int(joystick.get_axis(2)*250+1500)))
142 ax.UpdateAxis(2,FixDeadBand(int(-joystick.get_axis(3)*250+1500)))
143 ax.UpdateAxis(3,FixDeadBand(int(joystick.get_axis(0)*500+1500)))
144 textPrint.indent()
145 textPrint.show(screen, "CH1: {}".format(ax.axis[0]))
146 textPrint.show(screen, "CH2: {}".format(ax.axis[1]))
147 textPrint.show(screen, "CH3: {}".format(ax.axis[2]))
148 textPrint.show(screen, "CH4: {}".format(ax.axis[3]))
149 textPrint.show(screen, "AUX0: {}".format(ax.aux[0]))
150 textPrint.show(screen, "AUX1: {}".format(ax.aux[1]))
151 textPrint.unindent()
152
153 else:
154 textPrint.show(screen, "No Joystick Available" )
155 ax.UpdateAxis(0,0);
156 ax.UpdateAxis(1,0);
157 ax.UpdateAxis(2,0);
158 ax.UpdateAxis(3,0);
159
160 ax.ReadData()
161 if leddir:
162 if led >= 255:
163 led = 255
164 leddir = False
165 else:
166 led += 5
167 else:
168 if led <= 0:
169 led = 0
170 leddir = True
171 else:
172 led -= 5
173 #ax.SetLed(led);
174 textPrint.show(screen, "LED: {}".format(led) )
175 textPrint.unindent()
176 textPrint.unindent()
177 pygame.display.flip()
178 ax.KeepAlive()
179 clock.tick(240) # We need to update the KeepAlive more than 70 times per sec
180
181ax.Close()
182pygame.quit ()
axispipe_328.ino 原始檔案
1#include <Servo.h>
2
3Servo ch[6];
4
5unsigned char buff[3];
6unsigned short count;
7unsigned short kcount = 0;
8void TurnOn() {
9 ch[0].attach(3);
10 ch[1].attach(5);
11 ch[2].attach(6);
12 ch[3].attach(9);
13 ch[4].attach(8);
14 ch[5].attach(10);
15 digitalWrite(13, HIGH);
16 Serial.println("Controller ON");
17}
18
19void TurnOff() {
20 for(int i=0;i<6;i++)
21 ch[i].detach();
22 digitalWrite(3, LOW);
23 digitalWrite(5, LOW);
24 digitalWrite(6, LOW);
25 digitalWrite(8, LOW);
26 digitalWrite(9, LOW);
27 digitalWrite(10, LOW);
28 digitalWrite(13, LOW);
29 Serial.println("Controller OFF");
30}
31
32void KeepAlive() {
33 TCCR2B = 0x00;
34 TCNT2 = 0x00;
35 TIFR2 = 0x00;
36 TIMSK2 = 0x01;
37 TCCR2A = 0x00;
38 TCCR2B |= (1<< CS12) | (1<< CS10);
39 kcount = 0;
40}
41
42ISR(TIMER2_OVF_vect) {
43 //We didnt receive the signal yet. So we must shutdown the output
44 kcount++;
45 if(kcount == 255)
46 TurnOff();
47 TCCR2B = 0x00;
48 TCNT2 = 0x00;
49 TIFR2 = 0x00;
50 TIMSK2 = 0x01;
51 TCCR2A = 0x00;
52 TCCR2B |= (1<< CS12) | (1<< CS10);
53}
54
55void setup() {
56 Serial.begin(38400);
57 Serial.println("Inititializing controllers");
58 pinMode(13, OUTPUT);
59 pinMode(3, OUTPUT);
60 pinMode(5, OUTPUT);
61 pinMode(6, OUTPUT);
62 pinMode(8, OUTPUT);
63 pinMode(9, OUTPUT);
64 pinMode(10, OUTPUT);
65
66 digitalWrite(3, LOW);
67 digitalWrite(5, LOW);
68 digitalWrite(6, LOW);
69 digitalWrite(8, LOW);
70 digitalWrite(9, LOW);
71 digitalWrite(10, LOW);
72
73 count = 0;
74 Serial.println("AxisPipe Started!");
75 KeepAlive();
76}
77
78void loop() {
79 short val;
80 if(count == 3) {
81 count = 0;
82 val = buff[1] + buff[2] * 256;
83 switch(buff[0]) {
84 case 0:
85 case 1:
86 case 2:
87 case 3:
88 ch[buff[0]].writeMicroseconds(val);
89 break;
90 case 4: break; // Nothing on 328
91 case 5: TurnOn(); break;
92 case 6: TurnOff(); break;
93 case 7:
94 case 8: // Aux Outputs
95 ch[buff[0]-3].writeMicroseconds(val);
96 break;
97 case 9: break;
98 default:
99 Serial.println("Unknown CMD");
100 }
101 KeepAlive();
102 }
103 if (Serial.available() > 0) {
104 buff[count] = Serial.read();
105 count++;
106 }
107
108}

由 Opengist 提供支持 ⋅ Load: 49ms⋅

繁體中文