├── .gitignore ├── AUTHORS ├── LICENSE ├── README.md ├── run.py ├── serial ├── README.txt ├── serialSetup.py ├── serialTest.py ├── setup.sh ├── ttyO1_armhf.com-00A0.dtbo ├── ttyO2_armhf.com-00A0.dtbo ├── ttyO4_armhf.com-00A0.dtbo └── ttyO5_armhf.com-00A0.dtbo └── xbots ├── __init__.py ├── base.py ├── quickbot_v1.py ├── quickbot_v1_config.py ├── quickbot_v2.py ├── quickbot_v2_config.py ├── ultrabot.py ├── ultrabot_config.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | bin/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | eggs/ 16 | lib/ 17 | lib64/ 18 | parts/ 19 | sdist/ 20 | var/ 21 | *.egg-info/ 22 | .installed.cfg 23 | *.egg 24 | 25 | # Installer logs 26 | pip-log.txt 27 | pip-delete-this-directory.txt 28 | 29 | # Unit test / coverage reports 30 | htmlcov/ 31 | .tox/ 32 | .coverage 33 | .cache 34 | nosetests.xml 35 | coverage.xml 36 | 37 | # Translations 38 | *.mo 39 | 40 | # Mr Developer 41 | .mr.developer.cfg 42 | .project 43 | .pydevproject 44 | 45 | # Rope 46 | .ropeproject 47 | 48 | # Django stuff: 49 | *.log 50 | *.pot 51 | 52 | # Sphinx documentation 53 | docs/_build/ 54 | 55 | # vim 56 | *.swp 57 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | The PRIMARY AUTHORS are: 2 | 3 | * Rowland O'Flaherty (rowlandoflaherty.com) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Georgia Tech Research Corporation 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the Georgia Institute of Technology, Georgia Tech 12 | Research Corporation nor the names of its contributors may be used to 13 | endorse or promote products derived from this software without specific 14 | prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTOR BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # QuickBot BeagleBone Black Repo 2 | This is the code that runs on the BeagleBone Black to control the QuickBot. 3 | 4 | ## Overview 5 | Essentially this code establishes socket (UDP) connection with another device 6 | (BASE) and waits for commands. The commands are either of the form of 7 | directives or queries. An example directive is setting the PWM values of the 8 | motors. An example query is getting IR sensor values. 9 | 10 | ## Installation 11 | Clone the repo into home directory: 12 | 13 | >> cd ~ 14 | >> git clone https://github.com/o-botics/quickbot_bbb.git 15 | 16 | ## Running 17 | Check IP address of BASE and ROBOT (run command on both systems and look for IP 18 | address): 19 | 20 | >> ifconfig 21 | 22 | Example output from BBB: 23 | 24 | ra0 Link encap:Ethernet HWaddr 00:0C:43:00:14:F8 25 | inet addr:192.168.1.101 Bcast:192.168.1.255 Mask:255.255.255.0 26 | inet6 addr: fe80::20c:43ff:fe00:14f8/64 Scope:Link 27 | UP BROADCAST RUNNING MULTICAST MTU:1500 Metric:1 28 | RX packets:315687 errors:1113 dropped:1 overruns:0 frame:0 29 | TX packets:12321 errors:0 dropped:0 overruns:0 carrier:0 30 | collisions:0 txqueuelen:1000 31 | RX bytes:66840454 (63.7 MiB) TX bytes:1878384 (1.7 MiB) 32 | 33 | Here the IP address for the robot is **192.168.1.101**. Let's assume the IP address 34 | for the BASE is **192.168.1.100**. 35 | 36 | Change into working directory: 37 | 38 | >> cd ~/quickbot_bbb 39 | 40 | Quick run with given ips: 41 | 42 | >> python run.py -i 192.168.1.100 -r 192.168.1.101 43 | 44 | Use the help `--help` command to display the run command syntax. 45 | For example 46 | 47 | >> python run.py --help 48 | usage: run.py [-h] [--ip IP] [--rip RIP] [--rtype RTYPE] 49 | 50 | optional arguments: 51 | -h, --help show this help message and exit 52 | --ip IP, -i IP Computer ip (base ip) 53 | --rip RIP, -r RIP BBB ip (robot ip) 54 | --rtype RTYPE, -t RTYPE 55 | Type of robot (quick|ultra) 56 | 57 | The QuickBot is now ready and waiting for commands on what to do. 58 | These commands are special strings that sent over the UDP connection (see the full list of commands below). 59 | 60 | ## Send Commands From Python Example 61 | For example to send a command to the QuickBot from your local machine using Python do the following from the command line on your local machine. 62 | 63 | Open python interpreter: 64 | 65 | >> python 66 | 67 | Set up a socket to communicate with in Python 68 | 69 | >>> import socket 70 | >>> LOCAL_IP = "192.168.1.100" # Computer IP address (change to correct value) 71 | >>> QB_IP = "192.168.1.101" # QuickBot IP address (change to correct value) 72 | >>> PORT = 5005 73 | >>> sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 74 | >>> sock.setblocking(False) 75 | >>> sock.bind((LOCAL_IP, PORT)) 76 | 77 | Send a command to check if the QuickBot is ready 78 | 79 | >>> sock.sendto("$CHECK*\n", (QB_IP, PORT)) 80 | 81 | You should see `$CHECK*` appear in the QuickBot terminal where `python run.py` is running. 82 | To spin the wheels at 100% do 83 | 84 | >>> sock.sendto("$PWM=100,100*\n", (QB_IP, PORT)) 85 | 86 | To stop the wheels do 87 | 88 | >>> sock.sendto("$PWM=100,100*\n", (QB_IP, PORT)) 89 | 90 | To check the IR values do 91 | 92 | >>> sock.sendto("$IRVAL?*\n", (QB_IP, PORT)) 93 | 94 | The values will appear in the QuickBot terminal, to read them from the python on your local machine do 95 | 96 | >>> sock.recv(1024) 97 | '[0.0, 0.0, 0.0, 740.0, 0.0]\n' 98 | 99 | ## Command Set 100 | 101 | * Check that the QuickBot is up and running: 102 | * Command 103 | 104 | `"$CHECK*\n"` 105 | 106 | * Response: 107 | 108 | `'Hello from QuickBot\n'` 109 | 110 | * Set PWM values (e.g. to left wheel 70% backwards, right wheel 90% forward): 111 | * Command 112 | 113 | `"$PWM=-70,90*\n"` 114 | 115 | * Get PWM values: 116 | * Command 117 | 118 | `"$PWM?*\n"` 119 | 120 | * Example response: 121 | 122 | `'[-70, 90]\n'` 123 | 124 | * Get IR values: 125 | * Command 126 | 127 | `"$IRVAL?*\n"` 128 | 129 | * Example response: 130 | 131 | `'[402.0, 54.0, 33.0, 805.0, 24.0]\n'` 132 | 133 | * Get raw encoder values (value stored in encoder, only resets on power cycle): 134 | * Command 135 | 136 | `"$ENRAW?*\n"` 137 | 138 | * Example response: 139 | 140 | `'[1001, -5362]\n'` 141 | 142 | * Get current encoder values: 143 | * Command 144 | 145 | `"$ENVAL?*\n"` 146 | 147 | * Example response: 148 | 149 | `'[-16, 12]\n'` 150 | 151 | * Set encoder values 152 | * Command 153 | 154 | `$ENVAL=10,-10*\n"` 155 | 156 | * Get encoder value offsets from raw values: 157 | * Command 158 | 159 | `"$ENOFFSET?*\n"` 160 | 161 | * Example response: 162 | 163 | `'[10, 20]\n'` 164 | 165 | * Set encoder value offsets from raw values: 166 | * Command 167 | 168 | `"$ENOFFSET=0,0*\n"` 169 | 170 | * Reset encoder values to zero: 171 | * Command 172 | 173 | `"$ENRESET*\n"` 174 | 175 | * Get current encoder velocity values: 176 | * Command 177 | 178 | `"$ENVEL?*\n"` 179 | 180 | * Example response: 181 | 182 | `'[-16, 12]\n'` 183 | 184 | * End program QuickBot program 185 | * Command: 186 | 187 | `"$END*\n"` 188 | 189 | -------------------------------------------------------------------------------- /run.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | @brief Run QuickBot class for Beaglebone Black 4 | 5 | @author Josip Delic (delijati.net) 6 | @author Rowland O'Flaherty (rowlandoflaherty.com) 7 | @date 02/07/2014 8 | @version: 1.0 9 | @copyright: Copyright (C) 2014, see the LICENSE file 10 | """ 11 | 12 | import sys 13 | import argparse 14 | 15 | 16 | DESCRIPTION = "" 17 | RTYPES = ('quick_v2', 'quick_v1', 'ultra') 18 | 19 | 20 | def main(options): 21 | """ Main function """ 22 | print "Running XBot" 23 | 24 | print 'Running XBot Program' 25 | print ' Base IP: ', options.ip 26 | print ' Robot IP: ', options.rip 27 | print ' Robot Type: ', options.rtype 28 | 29 | if options.rtype == 'quick_v2': 30 | import xbots.quickbot_v2 31 | qb = xbots.quickbot_v2.QuickBot(options.ip, options.rip) 32 | qb.run() 33 | elif options.rtype == 'quick_v1': 34 | import xbots.quickbot_v1 35 | qb = xbots.quickbot_v1.QuickBot(options.ip, options.rip) 36 | qb.run() 37 | elif options.rtype == 'ultra': 38 | import xbots.ultrabot 39 | qb = xbots.ultrabot.UltraBot(options.ip, options.rip) 40 | qb.run() 41 | 42 | 43 | if __name__ == '__main__': 44 | parser = argparse.ArgumentParser(description=DESCRIPTION) 45 | parser.add_argument( 46 | '--ip', '-i', 47 | default='192.168.7.1', 48 | help="Computer ip (base ip)") 49 | parser.add_argument( 50 | '--rip', '-r', 51 | default='192.168.7.2', 52 | help="BBB ip (robot ip)") 53 | parser.add_argument( 54 | '--rtype', '-t', 55 | default='quick_v2', 56 | help="Type of robot (%s)" % '|'.join(RTYPES)) 57 | 58 | options = parser.parse_args() 59 | if options.rtype not in RTYPES: 60 | print "Chosen type not exists use (%s)" % '|'.join(RTYPES) 61 | sys.exit(0) 62 | main(options) 63 | -------------------------------------------------------------------------------- /serial/README.txt: -------------------------------------------------------------------------------- 1 | The "serialTest.py" script sends a "hello" message over ttyO4. 2 | 3 | If serial is not working run the "serialSetup.sh", which performs the commands discussed below. 4 | 5 | http://www.armhf.com/index.php/beaglebone-black-serial-uart-device-tree-overlays-for-ubuntu-and-debian-wheezy-tty01-tty02-tty04-tty05-dtbo-files/ 6 | 7 | Copy the following .dtbo overlay files to the /lib/firmware directory and apply them after each boot with the command: echo ttyO1_armhf.com > /sys/devices/bone_capemgr*/slots 8 | 9 | ttyO1_armhf.com-00A0.dtbo 10 | ttyO2_armhf.com-00A0.dtbo 11 | ttyO4_armhf.com-00A0.dtbo 12 | ttyO5_armhf.com-00A0.dtbo 13 | Note 1: ttyO3 does not have an RX pinout (it is tied to the TDA19988 HDMI chip) 14 | Note 2: ttyO5 shares pins with the HDMI overlay – both cannot be active at the same time 15 | Note 3: ttyO0 is available on J1 and does not require an overlay 16 | -------------------------------------------------------------------------------- /serial/serialSetup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import Adafruit_BBIO.UART as UART 3 | import serial 4 | 5 | UART.setup("UART1") 6 | UART.setup("UART2") 7 | UART.setup("UART4") 8 | UART.setup("UART5") 9 | -------------------------------------------------------------------------------- /serial/serialTest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import Adafruit_BBIO.UART as UART 3 | import serial 4 | 5 | UART.setup("UART4") 6 | 7 | ser = serial.Serial(port = "/dev/ttyO4", baudrate=9600) 8 | ser.close() 9 | ser.open() 10 | if ser.isOpen(): 11 | print "Serial is open!" 12 | ser.write("Hello from BBB!\n") 13 | ser.close() 14 | -------------------------------------------------------------------------------- /serial/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cp ~/quickbot_bbb/serial/ttyO1_armhf.com-00A0.dtbo /lib/firmware 3 | cp ~/quickbot_bbb/serial/ttyO2_armhf.com-00A0.dtbo /lib/firmware 4 | cp ~/quickbot_bbb/serial/ttyO4_armhf.com-00A0.dtbo /lib/firmware 5 | cp ~/quickbot_bbb/serial/ttyO5_armhf.com-00A0.dtbo /lib/firmware 6 | 7 | echo ttyO1_armhf.com > /sys/devices/bone_capemgr*/slots 8 | echo ttyO2_armhf.com > /sys/devices/bone_capemgr*/slots 9 | echo ttyO4_armhf.com > /sys/devices/bone_capemgr*/slots 10 | echo ttyO5_armhf.com > /sys/devices/bone_capemgr*/slots 11 | ~/quickbot_bbb/serial/serialSetup.py 12 | 13 | -------------------------------------------------------------------------------- /serial/ttyO1_armhf.com-00A0.dtbo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/o-botics/quickbot_bbb/b2f43017ef4142dbd05357508edbdd487e4f6c9e/serial/ttyO1_armhf.com-00A0.dtbo -------------------------------------------------------------------------------- /serial/ttyO2_armhf.com-00A0.dtbo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/o-botics/quickbot_bbb/b2f43017ef4142dbd05357508edbdd487e4f6c9e/serial/ttyO2_armhf.com-00A0.dtbo -------------------------------------------------------------------------------- /serial/ttyO4_armhf.com-00A0.dtbo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/o-botics/quickbot_bbb/b2f43017ef4142dbd05357508edbdd487e4f6c9e/serial/ttyO4_armhf.com-00A0.dtbo -------------------------------------------------------------------------------- /serial/ttyO5_armhf.com-00A0.dtbo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/o-botics/quickbot_bbb/b2f43017ef4142dbd05357508edbdd487e4f6c9e/serial/ttyO5_armhf.com-00A0.dtbo -------------------------------------------------------------------------------- /xbots/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | -------------------------------------------------------------------------------- /xbots/base.py: -------------------------------------------------------------------------------- 1 | """ 2 | @brief Base robot class 3 | 4 | @author Josip Delic (delijati.net) 5 | @author Rowland O'Flaherty (rowlandoflaherty.com) 6 | @date 04/23/2014 7 | @version: 1.0 8 | @copyright: Copyright (C) 2014, see the LICENSE file 9 | """ 10 | 11 | import sys 12 | import time 13 | import re 14 | import socket 15 | import threading 16 | 17 | import Adafruit_BBIO.GPIO as GPIO 18 | import Adafruit_BBIO.PWM as PWM 19 | 20 | LEFT = 0 21 | RIGHT = 1 22 | MIN = 0 23 | MAX = 1 24 | 25 | DEBUG = False 26 | VERBOSE = True 27 | 28 | class BaseBot(object): 29 | """ 30 | Base robot class. Mainly handles initialization and messages passing. 31 | """ 32 | 33 | # Parameters 34 | sample_time = 20.0 / 1000.0 35 | pwm_freq = 2000 36 | 37 | # Variables 38 | led_flag = True 39 | cmdBuffer = '' 40 | 41 | # Motor Pins -- (LEFT, RIGHT) 42 | dir1Pin = ("", "") 43 | dir2Pin = ("", "") 44 | pwmPin = ("", "") 45 | 46 | # Led pin 47 | led = "" 48 | 49 | # State PWM -- (LEFT, RIGHT) 50 | pwm = [0, 0] 51 | 52 | # Constraints 53 | pwmLimits = [-100, 100] # [min, max] 54 | 55 | # UDP 56 | port = 5005 57 | robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 58 | robotSocket.setblocking(False) 59 | 60 | def __init__(self, base_ip, robot_ip): 61 | # Run flag 62 | self.run_flag = True 63 | 64 | # Initialize GPIO pins 65 | self._setup_gpio() 66 | 67 | # Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0) 68 | self._init_pwm() 69 | 70 | # Set motor speed to 0 71 | self.set_pwm([0, 0]) 72 | 73 | # Set IP addresses 74 | self.base_ip = base_ip 75 | self.robot_ip = robot_ip 76 | self.robotSocket.bind((self.robot_ip, self.port)) 77 | 78 | # Initialize command parsing thread 79 | self.cmd_parsing_thread = threading.Thread(target=parse_cmd, 80 | args=(self,)) 81 | self.cmd_parsing_thread.daemon = True 82 | 83 | def _setup_gpio(self): 84 | """Initialize GPIO pins""" 85 | GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT) 86 | GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT) 87 | GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT) 88 | GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT) 89 | GPIO.setup(self.led, GPIO.OUT) 90 | 91 | def _init_pwm(self): 92 | """ 93 | Initialize PWM pins 94 | """ 95 | 96 | # It is currently not possible to set frequency for two PWM 97 | # a maybe solution patch pwm_test.c 98 | # https://github.com/SaadAhmad/beaglebone-black-cpp-PWM 99 | PWM.start(self.pwmPin[LEFT], 0) 100 | PWM.start(self.pwmPin[RIGHT], 0) 101 | 102 | def set_pwm(self, pwm): 103 | """ Set motor PWM values """ 104 | # [leftSpeed, rightSpeed]: 0 is off, caps at min and max values 105 | 106 | self.set_pwm_left(pwm[LEFT]) 107 | self.set_pwm_right(pwm[RIGHT]) 108 | 109 | def set_pwm_left(self, pwm_left): 110 | """ Set left motor PWM value """ 111 | 112 | self.pwm[LEFT] = min( 113 | max(pwm_left, self.pwmLimits[MIN]), self.pwmLimits[MAX]) 114 | 115 | if self.pwm[LEFT] > 0: 116 | GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) 117 | GPIO.output(self.dir2Pin[LEFT], GPIO.HIGH) 118 | PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) 119 | elif self.pwm[LEFT] < 0: 120 | GPIO.output(self.dir1Pin[LEFT], GPIO.HIGH) 121 | GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) 122 | PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT])) 123 | else: 124 | GPIO.output(self.dir1Pin[LEFT], GPIO.LOW) 125 | GPIO.output(self.dir2Pin[LEFT], GPIO.LOW) 126 | PWM.set_duty_cycle(self.pwmPin[LEFT], 0) 127 | 128 | def set_pwm_right(self, pwm_right): 129 | """ Set right motor PWM value """ 130 | 131 | self.pwm[RIGHT] = min( 132 | max(pwm_right, self.pwmLimits[MIN]), self.pwmLimits[MAX]) 133 | 134 | if self.pwm[RIGHT] > 0: 135 | GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) 136 | GPIO.output(self.dir2Pin[RIGHT], GPIO.HIGH) 137 | PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) 138 | elif self.pwm[RIGHT] < 0: 139 | GPIO.output(self.dir1Pin[RIGHT], GPIO.HIGH) 140 | GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) 141 | PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT])) 142 | else: 143 | GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW) 144 | GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW) 145 | PWM.set_duty_cycle(self.pwmPin[RIGHT], 0) 146 | 147 | def get_pwm(self): 148 | """ Get motor PWM values """ 149 | return self.pwm 150 | 151 | def start_threads(self): 152 | """ Start all threads """ 153 | self.cmd_parsing_thread.start() 154 | 155 | def update(self): 156 | """ Update which occures once per cycle of the run loop """ 157 | # Flash BBB LED 158 | if self.led_flag is True: 159 | self.led_flag = False 160 | GPIO.output(self.led, GPIO.HIGH) 161 | else: 162 | self.led_flag = True 163 | GPIO.output(self.led, GPIO.LOW) 164 | 165 | def run(self): 166 | """ The run loop """ 167 | # Start threads 168 | self.start_threads() 169 | 170 | # Run loop 171 | while self.run_flag: 172 | self.update() 173 | time.sleep(self.sample_time) 174 | self.cleanup() 175 | return 176 | 177 | def end_run(self): 178 | """ End the run loop. Gives time for threads to receive run_flag. """ 179 | self.run_flag = False 180 | time.sleep(2*self.sample_time) 181 | 182 | 183 | def cleanup(self): 184 | """ Clean up before shutting down. """ 185 | sys.stdout.write("Shutting down...") 186 | self.set_pwm([0, 0]) 187 | self.robotSocket.close() 188 | GPIO.cleanup() 189 | PWM.cleanup() 190 | if DEBUG: 191 | pass 192 | # tictocPrint() 193 | # self.writeBuffersToFile() 194 | sys.stdout.write("Done\n") 195 | 196 | # def writeBuffersToFile(self): 197 | # matrix = map(list, zip(*[self.encTimeRec[LEFT], self.encValRec[LEFT], 198 | # self.encPWMRec[LEFT], self.encNNewRec[LEFT], 199 | # self.encTickStateRec[LEFT], 200 | # self.enc_posRec[LEFT], 201 | # self.encVelRec[LEFT], 202 | # self.encThresholdRec[LEFT], 203 | # self.encTimeRec[RIGHT], 204 | # self.encValRec[RIGHT], 205 | # self.encPWMRec[RIGHT], self.encNNewRec[RIGHT], 206 | # self.encTickStateRec[RIGHT], 207 | # self.enc_posRec[RIGHT], self.encVelRec[RIGHT], 208 | # self.encThresholdRec[RIGHT]])) 209 | # s = [[str(e) for e in row] for row in matrix] 210 | # lens = [len(max(col, key=len)) for col in zip(*s)] 211 | # fmt = '\t'.join('{{:{}}}'.format(x) for x in lens) 212 | # table = [fmt.format(*row) for row in s] 213 | # f = open('output.txt', 'w') 214 | # f.write('\n'.join(table)) 215 | # f.close() 216 | # print "Wrote buffer to output.txt" 217 | 218 | 219 | def parse_cmd(self): 220 | """ Command parser """ 221 | try: 222 | while self.run_flag: 223 | try: 224 | line = self.robotSocket.recv(1024) 225 | except socket.error as msg: 226 | continue 227 | 228 | self.cmdBuffer += line 229 | 230 | # String contained within $ and * (with no $ or * symbols in it) 231 | buf_pattern = r'\$[^\$\*]*?\*' 232 | buf_regex = re.compile(buf_pattern) 233 | buf_result = buf_regex.search(self.cmdBuffer) 234 | 235 | if buf_result: 236 | msg = buf_result.group() 237 | print msg 238 | self.cmdBuffer = '' 239 | 240 | cmd_pattern = r'(?P[A-Z]{3,})' 241 | set_pattern = r'(?P=?)' 242 | query_pattern = r'(?P\??)' 243 | arg_pattern = r'(?(2)(?P.*))' 244 | msg_pattern = r'\$' + \ 245 | cmd_pattern + \ 246 | set_pattern + \ 247 | query_pattern + \ 248 | arg_pattern + \ 249 | r'.*\*' 250 | 251 | msg_regex = re.compile(msg_pattern) 252 | msg_result = msg_regex.search(msg) 253 | 254 | if msg_result.group('CMD') == 'CHECK': 255 | self.robotSocket.sendto( 256 | 'Hello from QuickBot\n', (self.base_ip, self.port)) 257 | 258 | elif msg_result.group('CMD') == 'PWM': 259 | if msg_result.group('QUERY'): 260 | if VERBOSE: 261 | print str(self.get_pwm()) 262 | self.robotSocket.sendto(str(self.get_pwm()) + '\n', 263 | (self.base_ip, self.port)) 264 | 265 | elif msg_result.group('SET') and msg_result.group('ARGS'): 266 | args = msg_result.group('ARGS') 267 | pwm_pattern = r'(?P[-]?\d+),(?P[-]?\d+)' 268 | pwm_regex = re.compile(pwm_pattern) 269 | pwm_result = pwm_regex.match(args) 270 | if pwm_result: 271 | pwm = [int(pwm_result.group('LEFT')), \ 272 | int(pwm_result.group('RIGHT'))] 273 | self.set_pwm(pwm) 274 | 275 | self.robotSocket.sendto(str(self.get_pwm()) + '\n', 276 | (self.base_ip, self.port)) 277 | 278 | elif msg_result.group('CMD') == 'IRVAL': 279 | if msg_result.group('QUERY'): 280 | reply = '[' + ', '.join(map(str, self.get_ir())) + ']' 281 | print 'Sending: ' + reply 282 | self.robotSocket.sendto( 283 | reply + '\n', (self.base_ip, self.port)) 284 | 285 | elif msg_result.group('CMD') == 'ULTRAVAL': 286 | if msg_result.group('QUERY'): 287 | reply = '[' + ', '.join(map(str, self.ultraVal)) + ']' 288 | print 'Sending: ' + reply 289 | self.robotSocket.sendto( 290 | reply + '\n', (self.base_ip, self.port)) 291 | 292 | elif msg_result.group('CMD') == 'WHEELANG': 293 | if msg_result.group('QUERY'): 294 | print 'Sending: ' + str(self.get_wheel_ang()) 295 | self.robotSocket.sendto( 296 | str(self.get_wheel_ang()) + 297 | '\n', (self.base_ip, self.port)) 298 | 299 | elif msg_result.group('SET') and msg_result.group('ARGS'): 300 | args = msg_result.group('ARGS') 301 | arg_pattern = \ 302 | r'(?P[-]?\d+[\.]?\d*),(?P[-]?\d+[\.]?\d*)' 303 | regex = re.compile(arg_pattern) 304 | result = regex.match(args) 305 | if result: 306 | pos = [float(regex.match(args).group('LEFT')), \ 307 | float(regex.match(args).group('RIGHT'))] 308 | self.set_wheel_ang(pos) 309 | 310 | elif msg_result.group('CMD') == 'ENVAL': 311 | if msg_result.group('QUERY'): 312 | reply = \ 313 | '[' + ', '.join(map(str, self.get_enc_val())) + ']' 314 | print 'Sending: ' + reply 315 | self.robotSocket.sendto( 316 | reply + '\n', (self.base_ip, self.port)) 317 | 318 | elif msg_result.group('SET') and msg_result.group('ARGS'): 319 | args = msg_result.group('ARGS') 320 | arg_pattern = \ 321 | r'(?P[-]?\d+[\.]?\d*),(?P[-]?\d+[\.]?\d*)' 322 | regex = re.compile(arg_pattern) 323 | result = regex.match(args) 324 | if result: 325 | enc_pos = [float(regex.match(args).group('LEFT')), \ 326 | float(regex.match(args).group('RIGHT'))] 327 | self.set_enc_val(enc_pos) 328 | 329 | elif msg_result.group('CMD') == 'ENRAW': 330 | if msg_result.group('QUERY'): 331 | reply = \ 332 | '[' + ', '.join(map(str, self.get_enc_raw())) + ']' 333 | print 'Sending: ' + reply 334 | self.robotSocket.sendto( 335 | reply + '\n', (self.base_ip, self.port)) 336 | 337 | elif msg_result.group('CMD') == 'ENOFFSET': 338 | if msg_result.group('QUERY'): 339 | reply = '[' + \ 340 | ', '.join(map(str, self.get_enc_offset())) + ']' 341 | print 'Sending: ' + reply 342 | self.robotSocket.sendto( 343 | reply + '\n', (self.base_ip, self.port)) 344 | 345 | elif msg_result.group('SET') and msg_result.group('ARGS'): 346 | args = msg_result.group('ARGS') 347 | arg_pattern = \ 348 | r'(?P[-]?\d+[\.]?\d*),(?P[-]?\d+[\.]?\d*)' 349 | regex = re.compile(arg_pattern) 350 | result = regex.match(args) 351 | if result: 352 | offset = [float(regex.match(args).group('LEFT')), \ 353 | float(regex.match(args).group('RIGHT'))] 354 | self.set_enc_offset(offset) 355 | 356 | elif msg_result.group('CMD') == 'ENVEL': 357 | if msg_result.group('QUERY'): 358 | reply = \ 359 | '[' + ', '.join(map(str, self.get_enc_vel())) + ']' 360 | print 'Sending: ' + reply 361 | self.robotSocket.sendto( 362 | reply + '\n', (self.base_ip, self.port)) 363 | 364 | elif msg_result.group('SET') and msg_result.group('ARGS'): 365 | args = msg_result.group('ARGS') 366 | arg_pattern = \ 367 | r'(?P[-]?\d+[\.]?\d*),(?P[-]?\d+[\.]?\d*)' 368 | regex = re.compile(arg_pattern) 369 | result = regex.match(args) 370 | if result: 371 | enc_vel = [float(regex.match(args).group('LEFT')), \ 372 | float(regex.match(args).group('RIGHT'))] 373 | self.set_enc_vel(enc_vel) 374 | 375 | elif msg_result.group('CMD') == 'WHEELANGVEL': 376 | if msg_result.group('QUERY'): 377 | reply = \ 378 | '[' + ', '.join(map(str, self.get_wheel_ang_vel())) + ']' 379 | print 'Sending: ' + reply 380 | self.robotSocket.sendto( 381 | reply + '\n', (self.base_ip, self.port)) 382 | 383 | elif msg_result.group('SET') and msg_result.group('ARGS'): 384 | args = msg_result.group('ARGS') 385 | arg_pattern = \ 386 | r'(?P[-]?\d+[\.]?\d*),(?P[-]?\d+[\.]?\d*)' 387 | regex = re.compile(arg_pattern) 388 | result = regex.match(args) 389 | if result: 390 | wheel_ang_vel = [float(regex.match(args).group('LEFT')), \ 391 | float(regex.match(args).group('RIGHT'))] 392 | self.set_wheel_ang_vel(wheel_ang_vel) 393 | 394 | elif msg_result.group('CMD') == 'ENRESET': 395 | self.reset_enc_val() 396 | reply = \ 397 | '[' + ', '.join(map(str, self.get_enc_val())) + ']' 398 | print 'Encoder values reset to ' + reply 399 | 400 | elif msg_result.group('CMD') == 'UPDATE': 401 | if msg_result.group('SET') and msg_result.group('ARGS'): 402 | args = msg_result.group('ARGS') 403 | pwm_pattern = r'(?P[-]?\d+),(?P[-]?\d+)' 404 | pwm_regex = re.compile(pwm_pattern) 405 | pwm_result = pwm_regex.match(args) 406 | if pwm_result: 407 | pwm = [int(pwm_regex.match(args).group('LEFT')), \ 408 | int(pwm_regex.match(args).group('RIGHT'))] 409 | self.set_pwm(pwm) 410 | 411 | reply = '[' + ', '.join(map(str, self.enc_pos)) + ', ' \ 412 | + ', '.join(map(str, self.encVel)) + ']' 413 | print 'Sending: ' + reply 414 | self.robotSocket.sendto( 415 | reply + '\n', (self.base_ip, self.port)) 416 | 417 | elif msg_result.group('CMD') == 'END': 418 | self.end_run() 419 | 420 | else: 421 | print 'Invalid: ' + msg 422 | except: 423 | self.end_run() 424 | raise 425 | -------------------------------------------------------------------------------- /xbots/quickbot_v1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | """ 3 | @brief QuickBot class for Beaglebone Black 4 | 5 | @author Rowland O'Flaherty (rowlandoflaherty.com) 6 | @date 02/07/2014 7 | @version: 1.0 8 | @copyright: Copyright (C) 2014, Georgia Tech Research Corporation 9 | see the LICENSE file included with this software (see LINENSE file) 10 | """ 11 | 12 | from __future__ import division 13 | import threading 14 | import time 15 | import base 16 | import utils 17 | 18 | import numpy as np 19 | import quickbot_v1_config as config 20 | import Adafruit_BBIO.ADC as ADC 21 | 22 | # Constants 23 | 24 | ADCTIME = 0.001 25 | 26 | # Encoder buffer constants and variables 27 | ENC_BUF_SIZE = 2 ** 9 28 | ENC_IND = [0, 0] 29 | ENC_TIME = [[0] * ENC_BUF_SIZE, [0] * ENC_BUF_SIZE] 30 | ENC_VAL = [[0] * ENC_BUF_SIZE, [0] * ENC_BUF_SIZE] 31 | 32 | ADC_LOCK = threading.Lock() 33 | 34 | 35 | class QuickBot(base.BaseBot): 36 | """The QuickBot Class""" 37 | 38 | # Parameters 39 | sampleTime = 20.0 / 1000.0 40 | 41 | # Motor Pins -- (LEFT, RIGHT) 42 | dir1Pin = (config.INl1, config.INr1) 43 | dir2Pin = (config.INl2, config.INr2) 44 | pwmPin = (config.PWMl, config.PWMr) 45 | 46 | # LED pin 47 | led = config.LED 48 | 49 | # Encoder counting parameter and variables 50 | ticksPerTurn = 16 # Number of ticks on encoder disc 51 | encWinSize = 2 ** 5 # Should be power of 2 52 | minPWMThreshold = [45, 45] # Threshold on the minimum value to turn wheel 53 | encTPrev = [0.0, 0.0] 54 | encThreshold = [0.0, 0.0] 55 | encTickState = [0, 0] 56 | encTickStateVec = np.zeros((2, encWinSize)) 57 | 58 | # Constraints 59 | pwmLimits = [-100, 100] # [min, max] 60 | 61 | # State PWM -- (LEFT, RIGHT) 62 | pwm = [0, 0] 63 | 64 | # State IR 65 | irVal = [0.0, 0.0, 0.0, 0.0, 0.0] 66 | ithIR = 0 67 | 68 | # State Encoder 69 | encTime = [0.0, 0.0] # Last time encoders were read 70 | encPos = [0.0, 0.0] # Last encoder tick position 71 | encVel = [0.0, 0.0] # Last encoder tick velocity 72 | 73 | # Encoder counting parameters 74 | encCnt = 0 # Count of number times encoders have been read 75 | encSumN = [0, 0] # Sum of total encoder samples 76 | encBufInd0 = [0, 0] # Index of beginning of new samples in buffer 77 | encBufInd1 = [0, 0] # Index of end of new samples in buffer 78 | # Moving window of encoder sample times 79 | encTimeWin = np.zeros((2, encWinSize)) 80 | # Moving window of encoder raw sample values 81 | encValWin = np.zeros((2, encWinSize)) 82 | # Moving window corresponding PWM input values 83 | encPWMWin = np.zeros((2, encWinSize)) 84 | encTau = [0.0, 0.0] # Average sampling time of encoders 85 | 86 | # Stats of encoder values while input = 0 and vel = 0 87 | # Min number of recorded values to start calculating stats 88 | encZeroCntMin = 2 ** 4 89 | encZeroMean = [0.0, 0.0] 90 | encZeroVar = [0.0, 0.0] 91 | encZeroCnt = [0, 0] 92 | encHighCnt = [0, 0] 93 | encLowCnt = [0, 0] 94 | encLowCntMin = 2 95 | 96 | def __init__(self, baseIP, robotIP): 97 | super(QuickBot, self).__init__(baseIP, robotIP) 98 | # init encoder 99 | self.encoderRead = EncoderReader() 100 | # Initialize ADC 101 | ADC.setup() 102 | 103 | if base.DEBUG: 104 | # Stats of encoder values while moving -- high, low, and all tick 105 | # state 106 | # Min number of recorded values to start calculating stats 107 | self.encHighLowCntMin = 2**5 108 | self.encHighMean = [0.0, 0.0] 109 | self.encHighVar = [0.0, 0.0] 110 | self.encHighTotalCnt = [0, 0] 111 | 112 | self.encLowMean = [0.0, 0.0] 113 | self.encLowVar = [0.0, 0.0] 114 | self.encLowTotalCnt = [0, 0] 115 | 116 | self.encNonZeroCntMin = 2**5 117 | self.encNonZeroMean = [0.0, 0.0] 118 | self.encNonZeroVar = [0.0, 0.0] 119 | self.encNonZeroCnt = [0, 0] 120 | 121 | # Record variables 122 | self.encRecSize = 2**13 123 | self.encRecInd = [0, 0] 124 | self.encTimeRec = np.zeros((2, self.encRecSize)) 125 | self.encValRec = np.zeros((2, self.encRecSize)) 126 | self.encPWMRec = np.zeros((2, self.encRecSize)) 127 | self.encNNewRec = np.zeros((2, self.encRecSize)) 128 | self.encPosRec = np.zeros((2, self.encRecSize)) 129 | self.encVelRec = np.zeros((2, self.encRecSize)) 130 | self.encTickStateRec = np.zeros((2, self.encRecSize)) 131 | self.encThresholdRec = np.zeros((2, self.encRecSize)) 132 | 133 | 134 | 135 | def update(self): 136 | self.readIRValues() 137 | self.readEncoderValues() 138 | 139 | def get_ir(self): 140 | """ Getter for IR sensor values """ 141 | return self.irVal 142 | 143 | def readIRValues(self): 144 | prevVal = self.irVal[self.ithIR] 145 | ADC_LOCK.acquire() 146 | self.irVal[self.ithIR] = ADC.read_raw(config.IRS[self.ithIR]) 147 | time.sleep(ADCTIME) 148 | ADC_LOCK.release() 149 | 150 | if self.irVal[self.ithIR] >= 1100: 151 | self.irVal[self.ithIR] = prevVal 152 | 153 | self.ithIR = ((self.ithIR + 1) % 5) 154 | 155 | def readEncoderValues(self): 156 | """ 157 | We read the raw adc data and try to determine if we had a rising or 158 | falling edge. After that we try to calculate how many ticks we got. 159 | """ 160 | if base.DEBUG and (self.encCnt % 10) == 0: 161 | print "------------------------------------" 162 | print "EncPos: " + str(self.encPos) 163 | print "EncVel: " + str(self.encVel) 164 | print "***" 165 | print "Threshold: " + str(self.encThreshold) 166 | print "***" 167 | print "Zero Cnt: " + str(self.encZeroCnt) 168 | print "Zero Mean: " + str(self.encZeroMean) 169 | print "Zero Var: " + str(self.encZeroVar) 170 | print "***" 171 | print "NonZero Cnt: " + str(self.encNonZeroCnt) 172 | print "NonZero Mean: " + str(self.encNonZeroMean) 173 | print "NonZero Var: " + str(self.encNonZeroVar) 174 | print "***" 175 | print "High Cnt: " + str(self.encHighTotalCnt) 176 | print "High Mean: " + str(self.encHighMean) 177 | print "High Var: " + str(self.encHighVar) 178 | print "***" 179 | print "Low Cnt: " + str(self.encLowTotalCnt) 180 | print "Low Mean: " + str(self.encLowMean) 181 | print "Low Var: " + str(self.encLowVar) 182 | 183 | self.encCnt = self.encCnt + 1 184 | 185 | # Fill window 186 | for side in range(0, 2): 187 | self.encTime[side] = self.encTimeWin[side][-1] 188 | 189 | self.encBufInd0[side] = self.encBufInd1[side] 190 | self.encBufInd1[side] = ENC_IND[side] 191 | ind0 = self.encBufInd0[side] # starting index 192 | # ending index (this element is not included until the next update) 193 | ind1 = self.encBufInd1[side] 194 | 195 | if ind0 < ind1: 196 | N = ind1 - ind0 # number of new elements 197 | self.encSumN[side] = self.encSumN[side] + N 198 | self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) 199 | self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:ind1] 200 | self.encValWin[side] = np.roll(self.encValWin[side], -N) 201 | self.encValWin[side, -N:] = ENC_VAL[side][ind0:ind1] 202 | self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) 203 | self.encPWMWin[side, -N:] = [self.pwm[side]] * N 204 | 205 | elif ind0 > ind1: 206 | N = ENC_BUF_SIZE - ind0 + ind1 # number of new elements 207 | self.encSumN[side] = self.encSumN[side] + N 208 | self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N) 209 | self.encValWin[side] = np.roll(self.encValWin[side], -N) 210 | self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N) 211 | self.encPWMWin[side, -N:] = [self.pwm[side]] * N 212 | if ind1 == 0: 213 | self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:] 214 | self.encValWin[side, -N:] = ENC_VAL[side][ind0:] 215 | else: 216 | self.encTimeWin[side, -N:-ind1] = ENC_TIME[side][ind0:] 217 | self.encValWin[side, -N:-ind1] = ENC_VAL[side][ind0:] 218 | self.encTimeWin[side, -ind1:] = ENC_TIME[side][0:ind1] 219 | self.encValWin[side, -ind1:] = ENC_VAL[side][0:ind1] 220 | 221 | if ind0 != ind1: 222 | tauNew = self.encTimeWin[side, -1] - self.encTimeWin[side, -N] 223 | # Running average 224 | self.encTau[side] = tauNew / self.encCnt + \ 225 | self.encTau[side] * (self.encCnt - 1) / self.encCnt 226 | if self.encSumN[side] > self.encWinSize: 227 | self.countEncoderTicks(side) 228 | 229 | # Fill records 230 | if base.DEBUG: 231 | ind = self.encRecInd[side] 232 | if ind + N < self.encRecSize: 233 | self.encTimeRec[ 234 | side, ind:ind + N] = self.encTimeWin[side, -N:] 235 | self.encValRec[ 236 | side, ind:ind + N] = self.encValWin[side, -N:] 237 | self.encPWMRec[ 238 | side, ind:ind + N] = self.encPWMWin[side, -N:] 239 | self.encNNewRec[side, ind:ind + N] = [N] * N 240 | self.encPosRec[ 241 | side, ind:ind + N] = [self.encPos[side]] * N 242 | self.encVelRec[ 243 | side, ind:ind + N] = [self.encVel[side]] * N 244 | self.encTickStateRec[ 245 | side, ind:ind + N] = self.encTickStateVec[side, 246 | -N:] 247 | self.encThresholdRec[ 248 | side, ind:ind + N] = [self.encThreshold[side]] * N 249 | self.encRecInd[side] = ind + N 250 | 251 | def countEncoderTicks(self, side): 252 | # Set variables 253 | # Time vector of data (non-consistent sampling time) 254 | t = self.encTimeWin[side] 255 | tPrev = self.encTPrev[side] # Previous read time 256 | pwm = self.encPWMWin[side] # Vector of PWM data 257 | pwmPrev = pwm[-1] # Last PWM value that was applied 258 | # Last state of tick (high (1), low (-1), or unsure (0)) 259 | tickStatePrev = self.encTickState[side] 260 | tickCnt = self.encPos[side] # Current tick count 261 | tickVel = self.encVel[side] # Current tick velocity 262 | encValWin = self.encValWin[side] # Encoder raw value buffer window 263 | threshold = self.encThreshold[side] # Encoder value threshold 264 | # Minimum PWM to move wheel 265 | minPWMThreshold = self.minPWMThreshold[side] 266 | 267 | N = np.sum(t > tPrev) # Number of new updates 268 | 269 | tickStateVec = np.roll(self.encTickStateVec[side], -N) 270 | 271 | # Determine wheel direction 272 | if tickVel != 0: 273 | wheelDir = np.sign(tickVel) 274 | else: 275 | wheelDir = np.sign(pwmPrev) 276 | 277 | # Count ticks and record tick state 278 | indTuple = np.where(t == tPrev) # Index of previous sample in window 279 | if len(indTuple[0] > 0): 280 | ind = indTuple[0][0] 281 | newInds = ind + np.arange(1, N + 1) # Indices of new samples 282 | for i in newInds: 283 | if encValWin[i] > threshold: # High tick state 284 | tickState = 1 285 | self.encHighCnt[side] = self.encHighCnt[side] + 1 286 | self.encLowCnt[side] = 0 287 | # Increment tick count on rising edge 288 | if tickStatePrev == -1: 289 | tickCnt = tickCnt + wheelDir 290 | 291 | else: # Low tick state 292 | tickState = -1 293 | self.encLowCnt[side] = self.encLowCnt[side] + 1 294 | self.encHighCnt[side] = 0 295 | tickStatePrev = tickState 296 | tickStateVec[i] = tickState 297 | 298 | # Measure tick speed 299 | # Tick state transition differences 300 | diffTickStateVec = np.diff(tickStateVec) 301 | # Times when tick state goes from high to low 302 | fallingTimes = t[np.hstack((False, diffTickStateVec == -2))] 303 | # Times when tick state goes from low to high 304 | risingTimes = t[np.hstack((False, diffTickStateVec == 2))] 305 | # Period times between falling edges 306 | fallingPeriods = np.diff(fallingTimes) 307 | # Period times between rising edges 308 | risingPeriods = np.diff(risingTimes) 309 | tickPeriods = np.hstack( 310 | (fallingPeriods, risingPeriods)) # All period times 311 | if len(tickPeriods) == 0: 312 | # If all inputs are less than min set velocity to 0 313 | if all(pwm[newInds] < minPWMThreshold): 314 | tickVel = 0 315 | else: 316 | # Average signed tick frequency 317 | tickVel = wheelDir * 1 / np.mean(tickPeriods) 318 | 319 | # Estimate new mean values 320 | newEncRaw = encValWin[newInds] 321 | if pwmPrev == 0 and tickVel == 0: 322 | x = newEncRaw 323 | l = self.encZeroCnt[side] 324 | mu = self.encZeroMean[side] 325 | sigma2 = self.encZeroVar[side] 326 | (muPlus, sigma2Plus, n) = utils.recursiveMeanVar(x, l, mu, 327 | sigma2) 328 | self.encZeroMean[side] = muPlus 329 | self.encZeroVar[side] = sigma2Plus 330 | self.encZeroCnt[side] = n 331 | elif tickVel != 0: 332 | if base.DEBUG: 333 | x = newEncRaw 334 | l = self.encNonZeroCnt[side] 335 | mu = self.encNonZeroMean[side] 336 | sigma2 = self.encNonZeroVar[side] 337 | (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( 338 | x, l, mu, sigma2) 339 | self.encNonZeroMean[side] = muPlus 340 | self.encNonZeroVar[side] = sigma2Plus 341 | self.encNonZeroCnt[side] = n 342 | 343 | NHigh = np.sum(tickStateVec[newInds] == 1) 344 | if NHigh != 0: 345 | indHighTuple = np.where(tickStateVec[newInds] == 1) 346 | x = newEncRaw[indHighTuple[0]] 347 | l = self.encHighTotalCnt[side] 348 | mu = self.encHighMean[side] 349 | sigma2 = self.encHighVar[side] 350 | (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( 351 | x, l, mu, sigma2) 352 | self.encHighMean[side] = muPlus 353 | self.encHighVar[side] = sigma2Plus 354 | self.encHighTotalCnt[side] = n 355 | 356 | NLow = np.sum(tickStateVec[newInds] == -1) 357 | if NLow != 0: 358 | indLowTuple = np.where(tickStateVec[newInds] == -1) 359 | x = newEncRaw[indLowTuple[0]] 360 | l = self.encLowTotalCnt[side] 361 | mu = self.encLowMean[side] 362 | sigma2 = self.encLowVar[side] 363 | (muPlus, sigma2Plus, n) = utils.recursiveMeanVar( 364 | x, l, mu, sigma2) 365 | self.encLowMean[side] = muPlus 366 | self.encLowVar[side] = sigma2Plus 367 | self.encLowTotalCnt[side] = n 368 | 369 | # Set threshold value 370 | if self.encZeroCnt[side] > self.encZeroCntMin: 371 | self.encThreshold[side] = self.encZeroMean[ 372 | side] - 3 * np.sqrt(self.encZeroVar[side]) 373 | 374 | # elif self.encNonZeroCnt[side] > self.encNonZeroCntMin: 375 | # self.encThreshold[side] = self.encNonZeroMean[side] 376 | 377 | # elif self.encHighTotalCnt[side] > self.encHighLowCntMin and \ 378 | # self.encLowTotalCnt > self.encHighLowCntMin: 379 | # mu1 = self.encHighMean[side] 380 | # sigma1 = self.encHighVar[side] 381 | # mu2 = self.encLowMean[side] 382 | # sigma2 = self.encLowVar[side] 383 | # alpha = (sigma1 * np.log(sigma1)) / (sigma2 * np.log(sigma1)) 384 | # A = (1 - alpha) 385 | # B = -2 * (mu1 - alpha*mu2) 386 | # C = mu1**2 - alpha * mu2**2 387 | # x1 = (-B + np.sqrt(B**2 - 4*A*C)) / (2*A) 388 | # x2 = (-B - np.sqrt(B**2 - 4*A*C)) / (2*A) 389 | # if x1 < mu1 and x1 > mu2: 390 | # self.encThreshold[side] = x1 391 | # else: 392 | # self.encThreshold[side] = x2 393 | 394 | # Update variables 395 | self.encPos[side] = tickCnt # New tick count 396 | self.encVel[side] = tickVel # New tick velocity 397 | self.encTickStateVec[side] = tickStateVec # New tick state vector 398 | 399 | self.encTPrev[side] = t[-1] # New latest update time 400 | 401 | 402 | class EncoderReader(threading.Thread): 403 | """EncoderReader thread""" 404 | 405 | def __init__(self, encPin=(config.Ol, config.Or)): 406 | 407 | # Initialize thread 408 | threading.Thread.__init__(self) 409 | 410 | # Set properties 411 | self.encPin = encPin 412 | 413 | def run(self): 414 | self.t0 = time.time() 415 | 416 | while base.RUN_FLAG: 417 | global ENC_IND 418 | global ENC_TIME 419 | global ENC_VAL 420 | 421 | for side in range(0, 2): 422 | ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0 423 | ADC_LOCK.acquire() 424 | ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side]) 425 | time.sleep(ADCTIME) 426 | ADC_LOCK.release() 427 | ENC_IND[side] = (ENC_IND[side] + 1) % ENC_BUF_SIZE 428 | -------------------------------------------------------------------------------- /xbots/quickbot_v1_config.py: -------------------------------------------------------------------------------- 1 | # quickbot 2 | # 3 | # _'_'_ IRfm 4 | # ++++++++++++++ 5 | # _/ + + \_ 6 | # IRfl _/ + + \_ IRfr 7 | # / + LMP RMP + \ 8 | # + __| |__ + 9 | # + | | + 10 | # + | | + 11 | # _|++++ | _ _ | ++++|_ 12 | # IRbl _|++++_| T T T T |_++++|_ IRbr 13 | # |++++ | | | | ++++| 14 | # ++++ Ol Or ++++ 15 | # + + 16 | # ++++++++++++++++++ 17 | # 18 | # ir 19 | IRbl = "P9_38" 20 | IRfl = "P9_40" 21 | IRfm = "P9_36" 22 | IRfr = "P9_35" 23 | IRbr = "P9_33" 24 | IRS = (IRbl, IRfl, IRfm, IRfr, IRbr) 25 | 26 | # encoder aka odometry 27 | Ol = "P9_39" 28 | Or = "P9_37" 29 | 30 | # motors 31 | INl1 = "P8_14" 32 | INl2 = "P8_16" 33 | PWMl = "P9_16" 34 | 35 | INr1 = "P8_12" 36 | INr2 = "P8_10" 37 | PWMr = "P9_14" 38 | 39 | RMP = (INr1, INr2, PWMr) 40 | LMP = (INl1, INl2, PWMl) 41 | 42 | # led 43 | LED = "USR1" 44 | -------------------------------------------------------------------------------- /xbots/quickbot_v2.py: -------------------------------------------------------------------------------- 1 | """ 2 | @brief QuickBot class 3 | 4 | @author Rowland O'Flaherty (rowlandoflaherty.com) 5 | @date 05/30/2014 6 | @version: 2.0 7 | @copyright: Copyright (C) 2014, see the LICENSE file 8 | """ 9 | 10 | from __future__ import division 11 | import threading 12 | import time 13 | import base 14 | import utils 15 | import re 16 | import serial 17 | import math 18 | 19 | import numpy as np 20 | from numpy import pi as PI 21 | 22 | import quickbot_v2_config as config 23 | import Adafruit_BBIO.ADC as ADC 24 | 25 | from base import RIGHT 26 | from base import LEFT 27 | 28 | # ADC Constants 29 | ADCTIME = 0.002 30 | ADC_LOCK = threading.Lock() 31 | 32 | class QuickBot(base.BaseBot): 33 | """The QuickBot Class""" 34 | 35 | # Parameters 36 | sample_time = 20.0 / 1000.0 37 | 38 | # Motor Pins -- (LEFT, RIGHT) 39 | dir1Pin = (config.MOTOR_L.dir1, config.MOTOR_R.dir1) 40 | dir2Pin = (config.MOTOR_L.dir2, config.MOTOR_R.dir2) 41 | pwmPin = (config.MOTOR_L.pwm, config.MOTOR_R.pwm) 42 | 43 | # LED pin 44 | led = config.LED 45 | 46 | # Encoder Serial 47 | encoderSerial = ( 48 | serial.Serial( 49 | port=config.EN_L.port, 50 | baudrate=config.EN_L.baudrate, 51 | timeout=.1), 52 | serial.Serial( 53 | port=config.EN_R.port, 54 | baudrate=config.EN_R.baudrate, 55 | timeout=.1)) 56 | encoderBuffer = ['', ''] 57 | 58 | # Wheel parameters 59 | ticksPerTurn = 128 # Number of ticks on encoder disc 60 | wheelRadius = (58.7 / 2.0) / 1000.0 # Radius of wheel in meters 61 | 62 | # State encoder 63 | enc_raw = [0.0, 0.0] # Last encoder tick position 64 | enc_vel = [0.0, 0.0] # Last encoder tick velocity 65 | enc_time = [0.0, 0.0] # Last encoder tick sample time 66 | enc_state = 2*[np.array([[0.0, 0.0]]).T] # Last encoder state -- pos, vel, acc 67 | enc_state_cov = 2*[np.array([[1.0, 0.0], 68 | [0.0, 1.0]])] 69 | 70 | # Controller paramerters 71 | enc_vel_controller_flag = 2*[False] 72 | 73 | def __init__(self, base_ip, robot_ip): 74 | super(QuickBot, self).__init__(base_ip, robot_ip) 75 | 76 | # Start time 77 | self.t0 = time.time() 78 | 79 | # State IR 80 | self.n_ir = len(config.IR) 81 | self.ir_val = self.n_ir*[0.0] 82 | 83 | # State Encoder 84 | self.enc_dir = [1, -1] # Last encoder direction 85 | self.enc_raw = [0, 0] # Last encoder tick position 86 | self.enc_vel = [0.0, 0.0] # Last encoder tick velocity 87 | self.enc_offset = [0.0, 0.0] # Offset from raw encoder tick 88 | 89 | # Set Points 90 | self.enc_vel_set_point = [0.0, 0.0] 91 | 92 | # Initialize ADC 93 | ADC.setup() 94 | 95 | # Initialize IR thread 96 | self.ir_thread = threading.Thread(target=read_ir_thread_fcn, args=(self, )) 97 | self.ir_thread.daemon = True 98 | 99 | # Initialize encoder threads 100 | self.enc_pos_thread = 2*[None] 101 | for side in range(0, 2): 102 | self.enc_pos_thread[side] = threading.Thread( 103 | target=read_enc_val_thread_fcn, args=(self, side)) 104 | self.enc_pos_thread[side].daemon = True 105 | 106 | # Initialize wheel controller thread 107 | self.enc_vel_controller_thread = 2*[None] 108 | for side in range(0, 2): 109 | self.enc_vel_controller_thread[side] = threading.Thread( 110 | target=enc_vel_controller_thread_fcn, args=(self, side)) 111 | self.enc_vel_controller_thread[side].daemon = True 112 | 113 | 114 | def start_threads(self): 115 | """ Start all threads """ 116 | self.ir_thread.start() 117 | for side in range(0, 2): 118 | self.enc_pos_thread[side].start() 119 | self.enc_vel_controller_thread[side].start() 120 | 121 | # Calibrate encoders 122 | self.calibrate_enc_val() 123 | 124 | # Call parent method 125 | super(QuickBot, self).start_threads() 126 | 127 | def set_pwm_left(self, pwm_left): 128 | """ Set left motor PWM value """ 129 | self.enc_vel_controller_flag[LEFT] = False 130 | super(QuickBot, self).set_pwm_left(pwm_left) 131 | 132 | 133 | def set_pwm_right(self, pwm_right): 134 | """ Set right motor PWM value """ 135 | self.enc_vel_controller_flag[RIGHT] = False 136 | super(QuickBot, self).set_pwm_right(pwm_right) 137 | 138 | 139 | def get_ir(self): 140 | """ Getter for IR sensor values """ 141 | return self.ir_val 142 | 143 | def calibrate_enc_val(self): 144 | """ Calibrate wheel encoder values""" 145 | self.set_pwm([100, 100]) 146 | time.sleep(0.1) 147 | self.set_pwm([0, 0]) 148 | time.sleep(1.0) 149 | self.reset_enc_val() 150 | 151 | def get_enc_raw(self): 152 | """ Getter for raw encoder values """ 153 | return self.enc_raw 154 | 155 | def get_enc_val(self): 156 | """ Getter for encoder tick values i.e (raw - offset) """ 157 | return [self.enc_raw[LEFT] - self.enc_offset[LEFT], 158 | -1*(self.enc_raw[RIGHT] - self.enc_offset[RIGHT])] 159 | 160 | def set_enc_val(self, enc_val): 161 | """ Setter for encoder tick positions """ 162 | offset = [0.0, 0.0] 163 | offset[LEFT] = self.enc_raw[LEFT] - enc_val[LEFT] 164 | offset[RIGHT] = -1*(self.enc_raw[RIGHT] - enc_val[RIGHT]) 165 | self.set_enc_offset(offset) 166 | 167 | def get_wheel_ang(self): 168 | """ Getter for wheel angles """ 169 | ang = [0.0, 0.0] 170 | enc_val = self.get_enc_val() 171 | for side in range(0, 2): 172 | ang[side] = enc_val[side] / self.ticksPerTurn * 2 * PI 173 | return ang 174 | 175 | def set_wheel_ang(self, ang): # FIXME - Should move wheel to that angle 176 | """ Setter for wheel angles """ 177 | enc_val = [0.0, 0.0] 178 | for side in range(0, 2): 179 | enc_val[side] = ang[side] * self.ticksPerTurn / (2 * PI) 180 | self.set_enc_val(enc_val) 181 | 182 | def get_enc_offset(self): 183 | """ Getter for encoder offset values """ 184 | return self.enc_offset 185 | 186 | def set_enc_offset(self, offset): 187 | """ Setter for encoder offset values """ 188 | for side in range(0, 2): 189 | self.enc_offset[side] = offset[side] 190 | 191 | def reset_enc_val(self): 192 | """ Reset encoder values to 0 """ 193 | self.enc_offset[LEFT] = self.enc_raw[LEFT] 194 | self.enc_offset[RIGHT] = self.enc_raw[RIGHT] 195 | 196 | def get_enc_vel(self): 197 | """ Getter for encoder velocity values """ 198 | return self.enc_vel 199 | 200 | def set_enc_vel(self, env_vel): 201 | """ Setter for encoder velocity values """ 202 | for side in range(0, 2): 203 | self.enc_vel_set_point[side] = env_vel[side] 204 | self.enc_vel_controller_flag = 2*[True] 205 | 206 | def get_wheel_ang_vel(self): 207 | """ Getter for wheel angular velocity values """ 208 | ang_vel = [0.0, 0.0] 209 | enc_vel = self.get_enc_vel() 210 | for side in range(0, 2): 211 | ang_vel[side] = enc_vel[side] * (2 * PI) / self.ticksPerTurn 212 | return ang_vel 213 | 214 | def set_wheel_ang_vel(self, ang_vel): 215 | """ Setter for wheel angular velocity values """ 216 | for side in range(0, 2): 217 | self.enc_vel_set_point[side] = ang_vel[side] * self.ticksPerTurn / (2 * PI) 218 | self.enc_vel_controller_flag = 2*[True] 219 | 220 | 221 | def enc_vel_controller_thread_fcn(self, side): 222 | """ Thread function for controlling for encoder tick velocity """ 223 | while self.run_flag: 224 | if self.enc_vel_controller_flag[side]: 225 | x = self.enc_vel[side] 226 | u = self.pwm[side] 227 | x_bar = self.enc_vel_set_point[side] 228 | 229 | u_plus = enc_vel_controller(x, u, x_bar) 230 | 231 | if side == LEFT: 232 | super(QuickBot, self).set_pwm_left(u_plus) 233 | else: 234 | super(QuickBot, self).set_pwm_right(u_plus) 235 | 236 | time.sleep(self.sample_time) 237 | 238 | def enc_vel_controller(x, u, x_bar): 239 | """ Wheel angular velocity controller """ 240 | controller_type = 'PID' 241 | 242 | if controller_type == 'PID': 243 | P = 0.05 244 | u_plus = P * (x_bar - x) + u 245 | 246 | return u_plus 247 | 248 | 249 | def read_ir_thread_fcn(self): 250 | """ Thread function for reading IR sensor values """ 251 | while self.run_flag: 252 | for i in range(0, self.n_ir): 253 | ADC_LOCK.acquire() 254 | self.ir_val[i] = ADC.read_raw(config.IR[i]) 255 | time.sleep(ADCTIME) 256 | ADC_LOCK.release() 257 | 258 | 259 | def read_enc_val_thread_fcn(self, side): 260 | """ Thread function for reading encoder values """ 261 | while self.run_flag: 262 | parse_encoder_buffer(self, side) 263 | time.sleep(self.sample_time) 264 | 265 | 266 | def parse_encoder_buffer(self, side): 267 | """ Parses encoder serial data """ 268 | enc_pos_update_flag = False 269 | enc_vel_update_flag = False 270 | 271 | t = time.time() - self.t0 272 | ts = t - self.enc_time[side] 273 | 274 | z = np.array([[np.NaN]]) 275 | 276 | bytes_in_waiting = self.encoderSerial[side].inWaiting() 277 | 278 | if bytes_in_waiting > 0: 279 | self.encoderBuffer[side] += \ 280 | self.encoderSerial[side].read(bytes_in_waiting) 281 | 282 | if len(self.encoderBuffer[side]) > 30: 283 | self.encoderBuffer[side] = self.encoderBuffer[side][-30:] 284 | 285 | if len(self.encoderBuffer[side]) >= 15: 286 | d_pattern = r'D([0-9A-F]{8})' 287 | d_regex = re.compile(d_pattern) 288 | d_result = d_regex.findall(self.encoderBuffer[side]) 289 | if len(d_result) >= 1: 290 | val = utils.convertHEXtoDEC(d_result[-1], 8) 291 | if not math.isnan(val): 292 | self.enc_raw[side] = val 293 | enc_pos_update_flag = True 294 | 295 | v_pattern = r'V([0-9A-F]{4})' 296 | v_regex = re.compile(v_pattern) 297 | v_result = v_regex.findall(self.encoderBuffer[side]) 298 | if len(v_result) >= 1: 299 | vel = utils.convertHEXtoDEC(v_result[-1], 4) 300 | 301 | if not math.isnan(vel) and not enc_vel_update_flag: 302 | if side == RIGHT: 303 | vel = -1*vel 304 | z = np.array([[vel]]) 305 | enc_vel_update_flag = True 306 | 307 | u = self.pwm[side] 308 | x = self.enc_state[side] 309 | P = self.enc_state_cov[side] 310 | 311 | A = np.array([[1.0, ts], 312 | [0.0, 0.0]]) 313 | B = np.array([[6.0]]) 314 | C = np.array([[0.0, 1.0]]) 315 | W = np.array([1.0, 1.0]) 316 | V = np.array([0.5]) 317 | 318 | (x_p, P_p) = utils.kalman(x, u, P, A, B, C, W, V, z) 319 | 320 | self.enc_state[side] = x_p 321 | self.enc_state_cov[side] = P_p 322 | 323 | self.enc_time[side] = t 324 | self.enc_vel[side] = np.asscalar(x_p[[1]]) 325 | 326 | return enc_pos_update_flag and enc_vel_update_flag 327 | -------------------------------------------------------------------------------- /xbots/quickbot_v2_config.py: -------------------------------------------------------------------------------- 1 | """ 2 | @brief QuickBot class configuration module 3 | 4 | 5 | _'_'_ IRfm 6 | ++++++++++++++ 7 | _/ + + \_ 8 | IRfl _/ + + \_ IRfr 9 | / + LMP RMP + \ 10 | + __| |__ + 11 | + | | + 12 | + | | + 13 | _|++++ | _ _ | ++++|_ 14 | IRbl _|++++_| T T T T |_++++|_ IRbr 15 | |++++ | | | | ++++| 16 | ++++ Ol Or ++++ 17 | + + 18 | ++++++++++++++++++ 19 | 20 | @author Rowland O'Flaherty (rowlandoflaherty.com) 21 | @date 05/30/2014 22 | @version: 2.0 23 | @copyright: Copyright (C) 2014, see the LICENSE file 24 | """ 25 | 26 | class EncoderSerial(object): #pylint: disable=R0903 27 | """ 28 | Stores in encoder serial parameters. 29 | """ 30 | def __init__(self, port, baudrate): 31 | self.port = port 32 | self.baudrate = baudrate 33 | 34 | class MotorPin(object): #pylint: disable=R0903 35 | """ 36 | Stores motor pin parameters. 37 | """ 38 | def __init__(self, dir1, dir2, pwm): 39 | self.dir1 = dir1 40 | self.dir2 = dir2 41 | self.pwm = pwm 42 | 43 | # IR sensor Pins 44 | IR_BL = "P9_40" # IR back left pin 45 | IR_FL = "P9_38" # IR front left pin 46 | IR_FM = "P9_36" # IR front middle pin 47 | IR_FR = "P9_35" # IR front right pin 48 | IR_BR = "P9_33" # IR back right pin 49 | IR = (IR_BL, IR_FL, IR_FM, IR_FR, IR_BR) # IR pin set 50 | 51 | # Encoder (aka odometry) serials 52 | # TTYO1: Rx=P9_26 Tx=P9_24 53 | # TTYO2: Rx=P9_22 Tx=P9_21 54 | # TTYO4: Rx=P9_11 Tx=P9_13 55 | # TTYO5: Rx=P9_38 Tx=P9_38 56 | EN_L = EncoderSerial('/dev/ttyO1', 38400) # Encoder left serial 57 | EN_R = EncoderSerial('/dev/ttyO2', 38400) # Encoder right serial 58 | 59 | # Motor pins 60 | MOTOR_L = MotorPin("P8_14", "P8_16", "P9_16") 61 | MOTOR_R = MotorPin("P8_12", "P8_10", "P9_14") 62 | 63 | # LED pin 64 | LED = "USR1" 65 | -------------------------------------------------------------------------------- /xbots/ultrabot.py: -------------------------------------------------------------------------------- 1 | """ 2 | @brief UltraBot class for Beaglebone Black 3 | 4 | @author Josip Delic (delijati.net) 5 | @date 04/23/2014 6 | @version: 0.1 7 | @copyright: Copyright (C) 2014, see the LICENSE file 8 | """ 9 | 10 | from __future__ import division 11 | import threading 12 | import time 13 | import base 14 | import math 15 | 16 | import ultrabot_config as config 17 | import Adafruit_BBIO.GPIO as GPIO 18 | 19 | # trigger duration 20 | DECPULSETRIGGER = 0.0001 21 | # loop iterations before timeout called 22 | INTTIMEOUT = 2100 23 | MAX_CENTIMETER = 350 24 | 25 | # encoder 26 | WHEEL_RADIUS = 1.2 # cm 27 | TIME_INTERVAL = 1.0 # seconds 28 | TICKS = 20 # holes in disc 29 | CONST = (2 * math.pi * WHEEL_RADIUS)/TICKS 30 | 31 | # globals 32 | ENC_VEL = [0.0, 0.0] 33 | ENC_POS = [0.0, 0.0] 34 | 35 | 36 | class UltraBot(base.BaseBot): 37 | """The UltraBot Class""" 38 | # Motor Pins -- (LEFT, RIGHT) 39 | dir1Pin = (config.INl1, config.INr1) 40 | dir2Pin = (config.INl2, config.INr2) 41 | pwmPin = (config.PWMl, config.PWMr) 42 | pwm_freq = 100 43 | 44 | # LED pin 45 | led = config.LED 46 | 47 | # Constraints 48 | pwmLimits = [-100, 100] # [min, max] 49 | 50 | # State PWM -- (LEFT, RIGHT) 51 | pwm = [0, 0] 52 | 53 | # State Ultras 54 | ultraVal = [0.0, 0.0, 0.0, 0.0, 0.0] 55 | 56 | # State ir 57 | irVal = [0, 0, 0, 0, 0] 58 | 59 | # State Encoder 60 | encPos = [0.0, 0.0] # Last encoder tick position 61 | encVel = [0.0, 0.0] # Last encoder tick velocity 62 | 63 | def __init__(self, baseIP, robotIP): 64 | super(UltraBot, self).__init__(baseIP, robotIP) 65 | # init encoder 66 | self.encoderRead = EncoderReader() 67 | # init ultras 68 | self._setup_ultras() 69 | 70 | def update(self): 71 | self.read_ultras() 72 | self.read_encoders() 73 | self.parseCmdBuffer() 74 | 75 | def _setup_ultras(self): 76 | for trigger, echo in config.ULTRAS: 77 | GPIO.setup(echo, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) 78 | GPIO.setup(trigger, GPIO.OUT) 79 | GPIO.output(trigger, False) 80 | 81 | def _measure_ultra(self, trigger, echo): 82 | GPIO.output(trigger, True) 83 | time.sleep(DECPULSETRIGGER) 84 | GPIO.output(trigger, False) 85 | 86 | # Wait for echo to go high (or timeout) 87 | intcountdown = INTTIMEOUT 88 | 89 | while (GPIO.input(echo) == 0 and intcountdown > 0): 90 | intcountdown = intcountdown - 1 91 | 92 | # If echo is high 93 | if intcountdown > 0: 94 | 95 | # Start timer and init timeout countdown 96 | echostart = time.time() 97 | intcountdown = INTTIMEOUT 98 | 99 | # Wait for echo to go low (or timeout) 100 | while (GPIO.input(echo) == 1 and intcountdown > 0): 101 | intcountdown = intcountdown - 1 102 | 103 | # Stop timer 104 | echoend = time.time() 105 | 106 | # Echo duration 107 | echoduration = echoend - echostart 108 | 109 | # Display distance 110 | if intcountdown > 0: 111 | intdistance = (echoduration*1000000)/58 112 | #print "Distance = " + str(intdistance) + "cm" 113 | return intdistance 114 | 115 | def read_ultras(self): 116 | for idx, (trigger, echo) in enumerate(config.ULTRAS): 117 | prevVal = self.ultraVal[idx] 118 | distance = self._measure_ultra(trigger, echo) 119 | self.ultraVal[idx] = distance 120 | 121 | if self.ultraVal[idx] >= MAX_CENTIMETER or distance is None: 122 | self.ultraVal[idx] = prevVal 123 | 124 | def read_encoders(self): 125 | self.encPos = ENC_POS # New tick count 126 | self.encVel = ENC_VEL # New tick velocity 127 | 128 | 129 | class EncoderReader(threading.Thread): 130 | """EncoderReader thread""" 131 | 132 | counter_l = 0 133 | counter_r = 0 134 | 135 | def __init__(self, encPin=(config.Ol, config.Or)): 136 | GPIO.setup(config.Ol, GPIO.IN) 137 | GPIO.setup(config.Or, GPIO.IN) 138 | 139 | # Initialize thread 140 | threading.Thread.__init__(self) 141 | 142 | def update_encoder_l(self, channel): 143 | global ENC_POS 144 | self.counter_l = self.counter_l + 1 145 | ENC_POS[base.LEFT] = self.counter_l 146 | #print "Encoder (left) counter updated: %d" % self.counter_l 147 | 148 | def update_encoder_r(self, channel): 149 | global ENC_POS 150 | self.counter_r = self.counter_r + 1 151 | ENC_POS[base.RIGHT] = self.counter_l 152 | #print "Encoder (right) counter updated: %d" % self.counter_r 153 | 154 | def run(self): 155 | GPIO.add_event_detect(config.Ol, GPIO.RISING, 156 | callback=self.update_encoder_l) 157 | GPIO.add_event_detect(config.Or, GPIO.RISING, 158 | callback=self.update_encoder_r) 159 | 160 | current_time_l = time.time() 161 | current_time_r = time.time() 162 | while base.RUN_FLAG: 163 | if (time.time() >= current_time_l + TIME_INTERVAL): 164 | global ENC_VEL 165 | velocity_l = ( 166 | self.counter_l * (WHEEL_RADIUS * CONST) 167 | ) / TIME_INTERVAL 168 | ENC_VEL[base.LEFT] = velocity_l 169 | 170 | self.counter_l = 0 171 | current_time_l = time.time() 172 | #print "velocity_l %s cm/s" % velocity_l 173 | if (time.time() >= current_time_r + TIME_INTERVAL): 174 | global ENC_VEL 175 | velocity_r = ( 176 | self.counter_r * (WHEEL_RADIUS * CONST) 177 | ) / TIME_INTERVAL 178 | ENC_VEL[base.RIGHT] = velocity_r 179 | 180 | self.counter_r = 0 181 | current_time_r = time.time() 182 | #print "velocity_r %s cm/s" % velocity_r 183 | -------------------------------------------------------------------------------- /xbots/ultrabot_config.py: -------------------------------------------------------------------------------- 1 | # ultrabot 2 | # 3 | # _'_'_ UXfm 4 | # ++++++++++++++ 5 | # _/ + + \_ 6 | # UXfl _/ + + \_ UXfr 7 | # / + LMP RMP + \ 8 | # + __| |__ + 9 | # + | | + 10 | # + | | + 11 | # _|++++ | _ _ | ++++|_ 12 | # UXbl _|++++_| T T T T |_++++|_ UXbr 13 | # |++++ | | | | ++++| 14 | # ++++ Ol Or ++++ 15 | # + + 16 | # ++++++++++++++++++ 17 | # 18 | # ultrasonic 19 | UTbl = "P8_12" 20 | UEbl = "P8_11" 21 | 22 | UTfl = "P9_21" 23 | UEfl = "P9_22" 24 | 25 | UTfm = "P9_23" 26 | UEfm = "P9_24" 27 | 28 | UTfr = "P9_25" 29 | UEfr = "P9_26" 30 | 31 | UTbr = "P9_27" 32 | UEbr = "P9_30" 33 | 34 | ULTRAS = ((UTbl, UEbl), (UTfl, UEfl), (UTfm, UEfm), (UTfr, UEfr), (UTbr, UEbr)) 35 | 36 | # encoder aka odometry 37 | Ol = "P9_41" 38 | Or = "P9_42" 39 | 40 | # motors 41 | INl1 = "P9_11" 42 | INl2 = "P9_12" 43 | PWMl = "P9_14" 44 | 45 | INr1 = "P9_13" 46 | INr2 = "P9_15" 47 | PWMr = "P9_16" 48 | 49 | RMP = (INr1, INr2, PWMr) 50 | LMP = (INl1, INl2, PWMl) 51 | 52 | # led 53 | LED = "USR1" 54 | -------------------------------------------------------------------------------- /xbots/utils.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | 4 | # Tic toc constants 5 | TICTOC_START = 0 6 | TICTOC_COUNT = 0 7 | TICTOC_MEAN = 0 8 | TICTOC_MAX = -float('inf') 9 | TICTOC_MIN = float('inf') 10 | 11 | 12 | def convertHEXtoDEC(hexString, N): 13 | """ 14 | Return 2's compliment of hexString 15 | """ 16 | for hexChar in hexString: 17 | asciiNum = ord(hexChar) 18 | if not ((asciiNum >= 48 and asciiNum <= 57) or 19 | (asciiNum >= 65 and asciiNum <= 70) or 20 | (asciiNum >= 97 and asciiNum <= 102)): 21 | val = float('nan') 22 | return val 23 | 24 | if len(hexString) == N: 25 | val = int(hexString, 16) 26 | bits = 4*len(hexString) 27 | if (val & (1 << (bits-1))) != 0: 28 | val = val - (1 << bits) 29 | return val 30 | 31 | 32 | def operatingPoint(uStar, uStarThreshold): 33 | """ 34 | This function returns the steady state tick velocity given some PWM input. 35 | 36 | uStar: PWM input. 37 | uStarThreshold: Threshold on the minimum magnitude of a PWM input value 38 | 39 | returns: omegaStar - steady state tick velocity 40 | """ 41 | # Matlab code to find beta values 42 | # X = [40; 50; 60]; % Air Test 43 | # Y = [180; 305; 400]; % Tick per second 44 | # 45 | # r = 0.0325; % Wheel radius 46 | # c = 2*pi*r; 47 | # X = [ 70; 70; 70; 75; 75; 75; 80; 80; 80; 85; 85; 48 | # 85; 90; 90; 90]; % Ground Test 49 | # Z = [4.25; 3.95; 4.23; 3.67; 3.53; 3.48; 3.19; 3.08; 2.93; 2.52; 2.59; 50 | # 2.56; 1.99; 2.02; 2.04]; % Time to go 1 m 51 | # Y = 1./(Z*c); 52 | # H = [X ones(size(X))]; 53 | # beta = H \ Y 54 | beta = [11.0, -255.0] # Air Test Results 55 | # beta = [0.0606, -3.1475] # Ground Test Results 56 | 57 | if np.abs(uStar) <= uStarThreshold: 58 | omegaStar = 0.0 59 | elif uStar > 0: 60 | omegaStar = beta[0] * uStar + beta[1] 61 | else: 62 | omegaStar = -1.0 * (beta[0] * np.abs(uStar) + beta[1]) 63 | 64 | return omegaStar 65 | 66 | 67 | def kalman(x, u, P, A, B, C, W, V, z=np.NaN): 68 | """ 69 | This function returns an optimal expected value of the state and covariance 70 | error matrix given an update and system parameters. 71 | 72 | x: Estimate of state at time t-1. 73 | u: Input at time t-1. 74 | P: Estimate of error covariance matrix at time t-1. 75 | A: Discrete time state tranistion matrix at time t-1. 76 | B: Input to state model matrix at time t-1. 77 | C: Observation model matrix at time t. 78 | W: Process noise covariance at time t-1. 79 | V: Measurement noise covariance at time t. 80 | z: Measurement at time t. 81 | 82 | returns: (x,P) tuple 83 | x: Updated estimate of state at time t. 84 | P: Updated estimate of error covariance matrix at time t. 85 | 86 | """ 87 | 88 | x = np.atleast_2d(x) 89 | u = np.atleast_2d(u) 90 | P = np.atleast_2d(P) 91 | A = np.atleast_2d(A) 92 | B = np.atleast_2d(B) 93 | x_p = np.dot(A, x) + np.dot(B, u) # Prediction of estimated state vector 94 | P_p = np.dot(A, np.dot(P, A.T)) + W # Prediction of error covariance matrix 95 | 96 | if np.any(np.isnan(z)): 97 | return (x_p, P_p) 98 | else: 99 | C = np.atleast_2d(C) 100 | W = np.atleast_2d(W) 101 | V = np.atleast_2d(V) 102 | z = np.atleast_2d(z) 103 | 104 | [M,N] = np.shape(C) 105 | 106 | if W.shape[0] == 1 or W.shape[1] == 1: 107 | W = np.diag(np.squeeze(W)) 108 | 109 | if (V.shape[0] == 1 or V.shape[1] == 1) and not (V.shape[0] == 1 and V.shape[1] == 1): 110 | V = np.diag(np.squeeze(V)) 111 | 112 | I = np.eye(N) # N x N identity matrix 113 | 114 | S = np.dot(C, np.dot(P_p, C.T)) + V # Sum of error variances 115 | S_inv = np.linalg.inv(S) # Inverse of sum of error variances 116 | K = np.dot(P_p, np.dot(C.T, S_inv)) # Kalman gain 117 | r = z - np.dot(C, x_p) # Prediction residual 118 | w = np.dot(-K, r) # Process error 119 | x = x_p - w # Update estimated state vector 120 | # v = z - np.dot(C, x) # Measurement error 121 | if np.any(np.isnan(np.dot(K, V))): 122 | P = P_p 123 | else: 124 | # Updated error covariance matrix 125 | P = np.dot((I - np.dot(K, C)), np.dot(P_p, (I - np.dot(K, C)).T)) + np.dot(K, np.dot(V, K.T)) 126 | return (x, P) 127 | 128 | 129 | def tic(): 130 | global TICTOC_START 131 | TICTOC_START = time.time() 132 | 133 | 134 | def toc(tictocName='toc', printFlag=True): 135 | global TICTOC_START 136 | global TICTOC_COUNT 137 | global TICTOC_MEAN 138 | global TICTOC_MAX 139 | global TICTOC_MIN 140 | 141 | tictocTime = time.time() - TICTOC_START 142 | TICTOC_COUNT = TICTOC_COUNT + 1 143 | TICTOC_MEAN = tictocTime / TICTOC_COUNT + \ 144 | TICTOC_MEAN * (TICTOC_COUNT - 1) / TICTOC_COUNT 145 | TICTOC_MAX = max(TICTOC_MAX, tictocTime) 146 | TICTOC_MIN = min(TICTOC_MIN, tictocTime) 147 | 148 | if printFlag: 149 | print tictocName + " time: " + str(tictocTime) 150 | 151 | 152 | def tictocPrint(): 153 | global TICTOC_COUNT 154 | global TICTOC_MEAN 155 | global TICTOC_MAX 156 | global TICTOC_MIN 157 | 158 | print "Tic Toc Stats:" 159 | print "Count = " + str(TICTOC_COUNT) 160 | print "Mean = " + str(TICTOC_MEAN) 161 | print "Max = " + str(TICTOC_MAX) 162 | print "Min = " + str(TICTOC_MIN) 163 | 164 | 165 | def recursiveMeanVar(x, l, mu, sigma2): 166 | """ 167 | This function calculates a new mean and variance given 168 | the current mean "mu", current variance "sigma2", current 169 | update count "l", and new samples "x" 170 | """ 171 | m = len(x) 172 | n = l + m 173 | muPlus = l / n * mu + m / n * np.mean(x) 174 | if n > 1: 175 | sigma2Plus = 1 / (n - 1) * ( 176 | (l - 1) * sigma2 + (m - 1) * np.var(x) + l * ( 177 | mu - muPlus) ** 2 + m * (np.mean(x) - muPlus) ** 2) 178 | else: 179 | sigma2Plus = 0 180 | 181 | return (muPlus, sigma2Plus, n) 182 | --------------------------------------------------------------------------------