def eventDownChecker(event):
global Motor
if event.Ascii is 115:
Motor = motor.Motor(ShoulderKey)
print("Initialize Motor on Shoulder")
if event.Ascii is 101:
Motor = motor.Motor(ElbowKey)
print("Initialize Motor on Elbow")
if event.Ascii is 119:
Motor = motor.Motor(WristKey)
print("Initialize Motor on Wrist")
if event.Ascii is 103:
Motor = motor.Motor(GripKey)
print("Initialize Motor on Grip")
if event.Ascii is 98:
Motor = motor.Motor(BaseKey)
print("Initialize Motor on Base")
if event.Ascii is 108:
if Motor is None:
print("Please initialize Motor before control LED")
else:
Motor.turnFlash()
if event.ScanCode is typeOfMovement[default_For_Down_Right_Close]:
if Motor is None:
print("Please initialize Motor before control")
else:
Motor.turnClockWise()
if event.ScanCode is typeOfMovement[default_For_Up_Left_Open]:
if Motor is None:
print("Please initialize Motor before control")
else:
Motor.turnCounterClockWise()
if event.Ascii is typeOfMovement[default_For_Finish_Program]:
print("My mission is complated, I'm out")
new_hook.cancel()
def __init__(self, in1=13, in2=12, ena=6, in3=21, in4=20, enb=26):
self.motorL = motor.Motor(in1, in2, ena) #motor LEFT
self.motorR = motor.Motor(in3, in4, enb) #motor RIGHT
self.servoLR = servos.Servo() #servo LEFT RIGHT
self.servoUD = servos.Servo(chan=1, centerVal=1800)
self.servoUD.stop()
self.stop() #servo UP DOWN
def __init__(self):
#setup gpio
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(12, GPIO.OUT)
self.mcp = analog.Analog()
self.motor2 = motor.Motor(19, 20)
self.motor1 = motor.Motor(26, 21)
self.sonic1 = sonic.Sonic(18, 23)
self.sonic4 = sonic.Sonic(25, 24)
self.sonic2 = sonic.Sonic(16, 12)
self.sonic3 = sonic.Sonic(6, 13)
self.sonic5 = sonic.Sonic(22, 5)
self.sense = heading.Heading()
self.data_status = False
self.wpath = os.getcwd() + "/data/" + time.strftime("%H%M%S") + ".csv"
self.file = open(self.wpath, "w")
self.file.write("time, motor, pwm\n")
with open('data/saved_net.pickle', 'rb') as handle:
self.net = pickle.load(handle)
# self.sensor_collect = open("data/sensors.txt","w")
# self.sensor_collect.write("time, sonic1, sonic2, sonic3, sonic4, sonic5\n")
self.data = list()
self.collecting = False
#setup controller values
self.state = 0
#create xbox controller class
self.xboxCont = XboxController.XboxController(joystickNo=0,
deadzone=0.1,
scale=1,
invertYAxis=False)
#setup call backs
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.A,
self.aButtonCallBack)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.BACK,
self.backButton)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.LTHUMBY,
self.LthumbY)
self.xboxCont.setupControlCallback(self.xboxCont.XboxControls.RTHUMBY,
self.RthumbY)
#start the controller
self.xboxCont.start()
self.running = True
def __init__(self):
# Define motors
self.motor_rt = motor.Motor(RT_PWM, RT_IN1, RT_IN2)
self.motor_lt = motor.Motor(LT_PWM, LT_IN1, LT_IN2)
# Start the PWM signals
self.motor_rt.startPWM(DUTY_CYCLE_RT)
self.motor_lt.startPWM(DUTY_CYCLE_LT)
def __init__(self, lMotorPins, rMotorPins, motorDc, motorFreq, ir_pin):
"""Robot constructor."""
self.rMotor = motor.Motor(rMotorPins[0], rMotorPins[1], rMotorPins[2],
motorDc, motorFreq)
self.lMotor = motor.Motor(lMotorPins[0], lMotorPins[1], lMotorPins[2],
motorDc, motorFreq)
self.ir = irSensor.IrSensor(ir_pin)
self._status = {"status": "stopped", "direction": "none"}
def __init__(self, parent):
tk.Frame.__init__(self, parent)
self.car = motor.Car(motor.Motor(5, 6, 13), motor.Motor(23, 24, 18))
self.pack(side=tk.LEFT)
self.pin_frame_left = PinFrame(self, self.car.left)
self.slider_frame_left = SliderFrame(self, self.car.left)
self.buffer_frame = BufferFrame(self, 1, 100, tk.LEFT)
self.slider_frame_right = SliderFrame(self, self.car.right)
self.pin_frame_right = PinFrame(self, self.car.right)
self.car.enable(0, 0)
def __init__(self):
right_motor_forwards_pin = 10
right_motor_backwards_pin = 9
left_motor_forwards_pin = 7
left_motor_backwards_pin = 8
self.left_motor = motor.Motor(left_motor_forwards_pin,
left_motor_backwards_pin, 'left')
self.right_motor = motor.Motor(right_motor_forwards_pin,
right_motor_backwards_pin, 'right')
self.distance_detector = distance_measurement.DistanceDetector()
def __init__(self, motor_port1, motor_port2, debug=False):
if debug:
import motor_base
self._motor1 = motor_base.MotorBase(2)
self._motor2 = motor_base.MotorBase(6)
else:
import motor
self._motor1 = motor.Motor(2)
self._motor2 = motor.Motor(6)
self._action = Queue.Queue()
self._run = True
threading.Thread.__init__(self)
def __init__(self): self.zone = 0 self.motors = [ motor.Motor(0,0), motor.Motor(1,0)] def Timer_exit(round_length): while True: time.sleep(round_length) print "END OF ROUND, NOW EXITING CODE." thread.interrupt_main() if self.mode == "comp": thread.start_new_thread(Timer_exit,(round_length))
def main():
camera = picamera.PiCamera()
camera.resolution = (600, 600)
#for the moment,we do 50 trials
#for loop is fast
for i in range(50):
camera.capture('test1.png') #take picture
image1 = cv2.imread('./test1.png')
image = cv2.GaussianBlur(image1, (5, 5), 0) #delete white noise
mono_src = RedDetect.red_detect(image)
# ラベリング処理
label = cv2.connectedComponentsWithStats(
mono_src) #make array to get imformation
'''connectedComponentsWithStats
0:the number of image , 1:image with labels, 2:center'''
if label:
# オブジェクト情報を項目別に抽出
n = label[0] - 1
data = np.delete(label[2], 0, 0)
center = np.delete(label[3], 0, 0)
'''delete is to delete black labels 0 is black flame, so first square is number 1,second square is number 2
0 is not needed, so I want to delete black flame of number 0
'''
s = Qlearning.discretization(center, data, n)
print(s)
#配列の最も大きい値のkeyかlabelを返す関数
max_surface_area = np.argmax(s)
#get upfunc ,そこにモータの動きをさせる
motor.Motor(max_surface_area)
else:
print("0")
continue
def __init__(self,
deadBand=0.02,
maxOutput=1,
negateLeft=False,
negateRight=False):
self.lMotor = motor.Motor(constants.lMotor)
self.rMotor = motor.Motor(constants.rMotor)
self.negateLeft = negateLeft
self.negateRight = negateRight
self.rSpeed = 0
self.lSpeed = 0
self.maxOutput = 1
self.deadBand = deadBand
def robot_init():
#set GPIO numbering mode and define output pins
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
global robot
robot = mv.Motor()
def __init__(self):
self.FLI=motor.Motor(motor.Motor.motor_A)#FrontLightInside
self.FLO=motor.Motor(motor.Motor.motor_B)#FrontLightOutside
self.BLI=motor.Motor(motor.Motor.motor_D)
self.BLO=motor.Motor(motor.Motor.motor_C)
self.FRI=motor.Motor(motor.Motor.motor_1)
self.FRO=motor.Motor(motor.Motor.motor_2)
self.BRI=motor.Motor(motor.Motor.motor_4)
self.BRO=motor.Motor(motor.Motor.motor_3)
def main():
global robot
robot = mv.Motor()
time.sleep(3)
# robot.motor_triage('Up/FF')
while True:
robot.adxl345.roll_car(robot)
def _pvLoad(d):
for pv, file in d.items():
if pv == "_name":
self.name = file
else:
preset = MotorPresets(mot.Motor(pv), loadFile=file)
self.add(preset)
didLoad = True
def main():
global robot
robot = mv.Motor()
time.sleep(3)
robot.motor_triage('Up/FF')
while True:
robot.sonic.ultrasonic(robot)
def __init__(self):
# Distance between the wheels
self.l = L
# Wheel radius
self.r = R
self.x = 0
self.y = 0
self.theta = 0
self.x_goal = 0
self.y_goal = 0
self.theta_goal = 0
self.m1 = motor.Motor()
self.m2 = motor.Motor()
self.acc = 0
self.speed_acc = 0
self.mode = 1
def __init__(self):
self.motor = motor.Motor()
self.gyro = gyroscope.Gyroscope()
def unwrap(self, previousAngle, currentAngle):
if abs(previousAngle - currentAngle) > 180:
while currentAngle > previousAngle:
currentAngle -= 360
while currentAngle < previousAngle:
currentAngle += 360
return currentAngle
def __init__(self):
self.rate = 100 #[Hz]
rospy.Subscriber("/joint_commands", Float32MultiArray, self.angle_cb)
rospy.Subscriber("/calib_joint_commands", Float32MultiArray, self.calib_angle_cb)
rospy.Subscriber("/calibration_offsets", Float32MultiArray, self.calibration_cb)
self.joint_angles_pub = rospy.Publisher("/joint_angles", Float32MultiArray, queue_size=10)
self.joint_currents_pub = rospy.Publisher("/joint_currents", Float32MultiArray, queue_size=10)
print "Initializing motors"
odrv0 = odrive.find_any(serial_number = "208637853548")
odrv1 = odrive.find_any(serial_number = "208737993548")
self.motor0 = motor.Motor(odrv0,0)
self.motor1 = motor.Motor(odrv1,0)
self.motor2 = motor.Motor(odrv1,1)
print "Finished Initializing motors"
rospy.Timer(rospy.Duration(1./self.rate), self.publish)
def main():
global robot
robot = mv.Motor()
sv_status = 0
sv_direction = 1
sv_panning = True
while True:
sv_status, sv_direction, sv_panning = robot.servo.servo_pan(
sv_status, sv_direction, sv_panning)
print('count:', sv_status)
print('Direction: ', sv_direction)
def take_z_stack(
name, step, n
): #Take a Z-Stack-specify file name, step increments, number of images
cam = camera.get_camera()
m = motor.Motor()
i = 0
while i <= n * step:
filename = name + str(i) + '.png'
cam.capture(filename)
m.steps(step)
i += step
def main():
global A, b
maxon_motor = motor.Motor(resistance=1.03,
torque_constant=0.0335,
speed_constant=0.0335,
rotor_inertia=135 * 1e-3 * (1e-2**2))
gearbox = motor.Gearbox(gear_ratio=20.0 / 60.0, gear_inertia=0)
gear_motor = motor.GearMotor(maxon_motor, gearbox)
robot = Robot(
robot_mass=6.5,
robot_radius=0.085,
gear_motor=gear_motor,
robot_inertia=6.5 * 0.085 * 0.085 * 0.5,
wheel_radius=0.029,
wheel_inertia=2.4e-5,
# wheel_angles=np.deg2rad([60, 129, -129, -60]))
wheel_angles=np.deg2rad([45, 135, -135, -45]))
# csv = pandas.read_csv('/Users/tdogb/robocup-analysis/model-python/robot_data.csv', delimiter=",").to_numpy()
# for row in csv:
# accels = np.asmatrix(row[0:3]).T
# vels = np.asmatrix(row[3:6]).T
# volts = np.asmatrix(row[6:10]).T
# # accels = robot.forward_dynamics_body(vels, volts)
# sysID(volts[0,0], volts[1,0], volts[2,0], volts[3,0], vels[0,0], vels[1,0], vels[2,0], accels[0,0], accels[1,0], accels[2,0])
lstsq_solution = np.linalg.lstsq(A, b)
print("-----")
print(lstsq_solution[1])
print("-----")
# b_ss = np.vstack((np.vstack((lstsq_solution[0][0:4,0].T, lstsq_solution[0][10:14,0].T)), lstsq_solution[0][20:24,0].T))
# A_ss = np.vstack((np.vstack((lstsq_solution[0][4:9,0].T, lstsq_solution[0][14:19,0].T)), lstsq_solution[0][24:29,0].T))
b_ss = np.matrix([[
-lstsq_solution[0][0, 0], -lstsq_solution[0][0, 0],
lstsq_solution[0][0, 0], lstsq_solution[0][0, 0]
],
[
lstsq_solution[0][7, 0], -lstsq_solution[0][7, 0],
-lstsq_solution[0][7, 0], lstsq_solution[0][7, 0]
],
[
lstsq_solution[0][14, 0], lstsq_solution[0][14, 0],
lstsq_solution[0][14, 0], lstsq_solution[0][14, 0]
]])
A_ss = np.vstack((np.vstack(
(lstsq_solution[0][1:6, 0].T, lstsq_solution[0][8:13, 0].T)),
lstsq_solution[0][15:20, 0].T))
print(A.shape)
print(A_ss)
print(b_ss)
def setUp(self):
RPi.GPIO.input.return_value = RPi.GPIO.HIGH
hwc = controller.GPIOController
pins = hwc.PIN
self.lid = lid.Lid(pins.PUSH_BUTTON)
hwc.add_component(self.lid)
self.assertEqual(self.lid.status, self.lid.CLOSED)
self.led_yellow = led.LED(pins.YELLOW_LED, led.LED.ON)
hwc.add_component(self.led_yellow)
RPi.GPIO.output.assert_called_with(self.led_yellow.pin, RPi.GPIO.HIGH)
self.led_blue = led.LED(pins.BLUE_LED, led.LED.OFF)
hwc.add_component(self.led_blue)
RPi.GPIO.output.assert_called_with(self.led_blue.pin, RPi.GPIO.LOW)
hwc.add_component(motor.Motor(*pins.MOTOR, enable_timer=False))
def paint(location, scan_location):
loc_x = scan_location[0] - location[
0] - 50 #location[0] # Target Robot Location X
loc_y = scan_location[1] + location[
1] + 70 #location[1] # Target Robot Location Y
loc_z = scan_location[2] - location[
2] + 300 #location[2] # Target Robot Location Z
# A connecting piece, to which our end effector attached, was mounted 30 degree clockwise rotated to the robot
loc_a = 30. # Target Robot Location A,
# We want to paint with the robot only facing vertically down
loc_b = 90. # Target Robot Location B
loc_c = 0. # Target Robot Location C
"""
Send Serial to robot to move to location
# Send "int float float float float float float"
# Send "COMMAND(1=move) POS_X POS_Y POS_Z POS_A POS_B POS_C"
Get Serial from robot when it moved to the location
# Receive "KUKA Moving"
# Receive "Finish"
Apply Paint
Check to refill
"""
# move syringe at paint location
message0 = "%i %f %f %f %f %f %f" % (int(1), loc_x, loc_y, loc_z, loc_a,
loc_b, loc_c)
ser.write(message0)
wait = True
while (wait):
message_from_serial = ser.readline()
wait = 'FinishPaint' not in message_from_serial
time.sleep(2) # To avoid problems caused by transmitting delay
# Move gear to apply paint
motor.GPIO_init()
print("gear start moving")
stepper = motor.Motor([6, 13, 19, 26])
time.sleep(5) # To avoid problems caused by transmitting delay
stepper.step(1000, False)
print("gear moved")
return
def __init__(self, mqtt_rasp_url):
self.motor = motor.Motor()
self.motor_dropper = motor_dropper.MotorDropper()
self.motor_dropper.drop()
self.camera = camera.Camera()
self.supersonic = supersonic.Supersonic()
self.shared_topics = ['smartbowl/bowl-state']
# Configure mqtt clients
self.rasp_mqtt = mqtt.Client()
url_rasp = urlparse(mqtt_rasp_url)
# Connect
self.rasp_mqtt.connect(url_rasp.hostname, url_rasp.port)
self.register_callbacks()
for topic in self.shared_topics:
self.rasp_mqtt.subscribe(topic)
self.start()
def robot_init(self):
print 'Robot Init'
pwm.enable()
os.system('./map')
#print 'Claw Init'
#self.__claw = claw.Claw()
#self.__claw.attach("P9_14")
#print 'Proximity Sensor Init'
#self.__prox = proximity.Proximity()
#self.__prox.attach("38", "P9_42", "48")
print 'Motor Init'
self.__motor = motor.Motor()
self.__motor.attach("P9_29", "P9_31", "49", "117", "115", "60")
print 'UDP Init'
self.__udp = udp.PhoneStream()
self.__udp.attach()
def main():
showUsage()
bike = motor.Motor()
control_motor_thread = threading.Thread(target=controlMotor,
args=(bike, ),
name='controlThread')
show_info_thread = threading.Thread(target=simulateMotor,
args=(bike, ),
name='showThread')
data_send_thread = threading.Thread(target=dataSend.sending_data,
args=(bike, ),
name='dataSendingThread')
control_motor_thread.start()
show_info_thread.start()
data_send_thread.start()
time.sleep(1)
data_send_thread.join()
control_motor_thread.join()
show_info_thread.join()
import serial
import micropyGPS
import threading
import time
import motor
gps = micropyGPS.MicropyGPS(9, 'dd') # MicroGPSオブジェクトを生成する。
# 引数はタイムゾーンの時差と出力フォーマット
motor = motor.Motor(18, 25, 24, 13, 17, 27)
def rungps(): # GPSモジュールを読み、GPSオブジェクトを更新する
s = serial.Serial('/dev/serial0', 9600, timeout=10)
s.readline() # 最初の1行は中途半端なデーターが読めることがあるので、捨てる
while True:
sentence = s.readline().decode('utf-8') # GPSデーターを読み、文字列に変換する
if sentence[0] != '$': # 先頭が'$'でなければ捨てる
continue
for x in sentence: # 読んだ文字列を解析してGPSオブジェクトにデーターを追加、更新する
gps.update(x)
def get_gps():
if gps.clean_sentences > 20:
return (gps.latitude[0], gps.longitude[0])
else:
return None
def init():
gpsthread = threading.Thread(target=rungps, args=()) # 上の関数を実行するスレッドを生成
curTimeStep = timeStep
frameRate = worldParams['frameRate']
movie = worldParams['movie']
plots = worldParams['plots']
endtime = worldParams['endTime']
time = [0]
#initialize foot, leg, and robot
Foot0 = foot.Foot(np.zeros(3), np.zeros((3, 2)), footParams, worldParams,
robotParams['mass'])
Foot1 = foot.Foot(np.zeros(3), np.zeros((3, 2)), footParams, worldParams,
robotParams['mass'])
Leg0 = leg.Leg(initLegAngle0, initLegPos0, legParams, worldParams, Foot0)
Leg1 = leg.Leg(initLegAngle1, initLegPos1, legParams, worldParams, Foot1)
Motor0 = motor.Motor(motorParams, initLegAngle0[0])
Motor1 = motor.Motor(motorParams, initLegAngle1[0])
Robot = robot.Robot(robotParams, worldParams, (Leg0, Leg1), (Motor0, Motor1))
#set up lists to store data
robotForces = [[], []]
robotTorque = [[], []]
robotpos = [[], []]
robotspeed = [[], []]
robotaccel = [[], []]
ftpos = [[], []]
ftspeed = [[], []]
ftaccel = [[], []]
footForce = [[], []]
footAngle = [[], []]
legPts = [np.inf, np.inf]
import os
import serial
import sys
import motor # this is the motor library
global motorSer
# enter the position EleksMaker is mounted
motorSer = "/dev/ttyUSB0"
if __name__ == "__main__":
# define the motor class
mot = motor.Motor(motorSer)
# open the serial and test connection
mot.motorOpen()
# set current motor position as orginal point
mot.setOrigin()
# set the step length to 2cm
mot.redefineStep(2)
# let the stage move to coordinate (4,4).
# The program will inform when the movement is finished
mot.takeCor(4, 4)
# return the current position of the stage
mot.returnPos()
