diff --git a/README.md b/README.md index d9d3d67..d03a304 100644 --- a/README.md +++ b/README.md @@ -6,9 +6,11 @@ This repository contains Arduino sketches and Arduino libraries maintained by th ## Sketches -- Blinky: A hello world example. +- **Blinky** +A hello world example used to play along with Git. -- Nexus_Omni4WD: Firmware for the Nexus Omni4WD chassis for setting the wheel speeds independently. It is used to control the robot from ROS. +- **Nexus_Omni4WD** +Firmware for the Nexus Omni4WD wheel base for setting the wheel speeds independently. It is used to control the robot with ROS. ![ ](figures/NexusOmni4WD.png "Nexus Omni 4WD") @@ -20,10 +22,19 @@ This folder contains the Arduino sketch of the firmware. The program works as a The Arduino controller 10009 with io expansion shield on the Omni4WD is Duemilanove/ATMEGA328 compatible. +*extra/nexuscontrol* +Script for controlling the Nexus with ROS. +- **RoFish** + a CPG-based locomotion control example of a robotic fish, implemented on the Arduino. + +- **adaptiveFreqOsc** +An Adaptive Frequency Oscillator example with the Arduino. ## Libraries -- NativeDDS: Arduino library for a software implemented Direct Digital Synthesizer. +- **NativeDDS**: +A library containing a software implemented Direct Digital Synthesizer. -- TrueRMS: Arduino library that calculates the average, rms and power of DC+AC input signals, measured with the ADC. +- **TrueRMS** +A library for calculating the average, rms and power of DC+AC input signals, measured with the ADC. diff --git a/extra/nexuscontrol/__init__.py b/extra/nexuscontrol/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/extra/nexuscontrol/carcomm.py b/extra/nexuscontrol/carcomm.py new file mode 100755 index 0000000..214103e --- /dev/null +++ b/extra/nexuscontrol/carcomm.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python2 + +# nog geen 3 omdat serial er nog niet bij zit. (nog steeds?) + +# To control Nexus robot car +# Via /dev/ttyUSB? +# + +import sys, os, serial, time, traceback +from struct import pack, unpack +import numpy as np +import re + +def key_description(character): + """generate a readable description for a key""" + ascii_code = ord(character) + if ascii_code < 32: + return 'Ctrl+%c' % (ord('@') + ascii_code) + else: + return repr(character) + +# voorlopig alleen posix +if os.name == 'posixnee': + import termios, sys, os + class Console: + def __init__(self): + self.fd = sys.stdin.fileno() + + def setup(self): + self.old = termios.tcgetattr(self.fd) + new = termios.tcgetattr(self.fd) + # new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG + new[3] = new[3] & ~termios.ICANON # & ~termios.ISIG + new[6][termios.VMIN] = 1 + new[6][termios.VTIME] = 0 + termios.tcsetattr(self.fd, termios.TCSANOW, new) + + # Uiteindelijk willen we meer tegelijk lezen.. + # zeker als er grotere hoeveelheden data verstuurd worden door het target + def getkey(self): + c = os.read(self.fd, 1) + return c + + def cleanup(self): + termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old) + + console = Console() + + def cleanup_console(): + console.cleanup() + + console.setup() + sys.exitfunc = cleanup_console #terminal modes have to be restored on exit... + +#else: +# raise NotImplementedError( "Sorry no implementation for your platform (%s) available." % sys.platform) + + +CONVERT_CRLF = 2 +CONVERT_CR = 1 +CONVERT_LF = 0 +NEWLINE_CONVERISON_MAP = ('\n', '\r', '\r\n') +LF_MODES = ('LF', 'CR', 'CR/LF') + +REPR_MODES = ('raw', 'some control', 'all control', 'hex') + +class CarControl: + def __init__(self, port, baudrate, parity, rtscts, xonxoff, echo=False, convert_outgoing=CONVERT_CRLF, repr_mode=0): + self.serial = serial.Serial(port, baudrate, parity=parity, rtscts=rtscts, xonxoff=xonxoff, timeout = 1) + self.echo = echo + self.repr_mode = repr_mode + self.convert_outgoing = convert_outgoing + self.newline = NEWLINE_CONVERISON_MAP[self.convert_outgoing] + self.dtr_state = True + self.rts_state = True + self.break_state = False + + # Het lijkt er op of er na een power up van de Nexus twee characters (waarde 0) gezonden worden. FTDI chip? + # De writer pas wat laten doen als we daar geen last meer van hebben. + tmp = self.serial.read(1) + #print 'pretmp', len(tmp) + #if len(tmp) > 0: + # print ord(tmp[0]) + time.sleep(1) + tmp = self.serial.read(1) + #print 'prettmp', len(tmp) + #if len(tmp) > 0: + # print ord(tmp[0]) + + def stop(self): + print 'Waar is de stop?' + self.serial.close() + + def getinfopacket(self): + try: + d = self.serial.read(24) + print len(d) #, ord(d[0]) + if ord(d[0]) <> 2 or ord(d[23]) <> 3: # STX and ETX + print 'Packet framing error!' + else: + print 'New info packet received.' + if ord(d[1]) == 1: # scope data packet (try that first) + status_control = ord(d[3]); + error_code = ord(d[4]); + status_car = ord(d[5]); + #(sensor,) = unpack('h', d[10:12]); + (speed,) = unpack('h', d[6:8]); + (debtime,) = unpack('h', d[10:12]); + # we gebruiken dit in de write thread en bij autoreply. + # evt. korte lock bij uitpakken data + waitreq = 1 + else: + print 'Packet type %x received, not implemented.' % d[1] + print 'Info packet received with error code %x %x %d and %d' % (error_code, status_car, speed, debtime) + except: + print 'Wanneer hier?' + traceback.print_exc(file=sys.stdout) + + + """ + Packets are 8 bytes. + { stx, command, par's ..., etx } + parameters dependant upon the command + + Nexus robot functions of type void(*function[11])(unsigned int speedMMPS) : + {goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop, backOff, upperLeft, lowerLeft, upperRight, lowerRight}; + { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } + this wil go in command. + Parameter speedMMPS in par + + -700 < speedMMPS < 700 + speedMMPS = 200 komt overeen met 0.21 m/sec + maximale snelheid 0.735 m/sec + van m/s naar speedMMPS : maal 952 + + + Other commands: + 12: setWheelspeedcommand (set speed of 4 wheels) + 13: set speedMMPS on Nexus + 20: ask info packet + a 24 byte packet will be received + 21: autoshutdown off (default) + 22: autoshutdown on + 23: autoreply off (default) + 24: autoreply on. reply with a infopacket + 25: start standalone? + 26: stop standalone + 27: keep alive + 28: repeat mode info packets off (default) + 29: repeat mode info packete on + + """ + + def sendpacket(self, command, par): + outpkt = bytearray(16) + outpkt[0] = 2 # STX + outpkt[1] = command + outpkt[2:4] = pack('h', par) + outpkt[15] = 3 # ETX + self.serial.write(outpkt) # send packet + print 'sendpacket', outpkt[0], outpkt[1], outpkt[2], outpkt[3], outpkt[4], outpkt[5], outpkt[6], ' ... ', outpkt[15] + + def sendwheelscommand(self, frontleft, backleft, frontright, backright): + outpkt = bytearray(16) + outpkt[0] = 2 # STX + outpkt[1] = 12 # setwheels command + outpkt[2:4] = pack('h', frontleft) + outpkt[4:6] = pack('h', backleft) + outpkt[6:8] = pack('h', frontright) + outpkt[8:10] = pack('h', backright) + + outpkt[15] = 3 # ETX + self.serial.write(outpkt) # send packet + #print 'outpkt: ', unpack('h', outpkt[2:4]), unpack('h', outpkt[4:6]), unpack('h', outpkt[6:8]), unpack('h',outpkt[8:10]) + +# def setspeed(self, lin,rot): + # # lineaire (m/sec) and rotatie (rad/sec) snelheid + # # convert to nexus (calibrated with nexus1) + # speed = lin * 1024 + # rot = rot * 300 + # self.sendwheelscommand(speed -rot, speed - rot, speed + rot , speed + rot) + + def setspeed(self, linx, liny, rot): + # lineaire x and y (m/sec) and rotatie (rad/sec) snelheid + # convert to nexus (calibrated with nexus1) + speedx = linx * -1024 + speedy = liny * -1024 + rot = rot * 300 + self.sendwheelscommand(speedx -speedy -rot, speedx +speedy -rot, speedx -speedy +rot, speedx +speedy +rot) + + diff --git a/extra/nexuscontrol/nexus_controller.py b/extra/nexuscontrol/nexus_controller.py new file mode 100644 index 0000000..eb9908a --- /dev/null +++ b/extra/nexuscontrol/nexus_controller.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python + +""" + Nexus controller + + +""" + +import rospy +import socket +from math import radians,sin,cos,pi +from std_msgs.msg import Float64 + +from diagnostic_msgs.msg import * +from geometry_msgs.msg import Quaternion +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from tf.broadcaster import TransformBroadcaster + +import carcomm + +class NexusController(): + + def __init__(self, device, name): + + self.status = "OK" + self.level = DiagnosticStatus.OK + # self.name = socket.gethostname() + + self.dirty = False # newly updated position? + self.enabled = True # can we take commands? + + self.last_cmd = rospy.Time.now() + self.rate = rospy.get_param("~nexus/rate", 50.0) + self.timeout = rospy.get_param('~nexus/timeout',1.0) + self.t_delta = rospy.Duration(1.0/self.rate) + self.t_next = rospy.Time.now() + self.t_delta + self.fake = rospy.get_param("~sim", False) + port = rospy.get_param("~port", "/dev/ttyUSB0") + + # parameters? +# self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', ''+self.name+'/base_footprint') + self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', name+'base_footprint') + self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom') + + self.x = 0.0 # position in xy plane + self.y = 0.0 + self.th = 0 # orientation (theta) + + self.dx = 0 # x-speed in meter/sec + self.dy = 0 # y-speed in meter/sec + self.dr = 0 # rotation in radians/sec + self.dx_last = 0 # previous x-speed + self.dy_last = 0 # previous y-speed + self.dr_last = 0 # previous rotation + self.then = rospy.Time.now() # time for determining dx/dy + + # ROS interfaces + rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) + self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5) + self.odomBroadcaster = TransformBroadcaster() + + rospy.loginfo("Started Controller ("+name+")") + + # connect to the nexus robot + if not self.fake: + # connect to Nexus + baudrate = 28800; parity = 'N'; rtscts = False; xonxoff = False; echo = False + convert_outgoing = carcomm.CONVERT_CRLF; repr_mode = 0 + self.carcon = carcomm.CarControl(port, baudrate, parity, rtscts, xonxoff, echo, convert_outgoing, repr_mode ) + # to be sure that the car is ready + rospy.sleep(2.0) + rospy.loginfo("Started Nexus connection on port " + port + ".") + + self.carcon.sendpacket(20,0) + self.carcon.getinfopacket() + + else: + rospy.loginfo("Nexus being simulated.") + + + def startup(self): + print "startup called" + + def shutdown(self): + # close connection with the nexus + if not self.fake: + console.cleanup_console() + carcon.stop() + print "shutdown called" + + def cmdVelCb(self,req): + """ Handle movement requests. """ + self.last_cmd = rospy.Time.now() + if self.fake: + self.dx = req.linear.x # m/s + self.dy = req.linear.y # m/s + self.dr = req.angular.z # rad/s + else: + # same as fake + self.dx = req.linear.x # m/s + self.dy = req.linear.y # m/s + self.dr = req.angular.z # rad/s + + def getDiagnostics(self): + """ Get a diagnostics status. """ + msg = DiagnosticStatus() + msg.name = self.name + return msg + + def update(self): + """ Do step in nexus controller. """ + now = rospy.Time.now() + if now > self.t_next: + elapsed = now - self.then + self.then = now + elapsed = elapsed.to_sec() + + if self.fake: + self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed + self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed + self.th += self.dr*elapsed + else: + # we use read reackoning for the moment + self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed + self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed + self.th += self.dr*elapsed + + + # publish or perish + quaternion = Quaternion() + quaternion.x = 0.0 + quaternion.y = 0.0 + quaternion.z = sin(self.th/2) + quaternion.w = cos(self.th/2) + self.odomBroadcaster.sendTransform( + (self.x, self.y, 0), + (quaternion.x, quaternion.y, quaternion.z, quaternion.w), + rospy.Time.now(), + self.base_frame_id, + self.odom_frame_id + ) + + odom = Odometry() + odom.header.stamp = now + odom.header.frame_id = self.odom_frame_id + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0 + odom.pose.pose.orientation = quaternion + odom.child_frame_id = self.base_frame_id + odom.twist.twist.linear.x = self.dx + odom.twist.twist.linear.y = self.dy + odom.twist.twist.angular.z = self.dr + self.odomPub.publish(odom) + + # stop on timeout. nu niet zinnig, er is toch geen terugkoppeling. +# if now > (self.last_cmd + rospy.Duration(self.timeout)): + # self.dx = 0; self.dr = 0; + # print "timeout reached" + + # update motors + if not self.fake: + # only send if it really is different! + if self.dx <> self.dx_last or self.dy <> self.dy_last or self.dr <> self.dr_last: +# voor of achteruit +# self.carcon.setspeed(-self.dx, self.dy, self.dr) + self.carcon.setspeed(self.dx, self.dy, self.dr) + self.dx_last = self.dx; self.dy_last = self.dy; self.dr_last = self.dr + + self.t_next = now + self.t_delta + + + + def getDiagnostics(self): + """ Update status of servos (voltages, temperatures). """ + print "diagnostics" + """ + if self.iter % 5 == 0 and not self.fake: + if self.device.use_sync_read: + # arbotix/servostik/wifi board sync_read + synclist = list() + for joint in self.dynamixels: + if joint.readable: + synclist.append(joint.id) + if len(synclist) > 0: + val = self.device.syncRead(synclist, P_PRESENT_VOLTAGE, 2) + if val: + for joint in self.dynamixels: + try: + i = synclist.index(joint.id)*2 + if val[i] < 250: + joint.voltage = val[i]/10.0 + if val[i+1] < 100: + joint.temperature = val[i+1] + except: + # not a readable servo + continue + else: + # direct connection, or other hardware with no sync_read capability + # hack to also use slave_id + for joint in self.dynamixels: + if joint.readable: + val = self.device.read(joint.id, P_PRESENT_VOLTAGE, 2) + vals = val + if joint.slave_id > 0: + vals = self.device.read(joint.slave_id, P_PRESENT_VOLTAGE, 2) + try: + joint.voltage = val[0] + # use maximum + if val[1] > vals[1]: + joint.temperature = val[1] + else: + joint.temperature = vals[1] + except: + continue + self.iter += 1 + return None + """ + diff --git a/extra/nexuscontrol/publishers.py b/extra/nexuscontrol/publishers.py new file mode 100644 index 0000000..68953a0 --- /dev/null +++ b/extra/nexuscontrol/publishers.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python + +""" + diagnostics.py - diagnostic output code + Copyright (c) 2011 Vanadium Labs LLC. All right reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Vanadium Labs LLC nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +""" + +import rospy +from diagnostic_msgs.msg import DiagnosticArray +from sensor_msgs.msg import JointState + +class DiagnosticsPublisher: + """ Class to handle publications of joint_states message. """ + + def __init__(self): + self.t_delta = rospy.Duration(1.0/rospy.get_param("~diagnostic_rate", 1.0)) + self.t_next = rospy.Time.now() + self.t_delta + self.pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=5) + + def update(self, joints, controllers): + """ Publish diagnostics. """ + now = rospy.Time.now() + if now > self.t_next: + # create message + msg = DiagnosticArray() + msg.header.stamp = now + for controller in controllers: + d = controller.getDiagnostics() + if d: + msg.status.append(d) + for joint in joints: + d = joint.getDiagnostics() + if d: + msg.status.append(d) + # publish and update stats + self.pub.publish(msg) + self.t_next = now + self.t_delta + + +class JointStatePublisher: + """ Class to handle publications of joint_states message. """ + + def __init__(self): + # parameters: throttle rate and geometry + self.rate = rospy.get_param("~read_rate", 10.0) + self.t_delta = rospy.Duration(1.0/self.rate) + self.t_next = rospy.Time.now() + self.t_delta + + # subscriber + self.pub = rospy.Publisher('joint_states', JointState, queue_size=5) + + def update(self, joints, controllers): + """ publish joint states. """ + if rospy.Time.now() > self.t_next: + msg = JointState() + msg.header.stamp = rospy.Time.now() + msg.name = list() + msg.position = list() + msg.velocity = list() + for joint in joints: + msg.name.append(joint.name) + msg.position.append(joint.position) + msg.velocity.append(joint.velocity) + for controller in controllers: + msg.name += controller.joint_names + msg.position += controller.joint_positions + msg.velocity += controller.joint_velocities + self.pub.publish(msg) + self.t_next = rospy.Time.now() + self.t_delta + diff --git a/extra/nexuscontrol/testcarcom.py b/extra/nexuscontrol/testcarcom.py new file mode 100755 index 0000000..08e378e --- /dev/null +++ b/extra/nexuscontrol/testcarcom.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +import sys, time +import carcomm + +if __name__ == "__main__": + + try: + # voorlopig vast + port = "/dev/ttyUSB0" + baudrate = 28800 + parity = 'N' + rtscts = False + xonxoff = False + echo = False + convert_outgoing = carcomm.CONVERT_CRLF + repr_mode = 0 + + carcon = carcomm.CarControl( + port, + baudrate, + parity, + rtscts, + xonxoff, + echo, + convert_outgoing, + repr_mode, + ) + time.sleep(2) # de nexus robot is niet zo snel.. + carcon.sendpacket(20,0) + carcon.getinfopacket() + + for x in range(0, 200): + carcon.setspeed( 2.5, 0.0) +# carcon.getinfopacket() + time.sleep(0.05) + carcon.setspeed( 2.5, 0.0) +# carcon.getinfopacket() + time.sleep(0.05) + carcon.setspeed( 0, 0) + + except carcomm.serial.SerialException as e: + sys.stderr.write("could not open port %r: %s\n" % (port, e)) + sys.exit(1) + + + + + diff --git a/sketches/RoFish/README.md b/sketches/RoFish/README.md new file mode 100644 index 0000000..eb0f3e7 --- /dev/null +++ b/sketches/RoFish/README.md @@ -0,0 +1,7 @@ +# Robotic Fish + +A CPG-based locomotion control example of a robotic fish, implemented on the Arduino. + +Please visit: + +[https://github.com/MartinStokroos/RoFish](https://github.com/MartinStokroos/RoFish) diff --git a/sketches/adaptiveFreqOsc/README.md b/sketches/adaptiveFreqOsc/README.md new file mode 100644 index 0000000..42fcac8 --- /dev/null +++ b/sketches/adaptiveFreqOsc/README.md @@ -0,0 +1,7 @@ +# Adaptive Frequency Oscillator + +An Adaptive Frequency Oscillator demo for Arduino. + +Please visit: + +[https://github.com/MartinStokroos/adaptiveFreqOsc](https://github.com/MartinStokroos/adaptiveFreqOsc)