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'