added RoFish and adaptiveFreqOsc sketches
This commit is contained in:
parent
4ee18c83aa
commit
1f3d63ea1c
19
README.md
19
README.md
|
@ -6,9 +6,11 @@ This repository contains Arduino sketches and Arduino libraries maintained by th
|
||||||
|
|
||||||
## Sketches
|
## 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")
|
![ ](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.
|
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
|
## 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.
|
||||||
|
|
|
@ -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)
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
"""
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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)
|
|
@ -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)
|
Loading…
Reference in New Issue