def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
# display something on the screen of the device
print('Hello World!')
# print something to the output panel in VS Code
debug_print('Hello VS Code!')
# announce program start
#ev3.Sound.speak('Test program starting!').wait()
# set the motor variables
#PID controller for movement and avoidance.
mb = ev3.LargeMotor('outB')
mc = ev3.LargeMotor('outC')
us3 = ev3.UltrasonicSensor('in3') #set ultrasonic sensor var
us2 =ev3.UltrasonicSensor('in2')
ls1=ev3.ColorSensor("in1")
ls1.mode="COL-AMBIENT"
tp=50 #target power, which is 50% power on both motors. may just be reworded to tsp(target speed) if we can't do % power
kp=4 #the constant for the proportional controller, or how fast it turns to corrects. 10 is just a wild guess.
# additionally kp=10(*10) because we divide p by 10 after the calculation to help the robot process ints. apparently...
# it doesn't like floats, so we multiply a 10 to 99 kp value by 10, and a .1 to 1 kp value by 100
ki=.05 # constant for the integral controller, or how fast it adds extra gentle turn. good for fixing small past errors. also divided by 10.
kd=1 # constant for the derivitive controller, or how fast it preemptivly adds/ subtracts turn based on the integral. also divided by 10
kpLight=5
kiLight=.05
kdLight=1
target= 200 #set the target distance. This is also known as the "offset". used for noting how close the robot can get to bjects before it panics
#setting containers for varibles to be used. Don't modify these, the code does that
startTime=time.time()
integral=0
lastError=0
derivitive=0
mcMove=0
mbMove=0
lightValue=0
lightError=0
maxLightvalue=0
currentLightValue=0
lastLightError=0
localMaxLightValue=0
i=0
romeObjDirection=1
oneTimeBoolLock=False
foundLocal=False
lookingForLocal=False
collisionWhileSearch=False
collisionSpinCounter=0
def __init__(self):
self.DEFAULT_SPEED = 400
self.pipe = None
# define sensors
# self.gyroscope_sensor = ev3.GyroSensor('in4')
# self.reset_gyroscope()
# self.gyro_value = 0
self.color_sensors = (2, 2)
self.ultrasonic_sensors = {"right": ev3.UltrasonicSensor('in2'), "top": 0,
"front-right": ev3.UltrasonicSensor('in4'), "front-left": ev3.UltrasonicSensor('in3')}
self.infrared_sensors = {"upper_front": 25, "left": ev3.InfraredSensor('in1')}
# define motors
self.motors = Duo(ev3.LargeMotor('outA'), ev3.LargeMotor('outB'))
self.motors.left.polarity = "inversed"
self.motors.right.polarity = "inversed"
self.handler = Duo(ev3.MediumMotor('outC'), ev3.LargeMotor('outD'))
# define status
self.historic = [""]
def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
# display something on the screen of the device
print('Best Robot 2019')
# print something to the output panel in VS Code
debug_print('Good evening')
# announce program start
ev3.Sound.speak('Program Starting').wait()
# set the motor variables
mb = ev3.LargeMotor('outB')
mc = ev3.LargeMotor('outC')
# set the ultrasonic sensor variable
us3 = ev3.UltrasonicSensor('in3')
# Get the ultrasonic sensors values
left_sensor = ev3.UltrasonicSensor('in3')
power_left, power_right = calculate_pid(left_sensor)
# Give power to the motors
mb.run_direct(duty_cycle_sp=-(power_left))
mc.run_direct(duty_cycle_sp=-(power_right))
def connect():# Connect Motors
global motorL
global motorR
global servoL
global servoR
global colSensorL
global colSensorR
global uSonic
global uSonic2
motorL = ev3.LargeMotor('outD')
assert motorL.connected, "Error while connecting left LargeMotor"
motorR = ev3.LargeMotor('outA')
assert motorR.connected, "Error while connecting right LargeMotor"
servoL = ev3.MediumMotor('outC')
assert servoL.connected, "Error while connecting left servo"
servoR = ev3.MediumMotor('outB')
assert servoR.connected, "Error while connecting right servo"
# Connect color sensors
colSensorL = ev3.ColorSensor('in4')
assert colSensorL.connected, "Error while connecting left ColorSensor"
colSensorR = ev3.ColorSensor('in1')
assert colSensorR.connected, "Error while connecting right ColorSensor"
# Connect ultrasonic sensor
uSonic = ev3.UltrasonicSensor('in2')
assert uSonic.connected, "Error while connecting UltrasonicSensor"
uSonic2 = ev3.UltrasonicSensor('in3')
ev3.Sound.speak('Sensors connected!').wait()
def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
# display something on the screen of the device
print('Best Robot 2019')
# print something to the output panel in VS Code
debug_print('Good evening')
# announce program start
ev3.Sound.speak('Program Starting').wait()
# set the motor variables
mb = ev3.LargeMotor('outB')
mc = ev3.LargeMotor('outD')
# set the ultrasonic sensor variable
left_sensor = ev3.UltrasonicSensor('in1')
right_sensor = ev3.UltrasonicSensor('in4')
while True:
time.sleep(0.005)
power_left, power_right = calculate_pid(left_sensor.value(),
right_sensor.value(), "P")
# Give power to the motors
debug_print("Power Left=" + str(power_left) + "Power Right=" +
str(power_right))
mb.run_direct(duty_cycle_sp=(power_left))
mc.run_direct(duty_cycle_sp=(power_right))
def lookForEdge(standard=2.0):
ultraSonic1 = ev3.UltrasonicSensor(ev3.INPUT_2)
ultraSonic2 = ev3.UltrasonicSensor(ev3.INPUT_3)
while True:
#print(ultraSonic1.distance_centimeters)
#print(ultraSonic2.distance_centimeters)
if ultraSonic1.distance_centimeters > 2*standard or ultraSonic1.distance_centimeters > 2 * standard:
stop()
def ReadSensors():
global touchingLeft
global touchingRight
global seeingLeft
global seeingRight
touchingLeft = ev3.TouchSensor().value()
touchingRight = ev3.TouchSensor().value()
seeingLeft = ev3.UltrasonicSensor().value()
seeingRight = ev3.UltrasonicSensor().value()
def __init__(self, settings):
self.distance_sensor_right = ev3.UltrasonicSensor(
settings.getRightDistanceSensorAddress())
self.distance_sensor_left = ev3.UltrasonicSensor(
settings.getLeftDistanceSensorAddress())
if (settings.hasFrontDistanceSensor()):
self.distance_sensor_front = ev3.UltrasonicSensor(
settings.getFrontDistanceSensorAddress())
else:
self.distance_sensor_front = None
def get_ultrasonic(port, settings):
try:
if settings['us_mode'] == 'distance':
if settings['units'] == 'cm': # convert from mm to cm
return ev3.UltrasonicSensor(port).value() * 0.1
elif settings[
'units'] == 'in': # convert from mm to in (mm -> cm -> in)
return ev3.UltrasonicSensor(port).value() * 0.1 * 0.393701
except ValueError:
return "Not found"
def setupSonars():
sonars = []
sonars.append(ev3.UltrasonicSensor('in4'))
sonars.append(ev3.UltrasonicSensor('in2'))
sonars.append(ev3.UltrasonicSensor('in3'))
for sonar in sonars:
sonar.connected
sonar.mode = 'US-DIST-CM'
return sonars
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 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 __init__(self, robot=None):
"""Set up robot and sensors"""
self.robot = robot
self.us = ev3.UltrasonicSensor("in3")
self.us.mode = "US-DIST-CM"
self.state = 'seeking'
self.robot.forward(0.3)
def rotate_sensor():
us = ev3.UltrasonicSensor()
us.mode = 'US-DIST-CM'
units = us.units
distance = us.value() / 10
X = []
motor_sensor = ev3.MediumMotor('outD')
motor_sensor.reset()
motor_sensor.run_to_rel_pos(position_sp=-180,
speed_sp=60,
stop_action="brake")
tick = motor_sensor.position
count = 0
tick_last = 0
while count < 90:
tick = motor_sensor.position
if tick % 2 == 0 and tick != tick_last:
distance = us.value() / 10
X.append(distance)
print(str(distance))
tick_last = tick
count = len(X)
sleep(4)
motor_sensor.run_to_rel_pos(position_sp=180,
speed_sp=200,
stop_action="brake")
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 sonarTesting():
# declare sonar
sonar = ev3.UltrasonicSensor(ev3.INPUT_4)
sonar.connected
sonar.mode = 'US-DIST-CM'
motorM = ev3.MediumMotor('outB')
motorM.connected
ev3.Sound.speak('Place robot at obstacle start').wait()
print('Place at obstacle start')
sonar1 = sonar.value()
print(sonar1)
motorM.run_timed(duty_cycle_sp=45, time_sp=400)
time.sleep(1)
ev3.Sound.speak('start moving').wait()
print('start moving')
time.sleep(1)
counter = 0
while (counter < 20):
sonar2 = sonar.value()
print(sonar2)
time.sleep(1)
counter += 1
ev3.Sound.speak('finished').wait()
def main():
initialization()
left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
us = ev3.UltrasonicSensor()
us.mode = 'US-DIST-CM'
assert us.connected
Analsys = Sensors(us)
Machine = Robot(400, 200, 30)
while (True):
distance = Analsys.read_distance(us)
print(str(distance))
time.sleep(0.5)
if distance < 200:
Machine.rotate_angle_anticlockwise(left_motor, right_motor)
time.sleep(0.01)
Machine.drive_sm(left_motor, right_motor)
time.sleep(0.01)
Machine.rotate_angle_clockwise(left_motor, right_motor)
else:
left_motor.run_forever(speed_sp=-400)
right_motor.run_forever(speed_sp=-400)
def __init__(self, robot = None):
"""Set up motors/robot and sensors here, set state to 'seeking' and forward
speed to nonzero"""
self.ultrasonic_sensor = ev3.UltrasonicSensor('in2')
self.robot = robot
self.state = 'seeking'
self.robot.runforever(0.1)
def USscanComplete(self):
motor2 = ev3.MediumMotor('outD')
motor2.connected
motor2.run_timed(time_sp=2000, speed_sp=-90)
us = ev3.UltrasonicSensor()
while (us.value() > 50):
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=-90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=-90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=-90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=-90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=90)
print us.value()
motor2.run_timed(time_sp=2000, speed_sp=90)
pass
sensor_value = us.value()
motor2.stop
ev3.Sound.speak('Object has been detected').wait()
print("object at:" + str(sensor_value) + "mm")
ev3.Sound.speak("object at" + str(motor2.position) + "degrees").wait()
def searchForObstacles():
motor = ev3.MediumMotor('outA')
motor.connected
sonar = ev3.UltrasonicSensor(ev3.INPUT_1)
sonar.connected
sonar.mode = 'US-DIST-CM' # will return value in mm
readings = ""
readings_file = open('results.txt', 'w')
btn = ev3.Button()
directions = (1, 1, -1, -1)
count = 0
while count < 12:
readings = readings + str(sonar.value()) + '\n'
direction = directions[count % 4]
motor.run_timed(duty_cycle_sp=25 * direction, time_sp=350)
time.sleep(2)
count += 1
readings_file.write(readings)
readings_file.close() # Will write to a text file in a column
def USscanForward_timed(self):
#This function scans for objects and implements avoidance measures
#if necessary
motor2 = ev3.MediumMotor('outD')
motor2.connected
#motor2.run_timed(time_sp=2000, speed_sp=-90)
us = ev3.UltrasonicSensor()
#t_end = time.time() + 5
#while time.time() < t_end:
print us.value()
if us.value() < 100:
ev3.Sound.speak('object detected').wait()
OD = OC.OdometryControl()
OD.turn_left_ObjectAvoid()
motor2.run_timed(time_sp=1000, speed_sp=90)
OD.objectAvoidForward()
motor2.run_timed(time_sp=1000, speed_sp=90)
ev3.Sound.speak('object still in range').wait()
motor2.run_timed(time_sp=1000, speed_sp=-90)
motor2.run_timed(time_sp=1000, speed_sp=-90)
OD.turn_right_ObjectAvoid()
OD.objectAvoidForward()
OD.turn_right_ObjectAvoid()
OD.objectAvoidForward2()
ev3.Sound.speak('object has been avoided')
def __init__(self,
robot=None,
bttn=None,
leftM='outD',
rightM='outA',
armM='outC',
medM='outB',
cs='in3',
us='in1',
cs2='in4'):
self.bttn = ev3.Button()
self.leftM = ev3.LargeMotor(leftM)
self.rightM = ev3.LargeMotor(rightM)
self.armM = ev3.LargeMotor(armM)
self.medM = ev3.MediumMotor(medM)
#self.leftT = ev3.TouchSensor('in4')
#self.rightT = ev3.TouchSensor('in1')
self.cs = ev3.ColorSensor('in3')
self.us = ev3.UltrasonicSensor('in1')
self.cs2 = ev3.ColorSensor('in4')
self.lflag = 0
self.rflag = 0
if (self.leftM.is_running):
lflag = 1
print("Left booster is on")
if (self.rightM.is_running):
rflag = 1
print("Right booster is on")
pass
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 __init__(self, out1, out2, in1):
self.lm1 = ev3.LargeMotor(out1)
assert self.lm1.connected
self.lm2 = ev3.LargeMotor(out2)
assert self.lm2.connected
self.us = ev3.UltrasonicSensor(in1)
assert self.us.connected
def __init__(self,out1,out2,in1,in2,in3, in4):
self.lm1 = ev3.LargeMotor(out1); assert self.lm1.connected
self.lm2 = ev3.LargeMotor(out2); assert self.lm2.connected
self.se = ev3.ColorSensor(in1); assert self.se.connected
self.sm = ev3.ColorSensor(in2); assert self.sm.connected
self.sd = ev3.ColorSensor(in3); assert self.sd.connected
self.us = ev3.UltrasonicSensor(in4); assert self.us.connected
def __init__(self, robot=None):
"""Set up motors/robot and sensors here, set state to 'seeking' and forward
speed to nonzero"""
self.robot = robot
self.us = ev3.UltrasonicSensor("in3")
self.us.mode = "US-DIST-CM"
self.state = 'seeking'
self.robot.forward(0.3) # <turn motors on>
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 main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
# display something on the screen of the device
print('Hello World!')
# print something to the output panel in VS Code
debug_print('Hello VS Code!')
# announce program start
ev3.Sound.speak('Test program starting!').wait()
# set the motor variables
mb = ev3.LargeMotor('outB')
mc = ev3.LargeMotor('outC')
sp = -25
# set the ultrasonic sensor variable
us3 = ev3.UltrasonicSensor('in3')
# program loop
for x in range(1, 5):
# fetch the distance
ds = us3.value()
# display the distance on the screen of the device
print('Distance =', ds)
# print the distance to the output panel in VS Code
debug_print('Distance =', ds)
# announce the distance
ev3.Sound.speak(ds).wait()
# move
mb.run_direct(duty_cycle_sp=sp)
mc.run_direct(duty_cycle_sp=sp)
time.sleep(1)
# stop
mb.run_direct(duty_cycle_sp=0)
mc.run_direct(duty_cycle_sp=0)
# reverse direction
sp = -sp
# announce program end
ev3.Sound.speak('Test program ending').wait()
def __init__(self, outMe, outMd, inUs, inRmp):
#declare all the motors and sensors
self.me = ev3.LargeMotor(outMe)
self.md = ev3.LargeMotor(outMd)
self.us = ev3.UltrasonicSensor(inUs)
self.ramp = ev3.LargeMotor(inRmp)
#reset everything and holds everything into position
self.ramp.reset()
self.reset()
self.stop('hold')
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'
