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
|
||||
|
||||
- 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.
|
||||
|
|
|
@ -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