source: trunk/brainstorms/Puzzlebox/Brainstorms/Wheelchair_Control.py @ 232

Last change on this file since 232 was 232, checked in by sc, 12 years ago

Brainstorms/Wheelchair_Control.py:

  • Wheelchair code updated to work with Aruidno connected to serial port
  • Property svn:executable set to *
File size: 4.8 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Wheelchair Control
5#
6# Copyright Puzzlebox Productions, LLC (2010)
7#
8# This code is released under the GNU Pulic License (GPL) version 2
9# For more information please refer to http://www.gnu.org/copyleft/gpl.html
10
11__changelog__ = """
12Last Update: 2010.11.30
13
14"""
15
16import time
17import signal
18#import parallel
19import serial
20
21#####################################################################
22# Globals
23#####################################################################
24
25# 'dir' : (low_pin, high_pin)
26CONTROLLS = {
27                'fwd'   : (1<<1, 1<<0),
28                'rev'   : (1<<3, 1<<2),
29                'right' : (1<<5, 1<<4),
30                'left'  : (1<<7, 1<<6),
31                }
32
33MOVE_COMANDS ={
34                ' ' : 'stop',
35                'i' : 'fwd',
36                'm' : 'rev',
37                'j' : 'left',
38                'k' : 'right',
39                }
40
41SPEED_COMMANDS = {
42                '1' : 'speed-1',
43                '2' : 'speed-2',
44                '3' : 'speed-3',
45                }
46
47STOP_TIME = 0
48STOP_INTERVAL = 0.2
49ALARM_INTERVAL = 0.1
50#PORT = parallel.Parallel()
51
52#####################################################################
53# Classes
54#####################################################################
55
56class Getch:
57        def __init__(self):
58                import tty, sys
59
60        def __call__(self):
61                import sys, tty, termios
62                fd = sys.stdin.fileno()
63                old_settings = termios.tcgetattr(fd)
64                try:
65                        tty.setraw(sys.stdin.fileno())
66                        ch = sys.stdin.read(1)
67                finally:
68                        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
69                return ch
70
71
72#####################################################################
73# Functions
74#####################################################################
75
76def alarmHandler(arg1, arg2):
77        print 'alarm!'
78        if STOP_TIME < time.time():
79                stopBot()
80
81def initAlarm():
82        signal.alarm(ALARM_INTERVAL)
83        signal.signal(signal.SIGALRM, alarmHandler)
84
85def setOutput(data):
86        output = data ^ int('00110011', 2)
87        #PORT.setData(output)
88       
89        output = int2bin(output)
90        device.write('x%s' % output)
91       
92        #print 'Output set to: ', int2bin(output)
93        print 'Output set to: ', output
94        print 'commands: i j k m to move, SPACE = stop, x = quit'
95
96def moveBot(dir, speed):
97        output = 0
98        pins = CONTROLLS[dir]
99        if speed == 1:
100                output = pins[0]
101        elif speed == 2:
102                output = pins[1]
103        elif speed == 3:
104                output = pins[0] | pins[1]
105        setOutput(output)
106        time.sleep(STOP_INTERVAL)
107        stopBot()
108
109def stopBot():
110        setOutput(0)
111
112def int2bin(n, count=8):
113        return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])
114
115#####################################################################
116
117def initializeSerial():
118        baudrate = 9600
119        bytesize = 8
120        parity = 'NONE'
121        stopbits = 1
122        software_flow_control = 'f'
123        rts_cts_flow_control = 't'
124        #timeout = 15
125        timeout = 5
126
127        # convert bytesize
128        if (bytesize == 5):
129                init_byte_size = serial.FIVEBITS
130        elif (bytesize == 6):
131                init_byte_size = serial.SIXBITS
132        elif (bytesize == 7):
133                init_byte_size = serial.SEVENBITS
134        elif (bytesize == 8):
135                init_byte_size = serial.EIGHTBITS
136        else:
137                #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
138                init_byte_size = serial.EIGHTBITS
139
140        # convert parity
141        if (parity == 'NONE'):
142                init_parity = serial.PARITY_NONE
143        elif (parity == 'EVEN'):
144                init_parity = serial.PARITY_EVEN
145        elif (parity == 'ODD'):
146                init_parity = serial.PARITY_ODD
147        else:
148                #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
149                init_parity = serial.PARITY_NONE
150
151        # convert stopbits
152        if (stopbits == 1):
153                init_stopbits = serial.STOPBITS_ONE
154        elif (stopbits == 2):
155                init_stopbits = serial.STOPBITS_TWO
156        else:
157                #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
158                init_byte_size = serial.STOPBITS_ONE
159
160        # convert software flow control
161        if (software_flow_control == 't'):
162                init_software_flow_control = 1
163        else:
164                init_software_flow_control = 0
165
166        # convert rts cts flow control
167        if (rts_cts_flow_control == 't'):
168                init_rts_cts_flow_control = 1
169        else:
170                init_rts_cts_flow_control = 0
171
172        device = serial.Serial(port = '/dev/ttyACM0', \
173                                                                         baudrate = baudrate, \
174                                                                         bytesize = init_byte_size, \
175                                                                         parity = init_parity, \
176                                                                         stopbits = init_stopbits, \
177                                                                         xonxoff = init_software_flow_control, \
178                                                                         rtscts = init_rts_cts_flow_control, \
179                                                                         timeout = timeout)
180
181        device.write('x00110011')
182        print "sent stop"
183        time.sleep(2)
184        print "slept 2"
185
186        return(device)
187
188
189#####################################################################
190# Main
191#####################################################################
192
193device = initializeSerial()
194
195MYGETCH = Getch()
196
197#initAlarm()
198
199stopBot()
200
201speed = 1
202while True:
203        cmd = MYGETCH()
204        if cmd == 'x':
205                exit()
206        if cmd in SPEED_COMMANDS.keys():
207                speed = int(cmd)
208                print SPEED_COMMANDS[cmd]
209        elif cmd in MOVE_COMANDS.keys():
210                if MOVE_COMANDS[cmd] == 'stop':
211                        stopBot()
212                else:
213                        print MOVE_COMANDS[cmd]
214                        moveBot(MOVE_COMANDS[cmd], speed)
215                        STOP_TIME = time.time() + STOP_INTERVAL
Note: See TracBrowser for help on using the repository browser.