def turnRight(degrees):
global rightMotor
global leftMotor
rightMotor.run_direct(duty_cycle_sp=-37)
leftMotor.run_direct(duty_cycle_sp=37)
initial = ev3.GyroSensor().value()
while ev3.GyroSensor().value() > initial - degrees:
_sleep(0.001)
rightMotor.reset()
leftMotor.reset()
def main():
initialization()
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
gy = ev3.GyroSensor(ev3.INPUT_3)
us = ev3.UltrasonicSensor(ev3.INPUT_4)
print('Ready')
Machine = Robot(200, 100, 0, 0)
#Machine.rotate_angle(-10, left_motor, right_motor, gy, us, a)
while (1):
# Listen to mqtt
command = input()
command = command.strip().split()
command = list(map(int, command))
if command[0] == 0:
x = command[1]
#print(x)
Machine.rotate_angle(x, left_motor, right_motor, gy, us, a)
else:
y = command[1]
#print (y)
Machine.drive_sm(y, left_motor, right_motor, gy, us, a)
distance = us.value()
self.angle = gy.value()
a["data"] = [str(distance), str(self.angle)]
print(a["data"][0] + ' ' + a["data"][1])
time.sleep(0.2)
def setup_hardware(self):
# setup I/O connecting to EV3
self.motorR = ev3.LargeMotor('outA')
self.motorL = ev3.LargeMotor('outD')
self.csR = ev3.ColorSensor('in2')
self.csM = ev3.ColorSensor('in3')
self.csL = ev3.ColorSensor('in4')
# mode set up (right and left senor only used for light intensity)
self.csR.reflected_light_intensity
self.csL.reflected_light_intensity
self.port = ev3.LegoPort('outB')
assert self.port.connected
self.port.mode = 'dc-motor'
time.sleep(5)
self.grabberArms = ev3.DcMotor("outB")
print("DcMotor connected")
self.grabberLift = ev3.MediumMotor('outC')
self.grabberLift.stop_action = 'brake'
self.grabber_initialise()
self.gy = ev3.GyroSensor('in1')
self.reset_gyro()
self.up_pos = self.grabberLift.position
def init_sensors():
""" Instantiates and calibrates both gyro sensor and motors
:return: Gyro and EV3Motors instances
"""
# Create EV3 resource objects
gyroSensor = ev3.GyroSensor()
gyroSensor.mode = gyroSensor.MODE_GYRO_RATE
motorLeft = ev3.LargeMotor('outC')
motorRight = ev3.LargeMotor('outB')
# Open sensor and motor files
gyroSensorValueRaw = open(gyroSensor._path + "/value0", "rb")
# Reset the motors
motorLeft.reset()
motorRight.reset()
motorLeft.run_direct()
motorRight.run_direct()
motorEncoderLeft = open(motorLeft._path + "/position", "rb")
motorEncoderRight = open(motorRight._path + "/position", "rb")
return gyroSensorValueRaw, motorEncoderLeft, motorEncoderRight
def __init__(self):
# note gyro angle is NOT representitive of robot's real angle
self.gyro = gyro = ev3.GyroSensor('in1')
self.gyro.mode = "GYRO-ANG"
self.motors = setupMotors()
self.start_arm_pos = self.motors[2].position
self.sonars = setupSonars()
def setupSensorsMotors(self, configs):
"""Takes in a dictionary where the key is a string that identifies a motor or sensor
and the value is the port for that motor or sensor. It sets up all the specified motors
and sensors accordingly."""
for item in configs:
port = configs[item]
if item == self.LEFT_MOTOR: #"outC"
self.leftMotor = ev3.LargeMotor(port)
elif item == self.RIGHT_MOTOR: #"outB"
self.rightMotor = ev3.LargeMotor(port)
elif item == self.SERVO_MOTOR: #"outD"
self.servoMotor = ev3.MediumMotor(port)
elif item == self.LEFT_TOUCH: #"in4"
self.leftTouch = ev3.TouchSensor(port)
elif item == self.RIGHT_TOUCH: #"in1"
self.rightTouch = ev3.TouchSensor(port)
elif item == self.ULTRA_SENSOR: #"in3"
self.ultraSensor = ev3.UltrasonicSensor(port)
elif item == self.COLOR_SENSOR: #"in2"
self.colorSensor = ev3.ColorSensor(port)
elif item == self.GYRO_SENSOR: #not connected
self.gyroSensor = ev3.GyroSensor(port)
elif item == self.PIXY:
self.pixy = ev3.Sensor(port)
else:
print("Unknown configuration item:", item)
def GyroTest():
motor0 = ev3.LargeMotor('outB'); assert motor0.connected
motor1 = ev3.LargeMotor('outC'); assert motor1.connected
g=ev3.GyroSensor()
g.connected
g.mode='GYRO-ANG'
units=g.units
print(units)
while True:
start_value = g.value()
print("start value is: ",start_value)
motor0.run_timed(time_sp=3000, speed_sp=50)
motor1.run_timed(time_sp=3000, speed_sp=-50)
sleep(5)
end_value = g.value()
print("end value is: ",end_value)
sleep(5)
start_value = g.value()
print("start value is: ",start_value)
motor0.run_timed(time_sp=3000, speed_sp=-50)
motor1.run_timed(time_sp=3000, speed_sp=50)
sleep(5)
end_value = g.value()
print("end value is: ",end_value)
def gyroReading():
print("gyro ------------------------")
# define motors
motorl =ev3.LargeMotor('outA')
motorl.connected
motorr =ev3.LargeMotor('outD')
motorr.connected
# gryo reading
g = ev3.GyroSensor(ev3.INPUT_2)
g.connected
g.mode = 'GYRO-ANG'
#what to put here--------
while not btn.backspace:
print g.value()
# run_time takes milliseconds
motorl.run_timed(duty_cycle_sp=50, time_sp=4000)
motorr.run_timed(duty_cycle_sp=50, time_sp=4000)
print("gyro done ------------------------")
time.sleep(1)
def __init__(self):
"""
Initialize the ev3dev robot
"""
right_motor = ev3.LargeMotor('outA')
logging.info("motor right connected: %s" % str(right_motor.connected))
left_motor = ev3.LargeMotor('outB')
logging.info("motor left connected: %s" % str(right_motor.connected))
right_motor.reset()
left_motor.reset()
gyro_sensor = ev3.GyroSensor()
logging.info("gyro sensor connected: %s" % str(gyro_sensor.connected))
gyro_sensor.mode = 'GYRO-ANG'
gyro_sensor.mode = 'GYRO-G&A'
self.gyro_sensor = gyro_sensor
self.motors = [left_motor, right_motor]
self.right_motor = right_motor
self.left_motor = left_motor
self.speed = self.DEFAULT_SPEED
def initialization():
global left_motor, right_motor, gy, us
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
gy = ev3.GyroSensor(ev3.INPUT_3)
us = ev3.UltrasonicSensor(ev3.INPUT_4)
assert left_motor.connected
assert right_motor.connected
assert gy.connected
assert us.connected
# left_motor.stop_action = ev3.Motor.STOP_ACTION_BRAKE
# right_motor.stop_action = ev3.Motor.STOP_ACTION_BRAKE
left_motor.stop_action = ev3.Motor.STOP_ACTION_HOLD
right_motor.stop_action = ev3.Motor.STOP_ACTION_HOLD
us.mode = 'US-DIST-CM' #put the US in the dist in sm mode
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG' #put the gyro into angule
while (not (gy.value() == 0)):
pass
gy.mode = 'GYRO-ANG'
def Sensors(one=None, two=None, three=None, four=None):
sensors = []
for i, v in enumerate([one, two, three, four]):
if not v:
sensors.append(None)
continue
sensor_port_name = 'in%d' % (i + 1)
v = v.lower()
if v == 'ir' or v.startswith('infra'):
sensors.append(ev3.InfraredSensor(sensor_port_name))
elif v == 'touch':
sensors.append(ev3.TouchSensor(sensor_port_name))
elif v == 'us' or v.startswith('ultra'):
sensors.append(ev3.UltrasonicSensor(sensor_port_name))
sensors[-1].mode = sensors[-1].MODE_US_DIST_CM
elif v == 'color':
sensors.append(ev3.ColorSensor(sensor_port_name))
sensors[-1].mode = sensors[-1].MODE_RGB_RAW
elif 'gyro' in v:
sensors.append(ev3.GyroSensor(sensor_port_name))
sensors[-1].mode = 'GYRO-ANG'
else:
raise ValueError('Not implemented:' % v)
return sensors
def __init__(self, initial_angle):
self.angle = initial_angle
# note gyro angle is NOT representitive of robot's real angle
self.gyro = gyro = ev3.GyroSensor('in1')
self.gyro.mode = "GYRO-ANG"
self.motors = setupMotors()
self.time_per_square = 650
self.start_arm_pos = self.motors[2].position
def __init__(self):
self.__self.__leftWheelWheel = ev3.LargeMotor('outB')
self.__self.__rightWheelWheel = ev3.LargeMotor('outD')
self.__gyro = ev3.GyroSensor('in2') # measure the difference in angle when turning
self.__color = ev3.ColorSensor('in1')
__color.mode = 'COL-COLOR' # put color sensor in COL-COLOR mode that can
# only identify 7 types of color
__gyro.mode = 'GYRO-ANG'
def Gyro_only(self):
g=ev3.GyroSensor()
g.connected
g.mode='GYRO-ANG'
units=g.units
while True:
read_start = g.value()
print str(read_start)
def followLines():
#Setup sensors and motors
color_sensor = ev3.ColorSensor(ev3.INPUT_4)
color_sensor.connected
color_sensor.mode = 'COL-REFLECT'
R_motor = ev3.LargeMotor('outC')
R_motor.connected
L_motor = ev3.LargeMotor('outB')
L_motor.connected
gyro_sensor = ev3.GyroSensor(ev3.INPUT_2)
gyro_sensor.connected
gyro_sensor.mode = 'GYRO-ANG'
line_count = 0
# The value which color sensor gives if its above the edge of the line
# going towards 80 means getting to white area and towards 0 to black
temp_color_value = color_sensor.value()
last_error = 0
#Calibration: Turn slightly to the right. If the sensor value increases, then the white space is to the
#right of the robot. If the value decreases, then the white space is to the left of the robot.
L_motor.run_timed(duty_cycle_sp=50, time_sp=100)
time.sleep(.5)
calibration_value = color_sensor.value()
print(str(calibration_value))
L_motor.run_timed(duty_cycle_sp=-50, time_sp=100)
time.sleep(.5)
L_motor.run_timed(duty_cycle_sp=-50, time_sp=100)
time.sleep(.5)
opposite_value = color_sensor.value()
print(str(opposite_value))
L_motor.run_timed(duty_cycle_sp=50, time_sp=100)
time.sleep(.5)
white_on_right = calibration_value > temp_color_value
#Call helper function
#IMPORTANT: Notice that we switch the motor parameters based on white_on_right.
#This way, all the logic stays the same; all that changes is the direction in which the robot must turn
if white_on_right:
print('White on my right')
white = calibration_value
black = opposite_value
midpoint = (white - black) / 2 + black
print("midpoint: " + str(midpoint))
traverse(midpoint, last_error, color_sensor, temp_color_value, L_motor,
R_motor, gyro_sensor, white)
else:
print('White on my left')
white = opposite_value
black = calibration_value
midpoint = (white - black) / 2 + black
print("midpoint: " + str(midpoint))
traverse(midpoint, last_error, color_sensor, temp_color_value, R_motor,
L_motor, gyro_sensor, white, line_count)
def __init__(self,
left_port,
right_port,
kinematics,
max_speed=700,
flip_dir=False):
self.left_motor = ev3.LargeMotor(left_port)
self.right_motor = ev3.LargeMotor(right_port)
self.sound = ev3.Sound()
self.kinematics = kinematics
try:
self.sound.beep()
self.gyro = ev3.GyroSensor()
self.gyro.mode = 'GYRO-CAL'
time.sleep(2)
self.gyro.mode = 'GYRO-ANG'
time.sleep(2)
self.sound.beep()
except:
self.gyro = None
print("Gyro not found")
try:
self.colorSensor = ev3.ColorSensor('in2')
self.colorSensor.mode = 'COL-REFLECT'
except:
self.colorSensor = None
print("Color sensor not found")
try:
self.left_push_sensor = ev3.TouchSensor('in3')
except:
self.left_push_sensor = None
print("Left push sensor not found.")
self.frontColorSensor = None
try:
self.right_push_sensor = ev3.TouchSensor('in1')
except:
self.right_push_sensor = None
print("Right push sensor not found.")
try:
self.ultrasonicSensor = ev3.UltrasonicSensor()
self.ultrasonicSensor.mode = 'US-DIST-CM'
except:
self.ultrasonicSensor = None
print("Ultrasonic sensor not found")
self.max_speed = max_speed
self.flip_dir = flip_dir
self.log = open("sensor.log", "w+")
def setup_hardware(self):
# setup I/O connecting to EV3
self.motorR = ev3.LargeMotor('outA')
self.motorL = ev3.LargeMotor('outD')
#self.cs = ev3.ColorSensor('in2')
self.csR = ev3.ColorSensor('in2')
self.csM = ev3.ColorSensor('in3')
self.csL = ev3.ColorSensor('in4')
#self.us = ev3.UltrasonicSensor('in1')
self.gy = ev3.GyroSensor('in3')
def __init__(self):
self.motR = ev3.LargeMotor('outA')
self.motL = ev3.LargeMotor('outD')
self.colorSensor = ev3.ColorSensor()
self.g = ev3.GyroSensor()
self.btn = ev3.Button()
self.colorSensor.mode = 'COL-REFLECT'
self.g.mode = 'GYRO-ANG'
self.Tp = 30
self.startAngle = self.g.value()
self.endAngle = self.g.value()
def __init__(self, m1, m2, ar):
self.m1 = ev3.LargeMotor(m1)
self.m2 = ev3.LargeMotor(m2)
self.ar = ev3.MediumMotor(ar)
self.cl = ev3.ColorSensor()
self.cl.mode = 'RGB-RAW'
self.gy = ev3.GyroSensor()
self.gy.mode = 'GYRO-CAL'
self.gy.mode = 'GYRO-ANG'
self.us = ev3.UltrasonicSensor()
self.us.mode = 'US-DIST-CM'
def __init__(self, rightMortor, leftMortor):
self.right_motor = rightMortor
self.left_motor = leftMortor
self.gyro_sensor = ev3.GyroSensor('in4')
self.battery = ev3.PowerSupply()
self._is_loop = True
# 最新取得値
self.gyro_sensor_rate = 0
self.left_motor_position = 0
self.right_motor_position = 0
self.battery_voltage = 0 #mV
def initialization(gy, us):
us = ev3.UltrasonicSensor('in4')
us.mode='US-DIST-CM' #put the US in the dist in sm mode
gy = ev3.GyroSensor('in3')
gy.mode = 'GYRO-ANG' #put the gyro into angle mode
assert gy.connected
assert us.connected
while (not(gy.value() = 0)):
pass
gy.mode = 'GYRO-ANG'
def __init__(self):
logging.debug("Setting up...")
# motors
right_motor = ev3.LargeMotor('outA')
logging.info("motor right connected: %s" % str(right_motor.connected))
left_motor = ev3.LargeMotor('outB')
logging.info("motor left connected: %s" % str(right_motor.connected))
right_motor.reset()
left_motor.reset()
self.motors = [left_motor, right_motor]
self.right_motor = right_motor
self.left_motor = left_motor
gyro_sensor = ev3.GyroSensor()
logging.info("gyro sensor connected: %s" % str(gyro_sensor.connected))
gyro_sensor.mode = 'GYRO-ANG'
self.gyro_sensor = gyro_sensor
try:
color_sensor = ev3.ColorSensor()
logging.info("color sensor connected: %s" %
str(color_sensor.connected))
color_sensor.mode = 'COL-REFLECT'
self.color_sensor = color_sensor
except Exception as e:
logging.exception("message")
try:
ultrasonic_sensor = ev3.UltrasonicSensor()
logging.info("ultrasonic sensor connected: %s" %
str(ultrasonic_sensor.connected))
ultrasonic_sensor.mode = 'US-DIST-CM'
self.ultrasonic_sensor = ultrasonic_sensor
except Exception as e:
logging.exception("message")
try:
ir_sensor = ev3.InfraredSensor()
logging.info("ir sensor connected: %s" % str(ir_sensor.connected))
ir_sensor.mode = 'IR-REMOTE'
self.ir_sensor = ir_sensor
except Exception as e:
logging.exception("message")
def main(): gy = ev3.GyroSensor(ev3.INPUT_3) us = ev3.UltrasonicSensor() initialization(gy, us) Analsys = Sensors(0, 0) while (True): angle = Analsys.read_angle(gy) #Analsys Sensors.read_accurate_angle() distance = Analsys.read_distance(us) print (str(distance)) time.sleep(0.5)
def main():
initialization()
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
gy = ev3.GyroSensor(ev3.INPUT_3)
us = ev3.UltrasonicSensor(ev3.INPUT_4)
Analsys = Sensors(gy, us)
Machine = Robot(400, 200, 40, 90)
for k in range(4):
Machine.drive_sm(40, left_motor, right_motor)
time.sleep(0.01)
Machine.rotate_angle(90, left_motor, right_motor)
time.sleep(0.01)
def turn_left(self):
g=ev3.GyroSensor()
g.connected
g.mode='GYRO-ANG'
units=g.units
read_start = g.value()
motor0 = ev3.LargeMotor('outB'); assert motor0.connected
motor1 = ev3.LargeMotor('outC'); assert motor1.connected
read_start = motor0.position
motor0.run_timed(time_sp=3000, speed_sp=-40, duty_cycle_sp=25)
motor1.run_timed(time_sp=3000, speed_sp=40, duty_cycle_sp=25)
sleep(3)
read_end = g.value()
print("start angle of rotation "+str(read_start)+" degrees")
print("end angle of rotation "+str(read_end)+" degrees")
def main():
# hardware initialization
initialization()
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
gy = ev3.GyroSensor(ev3.INPUT_3)
us = ev3.UltrasonicSensor(ev3.INPUT_4)
# MQTT initialization
pub_topic = "Sensors/" + str(lego_id)
sub_topic = "Command/" + str(lego_id)
lego = mqtt.Client()
lego.user_data_set(in_out_data)
lego.on_connect = on_connect
lego.on_message = on_message
lego.on_publish = on_publish
lego.on_subscribe = on_subscribe
lego.on_disconnect = on_disconnect
lego.on_unsubscribe = on_unsubscribe
lego.connect(server_ip, 1883, 60)
lego.subscribe(sub_topic, 0)
lego.loop_start()
print('Ready')
Machine = Robot(200, 100, 0, 0)
#Machine.rotate_angle(-10, left_motor, right_motor, gy, us, a)
while (1):
# Listen to MQTT
command = str(in_out_data["command"]) + " " + str(in_out_data["arg"])
command = command.strip().split()
command = list(map(int, command))
if command[0] == 0:
x = command[1]
#print(x)
Machine.rotate_angle(x, left_motor, right_motor, gy, us, a)
else:
y = command[1]
#print (y)
Machine.drive_sm(y, left_motor, right_motor, gy, us, a)
#Transmit to MQTT
in_out_data["distance"] = str(us.value())
in_out_data["angle"] = str(gy.value())
data = in_out_data["distance"] + " " + in_out_data["angle"]
lego.publish(pub_topic, data, 0)
time.sleep(0.2)
def initialization():
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
gy = ev3.GyroSensor(ev3.INPUT_3)
us = ev3.UltrasonicSensor(ev3.INPUT_4)
assert left_motor.connected
assert right_motor.connected
assert gy.connected
assert us.connected
left_motor.stop_action = ev3.Motor.STOP_ACTION_BRAKE
right_motor.stop_action = ev3.Motor.STOP_ACTION_BRAKE
us.mode = 'US-DIST-CM' #put the US in the dist in sm mode
gy.mode = 'GYRO-ANG' #put the gyro into angule
def setupSensorsMotors(self, configs):
for item in configs:
port = configs[item]
if item == self.LEFT_MOTOR:
self.lmot = ev3.LargeMotor(port)
elif item == self.RIGHT_MOTOR:
self.rmot = ev3.LargeMotor(port)
elif item == self.SERVO_MOTOR:
self.mmot = ev3.MediumMotor(port)
elif item == self.LEFT_TOUCH:
self.leftTouch = ev3.TouchSensor(port)
elif item == self.RIGHT_TOUCH:
self.rightTouch = ev3.TouchSensor(port)
elif item == self.ULTRA_SENSOR:
self.ultraSensor = ev3.UltrasonicSensor(port)
elif item == self.COLOR_SENSOR:
self.colorSensor = ev3.ColorSensor(port)
elif item == self.GYRO_SENSOR:
self.gyroSensor = ev3.GyroSensor(port)
else:
print("Unknown configuration item:", item)
def setup_hardware(self):
# setup I/O connecting to EV3
self.motorR = ev3.LargeMotor('outA')
self.motorL = ev3.LargeMotor('outD')
self.csR = ev3.ColorSensor('in2')
self.csM = ev3.ColorSensor('in3')
self.csL = ev3.ColorSensor('in4')
#TODO: implement using the DC motor if need
#self.grabberArms = ev3.MediumMotor('outC')
self.port = ev3.LegoPort('outB')
assert self.port.connected
self.port.mode = 'dc-motor'
time.sleep(5)
self.grabberArms = ev3.DcMotor("outB")
print("DcMotor connected")
self.grabberLift = ev3.MediumMotor('outC')
self.gy = ev3.GyroSensor('in1')
self.reset_gyro()
self.base_pos = self.grabberLift.position
def __init__(self, robotName, configDict=None):
"""Takes in a string, the name of the robot, and an optional dictionary
giving motor and sensor ports for the robot."""
self.name = robotName
self.leftMotor = ev3.LargeMotor('outC')
self.rightMotor = ev3.LargeMotor('outB')
self.servoMotor = ev3.MediumMotor('outD')
self.servoMotor.reset()
self.leftTouch = None
self.rightTouch = None
self.ultraSensor = ev3.UltrasonicSensor('in4')
self.colorSensor = None
self.gyroSensor = ev3.GyroSensor('in2')
self.pixyCam = PixyCam() #input_1
self.button = ev3.Button()
ev3.Sound.set_volume(100)
if configDict is not None:
self.setupSensorsMotors(configDict)
if self.leftMotor is None:
self.leftMotor = ev3.LargeMotor('outC')
if self.rightMotor is None:
self.rightMotor = ev3.LargeMotor('outB')
