res[cfg[0]] = cfg[1]
return res
### ARDUINO
board = Arduino('COM7')
pins = [board.get_pin('d:{0}:s'.format(i)) for i in range(2, 10)]
def move_servo(pin, angle):
print 'Writing {0} to pin {1}'.format(angle, pin)
pins[pin - 2].write(angle)
mute_angles = all_mute_angles()
for pin in mute_angles:
board.servo_config(pin, mute_angles[pin])
move_servo(pin, mute_angles[pin])
### MIDI
midi.init()
inp = midi.Input(1)
def play_note(note):
pin, mute_angle, play_angle = pins_by_note(note)
if pin is None:
print "Error: No pin for note {0}".format(note)
return
move_servo(pin, play_angle)
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
#from std_msgs.msg import UInt16
from std_msgs.msg import Int32
import time
from pyfirmata import Arduino
on_hardware = True # whether we are running this node on the actual car (so it can access the IO board)
wheelpin = 5 # to which pin on the IO sheld the whee pin got connected
drivepin = 3 # to which pin on the IO shield the drive cable got connected
if on_hardware == True: # running on hardware -- we need to set the board connection
board = Arduino('/dev/ttyACM99', baudrate=57600)
board.servo_config(wheelpin, min_pulse=1, max_pulse=100,
angle=90) # set initial direction to straight forward
board.servo_config(drivepin, min_pulse=1, max_pulse=20,
angle=90) # set initial speed to natural
speed_natural = 90
speed_current_angle = speed_natural # this variable will carry the actual speed at any time and will be used to determine direction of change (in case of decay or full stop)
speed_min_angle_reverse = 75 # this is the angle below which the car start moving in reverse
speed_min_angle_forward = 107 # this is the angle above which the car start moving forward
speed_max_angle_reverse = 65 # maximum angle allowed in reverse (which is actually a minimum mathematically, as the angle goes 0-90)
speed_max_angle_forward = 115 # maximum angle allowed in forward
speed_decay_angle = 2 # how much we decrease the angle when there is a decay request
speed_change_angle = 1 # when we receive a request to change the speed, this is the angle change we will do
speed_change_angle_decrease = 2 # this is the speed slowdown (breaking)
speed_direction_change_delay = 2 # in sec - delay enforced between changing direction (forward-backward)
last_stop_timestamp = 0.0 # the last time we have reached the zero speed from a non-zero speed (used with the speed_direction_change_delay)
direction_natural = 90 # this is the natural (straight ahead) position of the wheel in angles
Lmotor1 = board.get_pin('d:5:p')
Lmotor2 = board.get_pin('d:6:p')
#registr the rmotor
Rmotor1 = board.get_pin('d:10:p')
Rmotor2 = board.get_pin('d:11:p')
#pointer for current image capture
imageNumber = 0
#pin out constants for the rpi
shutdownBtn = 10
inputBtn = 12
#configure the mast servo motor
mastServo = board.servo_config(3)
#setup the GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
#Register pins
GPIO.setup(shutdownBtn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(inputBtn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(shutdownBtn, GPIO.RISING, callback=shutdown)
#function to shutdown the rpi
def shutdown():
GPIO.cleanup()
call("sudo shutdown now", shell=True)
'model': 'cfg/yolo.cfg',
'load': 'bin/yolov2.weights',
'threshold': 0.3,
'gpu': 0.6
}
#constant declareation
height = 2.15 #stage to LIGHT distance
frameWidth = 2.2 #stage width
height = height * 1280 / frameWidth #normalized height
#Arduino Initialization
board = Arduino('COM5')
ypin = board.get_pin('d:9:s')
xpin = board.get_pin('d:10:s')
board.servo_config(9, 544, 2400, 0)
board.servo_config(10, 544, 2400, 0)
xpin.write(0)
ypin.write(0)
#tensorflow code to detect and track
tfnet = TFNet(options)
colors = [tuple(255 * np.random.rand(3)) for _ in range(10)]
capture = cv2.VideoCapture(0)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1280)
posx = 0
posy = 0
time.sleep(5)
class firmata_servo(OpenRTM_aist.DataFlowComponentBase):
##
# @brief constructor
# @param manager Maneger Object
#
def __init__(self, manager):
OpenRTM_aist.DataFlowComponentBase.__init__(self, manager)
self._d_servo_data = RTC.TimedVector2D(RTC.Time(0,0),0)
"""
"""
self._servo_dataIn = OpenRTM_aist.InPort("servo_data", self._d_servo_data)
# initialize of configuration-data.
# <rtc-template block="init_conf_param">
"""
- Name: com_port
- DefaultValue: /dev/tty.USB0
"""
self._com_port = ['/dev/tty.USB0']
# </rtc-template>
##
#
# The initialize action (on CREATED->ALIVE transition)
# formaer rtc_init_entry()
#
# @return RTC::ReturnCode_t
#
#
def onInitialize(self):
# Bind variables and configuration variable
self.bindParameter("com_port", self._com_port, "/dev/tty.USB0")
# Set InPort buffers
self.addInPort("servo_data",self._servo_dataIn)
# Set OutPort buffers
# Set service provider to Ports
# Set service consumers to Ports
# Set CORBA Service Ports
return RTC.RTC_OK
# ##
# #
# # The finalize action (on ALIVE->END transition)
# # formaer rtc_exiting_entry()
# #
# # @return RTC::ReturnCode_t
#
# #
#def onFinalize(self):
#
# return RTC.RTC_OK
# ##
# #
# # The startup action when ExecutionContext startup
# # former rtc_starting_entry()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
def onStartup(self, ec_id):
return RTC.RTC_OK
# ##
# #
# # The shutdown action when ExecutionContext stop
# # former rtc_stopping_entry()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
#def onShutdown(self, ec_id):
#
# return RTC.RTC_OK
# ##
# #
# # The activated action (Active state entry action)
# # former rtc_active_entry()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
def onActivated(self, ec_id):
print "onActivated Begin"
pinNumber = [3, 5, 6, 7, 11]
self.pin = [None, None, None, None, None]
try:
self.board = Arduino(self._com_port[0])
for i in range(5):
self.board.servo_config(pinNumber[i], angle=90)
# Caution: Don't use board.get_pin('d:*:s') as it calls servo_config method with angle=0, which damages your servo.
self.pin[i] = self.board.digital[pinNumber[i]]
print "onActivated End"
except Exception as e:
print "some errors %s" % str(e)
self.pin[0].write(80)
return RTC.RTC_OK
# ##
# #
# # The deactivated action (Active state exit action)
# # former rtc_active_exit()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
#def onDeactivated(self, ec_id):
#
# return RTC.RTC_OK
# ##
# #
# # The execution action that is invoked periodically
# # former rtc_active_do()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
def onExecute(self, ec_id):
if self._servo_dataIn.isNew():
print "onExecute New"
servoData = self._servo_dataIn.read()
servoId = int(servoData.data.x)
angle = int(servoData.data.y)
print "%d %d" % (servoId, angle)
if servoId < len(self.pin) and angle <= 180 and angle >= 0:
self.pin[servoId].write(angle)
return RTC.RTC_OK
# ##
# #
# # The aborting action when main logic error occurred.
# # former rtc_aborting_entry()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
#def onAborting(self, ec_id):
#
# return RTC.RTC_OK
# ##
# #
# # The error action in ERROR state
# # former rtc_error_do()
# #
# # @param ec_id target ExecutionContext Id
# #
# # @return RTC::ReturnCode_t
# #
# #
def onError(self, ec_id):
print "error"
return RTC.RTC_OK
class MultiServo:
def __init__(self):
self.ardu1 = Arduino('COM9')
self.ardu2 = Arduino('COM3')
self.pinlist = np.array([3, 5, 6, 9, 10, 11])
self.servoset = np.array([[600, 2300], [500, 2380]])
self.logdata = []
self.usedservo = np.zeros((2, 6), dtype=np.uint8)
self.delaysec = 0.75
self.ispacked = False
time.sleep(2)
def command(self, pin, servotype, angle, ardu):
if (isinstance(pin, int)) and (isinstance(servotype, int)) and (isinstance(angle, int)) and \
(isinstance(ardu, int)) and (0 <= pin <= 5) and (0 <= servotype <= 1) and (1 <= ardu <= 2):
if ardu == 1:
self.ardu1.servo_config(self.pinlist[pin],
self.servoset[servotype, 0],
self.servoset[servotype, 1], angle)
elif ardu == 2:
self.ardu2.servo_config(self.pinlist[pin],
self.servoset[servotype, 0],
self.servoset[servotype, 1], angle)
else:
print('Wrong Command')
def run_command(self, cmd):
# Run command, with delay
if cmd[0] == 6:
sec = cmd[2]
time.sleep(sec)
print('Delay ' + str(sec) + ' sec')
if self.ispacked:
if sec > self.delaysec:
self.usedservo = np.zeros((2, 6), dtype=np.uint8)
else:
pinnow = int(cmd[0])
ardunow = int(cmd[3])
if self.ispacked:
if self.usedservo[ardunow - 1, pinnow] == 1:
time.sleep(self.delaysec)
self.usedservo = np.zeros((2, 6), dtype=np.uint8)
self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow)
else:
self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow)
self.usedservo[ardunow - 1, pinnow] = 1
else:
self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow)
def run_data(self, data):
# Run packed data
self.ispacked = True
for cmd in data:
self.run_command(cmd)
self.usedservo = np.zeros((2, 6), dtype=np.uint8)
self.ispacked = False
print('Moveset Ended')
def get_data(self, file):
try:
print(file)
data = np.genfromtxt(file, delimiter=',', defaultfmt='%d')
except Exception as ex:
print('Error Occured')
print(ex)
# Temp data-1 sec delay
data = [6, 0, 1, 0]
# print(data)
self.run_data(data)
def save_data(self, inittime):
# PIN TYPE ANGLE ARDU
# PIN 6 then ANGLE is DELAY
# print(logdata)
np.savetxt('MoveSet_' + inittime.strftime('%y%m%d_%H%M%S.csv'),
self.logdata,
fmt='%d',
delimiter=',')
def log_data(self, cmd):
# cmd format:
# PIN TYPE ANGLE ARDU
self.logdata.append(cmd)
### Values for motor direction
FORWARD = 1
BACKWARD = 2
BRAKE = None # These don't work.
RELEASE = None
clients = []
yun = Arduino('/dev/ttyATH0', baudrate=115200)
s = yun.get_shield()
m1 = s.get_motor(1)
m2 = s.get_motor(2)
m1.dir, m2.dir = FORWARD, FORWARD
m1.spd, m2.spd = 0,0
yun.servo_config(9);
yun.servo_config(10);
yun.digital[10].write(60); # Start it at a different angle, so it can move opposite the other servo
def butane_on():
yun.digital[9].write(60);
yun.digital[10].write(0);
def butane_off():
yun.digital[9].write(0);
yun.digital[10].write(60);
def retrieve(client,value):
client.send(value)
### Class to handle websocket connections
from common.servoserver import ServoServer
from pyfirmata import Arduino, util
port = '/dev/tty.usbmodem1411'
board = Arduino(port)
board.servo_config(9)
board.servo_config(10)
def callback(channels):
board.digital[9].write(channels[0] / 255.0 * 180)
board.digital[10].write(channels[1] / 255.0 * 180)
server = ServoServer(callback)
server.start()
