#!/usr/bin/env micropython
import sys
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.button import Button
import os
os.system('setfont Lat15-TerminusBold32x16')
# Manually move the robot to calibrate light sensor
# in case that the other automatical one is not allowed during competetion
btn = Button()
cl = ColorSensor(INPUT_1)
cr = ColorSensor(INPUT_4)
r_white = cr.reflected_light_intensity
l_white = cl.reflected_light_intensity
print("Ready, move to black")
while btn.buttons_pressed == []:
pass
r_black = cr.reflected_light_intensity
l_black = cl.reflected_light_intensity
print(l_black, file=sys.stderr)
print(l_white, file=sys.stderr)
print(r_black, file=sys.stderr)
print(r_white, file=sys.stderr)
#!/usr/bin/env python3 from ev3dev2.sensor.lego import ColorSensor import time color = ColorSensor() color.calibrate_white()
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, MoveTank, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
import xml.etree.ElementTree as ET
import threading
import time
from time import sleep
from sys import stderr
colourAttachment = ColorSensor(INPUT_4)
colourLeft = ColorSensor(INPUT_3)
colourRight = ColorSensor(INPUT_2)
gyro = GyroSensor(INPUT_1)
largeMotor_Left = LargeMotor(OUTPUT_B)
largeMotor_Right = LargeMotor(OUTPUT_C)
mediumMotor = MediumMotor(OUTPUT_D)
steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C)
tank_block = MoveTank(OUTPUT_B, OUTPUT_C)
#_________________________________________________________________________________________________________________________________
def BlackLine_rotations(stop, speed, rotations, sensor, lineSide, correction):
print("In BlackLine_rotations", file=stderr)
# calculate how far to drive in degrees
rotations = rotations * 360
# saves the current positions of the motors
currentDegrees_left = largeMotor_Left.position
currentDegrees_right = largeMotor_Right.position
# calculates the target rotations for each motor
from ev3dev2.button import Button
from ev3dev2.sound import Sound
ON = True
OFF = False
music = Sound()
music.play_file("Confirm.wav")
tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)
steering_drive = MoveSteering(OUTPUT_A, OUTPUT_D)
ir = InfraredSensor()
ir.mode = "IR-PROX"
touch_sensor = TouchSensor()
touch_sensor.mode = "TOUCH"
color_arm = MediumMotor(OUTPUT_B)
display_button = Button()
color_sensor = ColorSensor()
def deploy_color_sensor():
color_arm.on_for_rotations(SpeedPercent(5), 0.30)
time.sleep(4.5)
if color_sensor.color == 1:
music.speak("I found something black on the surface of Mars")
elif color_sensor.color == 2:
music.speak("I found water on the surface of Mars")
elif color_sensor.color == 3:
music.speak("I found a plant on the surface of Mars")
elif color_sensor.color == 4:
music.speak("I found something yellow on the surface of Mars")
elif color_sensor.color == 5:
music.speak("I found a rock on the surface of Mars")
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sound import Sound
from time import sleep
cl = ColorSensor()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
sound = Sound()
while True:
if cl.color_name=='Green':
steer_pair.on(0,50)
elif cl.color_name=='Red':
steer_pair.off()
sleep(0.01)
from ev3dev2.sensor.lego import ColorSensor, GyroSensor
from ev3dev2.motor import MoveTank
import movement as movement
import time
sensor = ColorSensor()
gyro = GyroSensor()
move = movement.Movement(gyro)
LEFT = move.left_turn
RIGHT = move.right_turn
def follow_line(self, line_color1, line_color2=None):
angle = 3
last_turn = RIGHT
color = sensor.color
print(color)
if line_color2 != None:
if color == line_color2:
move.stop
elif color == line_color1:
move.go_forward_slow()
print("found")
else:
while True:
color = sensor.color
print("lost")
def MoveLeftMotor(leftMotor = LargeMotor(OUTPUT_A), colorLeft = ColorSensor(INPUT_1)):
while colorLeft.reflected_light_intensity > Constants.BLACK and False == Constants.STOP:
leftMotor.on(speed=10)
leftMotor.off()
def run(self, target_color):
lm = self.lm
rm = self.rm
dt = 500
stop_action = "coast"
speed = 400
cs = ColorSensor()
cs.mode = 'COL-REFLECT'
# PID tuning
Kp = 1 # proportional gain
Ki = 0.01 # integral gain
Kd = 0.01 # derivative gain
target_value = self.correct_value
count = 0
integral = 0
previous_error = 0
factor_negative = (self.correct_value - self.too_dark) / 100
factor_positive = (self.too_light - self.correct_value) / 100
factor = (self.too_light - self.correct_value) / (self.correct_value -
self.too_dark)
# if value is 0 turned to left last
turn = -1
turn_speed_value = 200
while not self.end:
measured_value = cs.value()
color = 6
while color == 6:
error = measured_value - target_value
integral += (error * dt)
derivative = (error - previous_error) / dt
if error < 0:
u = (Kp * factor * factor_negative *
error) + (Ki * integral) + (Kd * derivative)
else:
u = (Kp * factor_positive *
error) + (Ki * integral) + (Kd * derivative)
if speed + abs(u) > 1000:
if u >= 0:
u = 1000 - speed
else:
u = speed - 1000
if u < 0:
lm.run_timed(time_sp=dt,
speed_sp=speed - abs(u),
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=speed + abs(u),
stop_action=stop_action)
sleep(dt / 2000)
else:
lm.run_timed(time_sp=dt,
speed_sp=speed + abs(u),
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=speed - abs(u),
stop_action=stop_action)
sleep(dt / 2000)
color = cs.color
previous_error = error
found_white = False
count = 22
while not found_white:
left_number = 0
count *= 2.5
while not found_white:
lm.run_timed(time_sp=dt,
speed_sp=-1 * turn * turn_speed_value,
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=turn * turn_speed_value,
stop_action=stop_action)
print(cs.color)
if cs.color == 6:
lm.run_timed(time_sp=dt,
speed_sp=-1 * turn * turn_speed_value,
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=turn * turn_speed_value,
stop_action=stop_action)
found_white = True
turn *= -1
if left_number >= count:
break
left_number += 1
if count > 200:
if cs.color == 4:
lm.run_timed(time_sp=dt,
speed_sp=turn_speed_value,
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=turn_speed_value,
stop_action=stop_action)
Driver().turn_degrees(180)
lm.run_timed(time_sp=dt,
speed_sp=turn_speed_value,
stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=turn_speed_value,
stop_action=stop_action)
turn *= -1
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, OUTPUT_A, OUTPUT_B
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.button import Button
from time import sleep
cl = ColorSensor(INPUT_1)
btn = Button()
steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
target = 25
# while not btn.any(): # Stop program by pressing any button
# error = target - cl.reflected_light_intensity
# turn = error * 1.5
# steer_pair.on(turn, 20)
# sleep(0.2)
while not btn.any(): # Stop program by pressing any button
steer_pair.on(0, 50)
sleep(0.2)
#!/usr/bin/env python3
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_B, follow_for_ms
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM, MoveTank, MoveSteering, SpeedPercent
from time import sleep
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
import os
import sys
os.system('setfont Lat15-TerminusBold14')
lmB = LargeMotor(OUTPUT_B)
lmC = LargeMotor(OUTPUT_C)
cs1 = ColorSensor(INPUT_1)
cs4 = ColorSensor(INPUT_4)
lmB.on(10)
lmC.on(10)
loopCounter = 0
BlackThereshold = 15
WhiteThereshold = 80
#while (cs4.color != ColorSensor.COLOR_BLACK and cs1.color != ColorSensor.COLOR_BLACK):
while (loopCounter < 48):
print(
"{0:02d} - CS1: {1:10}, CS4: {2:10}, CS1Reflected: {3:03d}, CS4Reflected: {4:03d}"
.format(loopCounter, cs1.color_name, cs4.color_name,
cs1.reflected_light_intensity, cs4.reflected_light_intensity),
file=sys.stderr)
loopCounter += 1
if (cs1.reflected_light_intensity > WhiteThereshold):
#^^^^This line is required to tell the EV3 to run this file using Python|it is called a shebang line|it MUST be on the first line
# FLL 42, Pythonian Rabbotics's master program.
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank, MediumMotor #gives us accses to everything we need to run EV3 dev
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor.lego import GyroSensor
from ev3dev2.button import Button
from ev3dev2.sound import Sound
from ev3dev2.display import Display
from robotClass import Robot
import time
btn = Button() # variable so we can get buttons pressed on EV3
color = ColorSensor(INPUT_4) # color sensor for checking attachment color
tank_drive = MoveTank(
OUTPUT_B, OUTPUT_C) # Creates a variable so we can control the drivetrain
motorA = MediumMotor(OUTPUT_A) # left medium motor
motorD = MediumMotor(OUTPUT_D) # right medium motor
gyro = GyroSensor(INPUT_1) # gyro variable
Sound_ = Sound() # beepity beep
Display_ = Display() # for displaying text
Sound_.play_tone(
frequency=400, duration=0.5, volume=50
) #there is a 15-20 second lag when we start a program so this tells us that master has alreafy started by beeping
start = time.time(
) #this makes it so that when we call start it is equal to what ever far the robot is in the code (IE: 00:12 if it was 12 seconds in)
btn.on_backspace = failsafe
import time
#from ev3dev.ev3 import *
#m1 and m2 are driving motors
#m3 is hopper motor
#c1 is color sensor for container
#c2 is color sensor for pills
#u3 is Ultrasnoic sensor for distance
#g4 is gyro
#Color sensor will return 3 for green and 5 for red. Needs to be 8 mm away
#Assign slots to each component
#m1 = LargeMotor(OUTPUT_A)
#m2 = LargeMotor(OUTPUT_B)
m3 = LargeMotor(OUTPUT_C)
#tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
c1 = ColorSensor(INPUT_1)
c2 = ColorSensor(INPUT_2)
us = UltrasonicSensor()
gs = GyroSensor()
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
def Rotate(angle):
#reset gyro
angle_degrees = math.degrees(angle)
gs.mode = 'GYRO-RATE';
gs.mode = 'GYRO-ANG';
bool = True;
tank_drive.on(left_speed=-10,right_speed=10)
while bool == True:
time.sleep(.1)
def Drilling(self):
cl = ColorSensor()
cl.mode = 'COL-COLOR'
return cl.color
#!/usr/bin/env python3
from ev3dev2.motor import Motor, MediumMotor, LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, follow_for_forever
from ev3dev2.sensor.lego import ColorSensor
lifting_motor = MediumMotor('outA')
left_motor = OUTPUT_B
right_motor = OUTPUT_C
tank_drive = MoveTank(left_motor, right_motor)
# Setpoints
speed = 15
tank_drive.cs = ColorSensor()
def move(move_dir):
if move_dir == 'forward':
tank_drive.on(SpeedPercent(speed), SpeedPercent(speed))
if move_dir == 'reverse':
tank_drive.on(SpeedPercent(-speed), SpeedPercent(-speed))
elif move_dir == '':
tank_drive.off()
def turn(turn_dir, custom_turn_degree=0):
left_turn_degree = 420
right_turn_degree = 420
u_turn_degree = 360
# Turn on left and right motor with defined speed for defined rotations
if turn_dir == 'left':
tank_drive.on(SpeedPercent(0), SpeedPercent(speed))
elif turn_dir == 'right':
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank, MediumMotor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor.lego import GyroSensor
from ev3dev2.button import Button
from ev3dev2.sound import Sound
from ev3dev2.display import Display
import time
import sys
btn = Button()
color = ColorSensor(INPUT_4)
tank_drive = MoveTank(OUTPUT_B,
OUTPUT_C) #This is the template whenever we code
motorA = MediumMotor(OUTPUT_A)
motorD = MediumMotor(OUTPUT_D)
Sound_ = Sound()
Display_ = Display()
Sound_.play_tone(frequency=400, duration=0.5,
volume=50) #plays a note so we know when the code starts
#yellow = forwards
def swing_and_safety():
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
motorA = MediumMotor(OUTPUT_A)
motorD = MediumMotor(OUTPUT_D)
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50),
6.67) #ROBOT MOVES FORWARD FROM BASE
tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(20),
def main():
threadPool = []
stopProcessing = False
ts = TouchSensor()
cl = ColorSensor()
# Load programs ..
programsXML = ET.parse('Program24_programs.xml')
programs = programsXML.getroot()
while True:
rgb = cl.raw
for program in programs:
programName = program.get('name')
rProgram = int(program.get('r'))
gProgram = int(program.get('g'))
bProgram = int(program.get('b'))
rColourSensor = rgb[0]
gColourSensor = rgb[1]
bColourSensor = rgb[2]
if abs(rColourSensor - rProgram) < 20 and abs(
gColourSensor - gProgram) < 20 and abs(bColourSensor -
bProgram) < 20:
fileName = program.get('fileName')
# Load program into memory ..
dataXML = ET.parse(fileName)
steps = dataXML.getroot()
for step in steps:
action = step.get('action')
# are their multiple actions to execute in parallel?
if action == 'launchInParallel':
for subSteps in step:
thread = launchStep(lambda: stopProcessing,
subSteps)
threadPool.append(thread)
# is there a single action to execute?
else:
thread = launchStep(lambda: stopProcessing, step)
threadPool.append(thread)
while not stopProcessing:
# remove any completed threads from the pool
for thread in threadPool:
if not thread.isAlive():
threadPool.remove(thread)
# if there are no threads running, exist the 'while' loop
# and start the next action from the list
if not threadPool:
break
# if the touch sensor is pressed then complete everything
if ts.is_pressed:
stopProcessing = True
break
sleep(0.25)
# if the 'stopProcessing' flag has been set then finish the step loop
if stopProcessing:
break
"""
PATH = directory + filename + '.csv'
with open(PATH, 'w') as f: # You will need 'wb' mode in Python 2.x
w = csv.DictWriter(f, log.keys())
w.writeheader()
w.writerow(log)
print('log written to: ' + PATH)
############################
# SENSORS #
############################
"""
initialise sensors
"""
l_sensor1 = ColorSensor(INPUT_1)
l_sensor2 = ColorSensor(INPUT_3)
us_sensor = UltrasonicSensor()
############################
# RUN #
############################
#hyperparams
N = 200
dt = 0.00000001
dt = 0.000001
dt_s1 = 0.000001
dt_both = 0.00001
dt_US = 0.01 # LEARNING RATE FOR THE US SENSOR MODEL
V = 60 #true hidden state (dist)
#!/usr/bin/env python3 from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D from ev3dev2.sensor.lego import ColorSensor, UltrasonicSensor import sensorReading # initiate color sensors # the colour sensor needs to be between 1-2 cm away from the surface you are trying to measure. (color mode) colorSensor_lt = ColorSensor(INPUT_4) colorSensor_rt = ColorSensor(INPUT_1) colorSensor_lt.mode = sensorReading.colorSensor_mode_default colorSensor_rt.mode = sensorReading.colorSensor_mode_default ultrasonicSensor = UltrasonicSensor(INPUT_3) ultrasonicSensor.mode = "US-DIST-CM" # initiate all motors largeMotor_port_lt = OUTPUT_D largeMotor_port_rt = OUTPUT_A largeMotor_lt = LargeMotor(largeMotor_port_lt) largeMotor_rt = LargeMotor(largeMotor_port_rt) largeMotor_lt.speed
#!/usr/bin/env micropython
from ev3dev2.sensor.lego import ColorSensor
from time import sleep
cl = ColorSensor()
input("Place white paper under colorsensor and press [Enter]")
cl.calibrate_white()
while True:
print("reflected:", cl.reflected_light_intensity)
sleep(0.2)
print("ambient:", cl.ambient_light_intensity)
sleep(0.2)
r, g, b = cl.rgb
print("r: {}, g: {}, b: {}".format(r, g, b))
sleep(0.2)
import socketio
from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_C, OUTPUT_B, OUTPUT_A, MoveTank, MoveSteering
from ev3dev2.sensor.lego import ColorSensor, TouchSensor
from time import sleep
from queue import Queue
sio = socketio.Client()
host = 'http://192.168.0.21:5000'
ts = TouchSensor()
cs = ColorSensor()
tank_pair = MoveTank(OUTPUT_C, OUTPUT_B)
left_motor = LargeMotor(OUTPUT_C)
right_motor = LargeMotor(OUTPUT_B)
forklift = MediumMotor(OUTPUT_A)
@sio.on('go_forward')
def go_forward():
tank_pair.on_for_seconds(left_speed=10, right_speed=10, seconds=1)
@sio.on('turn_left')
def turn_left():
right_motor.on_for_seconds(speed=10, seconds=1)
@sio.on('turn_right')
def turn_right():
left_motor.on_for_seconds(speed=10, seconds=1)
def MoveRightMotor(rightMotor = LargeMotor(OUTPUT_B), colorRight = ColorSensor(INPUT_3)):
while colorRight.reflected_light_intensity > Constants.BLACK and False == Constants.STOP:
rightMotor.on(speed=10)
rightMotor.off()
from ev3dev2.motor import MediumMotor, LargeMotor, MoveSteering, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor
from time import sleep
from sys import stderr
colourLeft = ColorSensor(INPUT_3)
colourRight = ColorSensor(INPUT_2)
gyro = GyroSensor(INPUT_1)
steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C)
largeMotor_Left= LargeMotor(OUTPUT_B)
largeMotor_Right= LargeMotor(OUTPUT_C)
mediumMotor = MediumMotor(OUTPUT_D)
#_________________________________________________________________________________________________________________________________
def off ():
# turn brake off on the motors
print('Turning motors off', file=stderr)
largeMotor_Left.off(brake = False)
largeMotor_Right.off(brake = False)
mediumMotor.off(brake = False)
# DEFINIÇÕES ent_motor_esq = OUTPUT_C ent_motor_dir = OUTPUT_D ent_motor_grande = OUTPUT_B ent_motor_medio = OUTPUT_A ent_sc_esq = INPUT_3 ent_sc_dir = INPUT_4 ent_us_lat = INPUT_2 ent_us_fr = INPUT_1 steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir) tank = MoveTank(ent_motor_esq, ent_motor_dir) tank.cs = ColorSensor(ent_sc_esq) garra_g = LargeMotor(ent_motor_grande) garra_m = MediumMotor(ent_motor_medio) cor_esq = ColorSensor(ent_sc_esq) cor_dir = ColorSensor(ent_sc_dir) usl = UltrasonicSensor(ent_us_lat) usf = UltrasonicSensor(ent_us_fr) sound = Sound() btn = Button() #Funções #Cor #Locomoção
def __init__(self,
motorPort1,
motorPort2,
modulePort,
colorPort1,
colorPort2,
gyro1=None,
gyro2=None,
motorDiff=1,
colMode="COL-COLOR",
moduleSensor=None,
printErrors=True,
enableConsole=False,
modulePort2=None):
if motorPort1 and motorPort2:
self.ms = MoveSteering(
motorPort1,
motorPort2) # If defined in parameters, define MoveSteering
if motorPort1 and motorPort2:
self.mt = MoveTank(
motorPort1,
motorPort2) # If defined in parameters, define MoveTank
if motorPort1:
self.m1 = LargeMotor(
motorPort1) # If defined in parameters, define Left Motor
if motorPort2:
self.m2 = LargeMotor(
motorPort2) # If defined in parameters, define Right Motor
if modulePort:
self.mmt = Motor(modulePort)
# If defined in parameters, define module motor
if modulePort2:
self.mmt2 = Motor(modulePort2)
# If defined in parameters, define module motor
if enableConsole: self.resetMotors()
if colorPort1 != None: # If defined in parameters, define Left Color sensor
self.csLeft = ColorSensor(colorPort1)
self.csLeft.mode = colMode # Set color mode to the one in the parameters
if colorPort2 != None: # If defined in parameters, define Right Color sensor
self.csRight = ColorSensor(colorPort2)
self.csRight.mode = colMode # Set color mode to the one in the parameters
if moduleSensor != None: # If defined in parameters, define module sensor
self.moduleSensor = ColorSensor(moduleSensor)
self.moduleSensor.mode = "COL-COLOR"
try:
self.gs = GyroSensor(gyro1)
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
except:
print("Gyro 1 can't be defined!"
) # Define gyro if possible, otherwise show error
try:
self.gs2 = GyroSensor(gyro2)
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro 2 can't be defined!"
) # Define gyro if possible, otherwise show error
self.using2ndGyro = False # Set default gyro to the 1st one
self.motorDiff = motorDiff # Set motor difference to the one defined
self.leds = Leds() # Setup brick leds
self.bt = Button() # Setup brick buttons
if enableConsole:
self.console = Console(
font="Lat15-Terminus32x16") # Enable console access if needed
try:
with open("/home/robot/sappers2/pid.txt", "w") as pid:
pid.write(str(os.getpid()))
except:
pass # Write PID into file for other applications.
if printErrors: print("Successfully Initialized. ")
self.timerStart = time.time()
#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File : color_sensor.py
@Time : 2019/01/19 11:22:34
@Author : Zijing Feng
颜色传感器测试程序
需要更改的是第22行
第22行需要根据实际传感器插入主机的接口更改
更多细节参考https://ev3dev-lang.readthedocs.io/projects/python-ev3dev/en/ev3dev-stretch/sensors.html?highlight=Sensor#color-sensor
'''
import sys
sys.path.append(r"/home/robot/SmartCar-master/")
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor import *
#主机与传感器建立连接
__sensor = ColorSensor(INPUT_1)
#获得颜色传感器反射光的值
ColorSensor().mode = 'COL-REFLECT'
while 1:
debug_print(ColorSensor.value)
#ColorSensor().value() reflected_light_intensity 反射光亮度 单位%
#!/usr/bin/python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, MediumMotor
#from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import ColorSensor
# from ev3dev2.led import Leds
from ev3dev2.button import Button
from time import sleep
defaultSpeed = 20
slowSpeed = 15
penStateDown = False
penPositionX = 500
cl = ColorSensor()
feeder = LargeMotor(OUTPUT_B)
penRL = LargeMotor(OUTPUT_A)
penLift = MediumMotor(OUTPUT_C)
btn = Button()
cl.mode = 'COL-COLOR'
colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red', 'white',
'brown')
def toPositive(x, y=0):
if x < 0:
print("inverted value: ", x * (-1))
return x * (-1)
else:
print("unchanged value: ", x)
def __init__(self, port):
self.sensor = ColorSensor(port)
self.mode = self.sensor.mode # Used to save time so not to call the get method each time
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_B, follow_for_ms
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM, MoveTank, MoveSteering, SpeedPercent
from time import sleep
from ev3dev2.sensor.lego import ColorSensor
<<<<<<< HEAD
#LeftWheel = LargeMotor(OUTPUT_B)
#RightWheel = LargeMotor(OUTPUT_C)
TankPair = MoveTank(OUTPUT_C, OUTPUT_B, motor_class=LargeMotor)
LeftSensor = ColorSensor(INPUT_1)
RightSensor = ColorSensor(INPUT_4)
N = 0
#while N<4:
#TankPair.on_for_seconds(SpeedDPS(-400),SpeedDPS(-400), 1)
#TankPair.on_for_degrees(SpeedDPS(-250),SpeedDPS(250),115,True,True)
# N = N + 1
=======
>>>>>>> 8c5914dbd59ea2113de00c06e2cc0afc2281b666
class myRobot():
def __init__(self,
motorPort1,
motorPort2,
modulePort,
colorPort1,
colorPort2,
gyro1=None,
gyro2=None,
motorDiff=1,
colMode="COL-COLOR",
moduleSensor=None,
printErrors=True,
enableConsole=False,
modulePort2=None):
if motorPort1 and motorPort2:
self.ms = MoveSteering(
motorPort1,
motorPort2) # If defined in parameters, define MoveSteering
if motorPort1 and motorPort2:
self.mt = MoveTank(
motorPort1,
motorPort2) # If defined in parameters, define MoveTank
if motorPort1:
self.m1 = LargeMotor(
motorPort1) # If defined in parameters, define Left Motor
if motorPort2:
self.m2 = LargeMotor(
motorPort2) # If defined in parameters, define Right Motor
if modulePort:
self.mmt = Motor(modulePort)
# If defined in parameters, define module motor
if modulePort2:
self.mmt2 = Motor(modulePort2)
# If defined in parameters, define module motor
if enableConsole: self.resetMotors()
if colorPort1 != None: # If defined in parameters, define Left Color sensor
self.csLeft = ColorSensor(colorPort1)
self.csLeft.mode = colMode # Set color mode to the one in the parameters
if colorPort2 != None: # If defined in parameters, define Right Color sensor
self.csRight = ColorSensor(colorPort2)
self.csRight.mode = colMode # Set color mode to the one in the parameters
if moduleSensor != None: # If defined in parameters, define module sensor
self.moduleSensor = ColorSensor(moduleSensor)
self.moduleSensor.mode = "COL-COLOR"
try:
self.gs = GyroSensor(gyro1)
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
except:
print("Gyro 1 can't be defined!"
) # Define gyro if possible, otherwise show error
try:
self.gs2 = GyroSensor(gyro2)
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro 2 can't be defined!"
) # Define gyro if possible, otherwise show error
self.using2ndGyro = False # Set default gyro to the 1st one
self.motorDiff = motorDiff # Set motor difference to the one defined
self.leds = Leds() # Setup brick leds
self.bt = Button() # Setup brick buttons
if enableConsole:
self.console = Console(
font="Lat15-Terminus32x16") # Enable console access if needed
try:
with open("/home/robot/sappers2/pid.txt", "w") as pid:
pid.write(str(os.getpid()))
except:
pass # Write PID into file for other applications.
if printErrors: print("Successfully Initialized. ")
self.timerStart = time.time()
def prepareForRun(self): # Reset gyro and motors
self.ms.reset()
self.gyroReset()
def changeGyro(self, newGyroPort): # Change gyro to 2nd one
try:
self.gs = GyroSensor(newGyroPort)
self.using2ndGyro = True
except:
print("Can't change Gyro!")
def attachNew(self,
isMotor=True,
port="outA",
largeMotor=False): # Attach new motor
if isMotor:
self.m3 = Motor(port)
else:
self.m3 = LargeMotor(port)
def followLine(self,
timeOfGoing,
speed,
multiplier=5,
debug=False): # Simple line follower
currentTime = time.time()
while time.time() - currentTime < timeOfGoing:
colorLeft = int(self.csLeft.value() / 1)
colorRight = int(self.csRight.value() / 1)
print(colorLeft, colorRight)
if colorLeft != 6 and colorRight != 6:
tempSteering = 0
elif colorLeft != 6:
tempSteering = -multiplier
elif colorRight != 6:
tempSteering = multiplier
else:
tempSteering = 0
self.ms.on(tempSteering, speed * self.motorDiff)
def goByWay(self,
waySteering,
timeToGo,
speed=80,
debug=False): # MoveSteering but for time
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(waySteering, speed * self.motorDiff)
def goUntilLine(self,
speed,
lineColor,
multiplier=1,
debug=False
): # Goes forward with gyro, but stops when detecting line
pastGyro = self.gs.value()
while (self.csLeft.value() not in lineColor) and (self.csRight.value()
not in lineColor):
self.ms.on((self.gs.value() - pastGyro) * multiplier,
speed * self.motorDiff)
if debug:
print(self.gs.value(), self.csLeft.value(),
self.csRight.value())
print(self.gs.value(), self.csLeft.value(), self.csRight.value())
def goWithGyro(self, timeToGo, speed=70, multiplier=2, debug=False):
# Forward with gyro for time
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
self.ms.on(self.max100((self.gs.value() - pastGyro) * multiplier),
speed * self.motorDiff)
if debug: print(self.gs.value())
def moveModule(self,
length,
speed=30,
stopMoving=True,
useLarge=False,
waitAfter=False): # Module motor access
if useLarge:
try:
self.m3.on(speed)
time.sleep(length)
if stopMoving: self.m3.reset()
except:
print("not working bruh you should fix it like rn")
else:
while True:
try:
self.mmt.on(speed * -1)
time.sleep(length)
if stopMoving: self.mmt.reset()
if waitAfter: time.sleep(waitAfter)
break
except Exception as e:
print(e)
break
def moveModuleByRot(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveModuleByRot2(self,
rotations,
speed,
block=True): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=block)
def moveBothModules(self,
rotations,
speed,
block=True,
speed2=False): # Deprecated module moving
self.mmt.on_for_rotations(-speed, rotations, block=False)
self.mmt2.on_for_rotations((speed2 if speed2 else -speed),
rotations,
block=block)
def resetMotors(self): # Motor reset
self.ms.reset()
self.mmt.on(100)
time.sleep(1)
self.mmt.reset()
def gyroReset(self): # Gyro reset
self.gs.mode = "GYRO-RATE"
self.gs.mode = "GYRO-ANG"
try:
self.gs2.mode = "GYRO-RATE"
self.gs2.mode = "TILT-ANG"
except:
print("Gyro2 not found!")
def resetGyro(self, gyroObject, desiredMode="TILT-ANG"):
gyroObject.mode = "GYRO-RATE"
gyroObject.mode = desiredMode
def fullStop(self, brakeOrNo=True): # Stops the robot, breaks if defined.
self.ms.stop(brake=brakeOrNo)
self.mmt.reset()
try:
self.mmt2.reset()
except:
pass
def turnWithGyro(
self,
degreesToTurn=90,
speedToTurnAt=20): # Turns with gyro, and saves past value
gyroPast = self.gs.value()
self.ms.on(90, speedToTurnAt)
while gyroPast - self.gs.value() <= degreesToTurn:
print(gyroPast - self.gs.value())
self.ms.stop(brake=True)
print(self.gs.value())
def justGo(self, speed,
timeOfGoing): # Goes forward without gyro, for time
self.ms.on(0, speed * self.motorDiff)
time.sleep(timeOfGoing)
def justTurn(self, speed,
timeOfGoing): # Turns withot gyro, useful for quick turns.
self.ms.on(90, speed * self.motorDiff)
time.sleep(timeOfGoing)
def absoluteTurn(self,
speed,
absoluteLoc,
rotWay=-1,
giveOrTake=1,
debug=False,
ver2=False): # Turns using absolute value from gyro
self.ms.on(90 * rotWay, speed * self.motorDiff)
# while self.gs.value() > absoluteLoc+giveOrTake or self.gs.value() < absoluteLoc+giveOrTake :
# if debug: print(self.gs.value())
# pass
while self.gs.value() > absoluteLoc + giveOrTake or self.gs.value(
) < absoluteLoc - giveOrTake:
if debug: print(self.gs.value())
pass
def isPositive(self, number):
if number > 0:
return 1
elif number < 0:
return -1
else:
return 0
def absolute(self, speed, location, giveOrTake=2, debug=False):
"""
Absolute turning
"""
while self.gs.value() > location + giveOrTake or self.gs.value(
) < location - giveOrTake:
self.ms.on(90 * self.isPositive(self.gs.value() - location),
speed * self.motorDiff)
if debug: print(self.gs.value())
pass
def turnForMore(
self,
speed,
minDeg,
rotWay=-1,
debug=False,
ver2=False
): # Turns with gyro, but stops quicker than absolute turn (useful for big turns)
if debug: print(self.gs.value())
self.ms.on(90 * rotWay, speed * self.motorDiff)
if ver2:
currentDeg = int(self.gs.value())
while minDeg - 1 <= currentDeg <= minDeg + 1:
if debug: print(self.gs.value())
else:
if rotWay < 0:
while int(self.gs.value()) < int(minDeg):
if debug: print(self.gs.value())
pass
else:
while int(self.gs.value()) > int(minDeg):
if debug: print(self.gs.value())
pass
def goWithShift(self,
timeToGo,
speed=70,
multiplier=2,
shift=1): # Goes with gyro using shift
pastGyro = self.gs.value()
currentTime = time.time()
while time.time() - currentTime < timeToGo:
if self.gs.value() == 0:
gsValue = shift
else:
gsValue = self.gs.value()
self.ms.on((gsValue - pastGyro) * multiplier,
speed * self.motorDiff)
def goWithGyroRot(self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes using gyro using one motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if speed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def waitForColor(self,
color=[1, 2, 3, 4, 5, 6],
amountOfTime=0.5,
debug=False,
waitingTime=0.5): # Waits until color detected
colorArray = []
while True:
if self.moduleSensor.value() in color:
colorArray.append(self.moduleSensor.value())
time.sleep(amountOfTime / 10)
if debug: print(colorArray, end="\r")
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
elif self.bt.down == True:
print("awaiting input")
return False
else:
colorArray = []
if debug: print("waiting", self.moduleSensor.value(), end="\r")
self.leds.set_color("LEFT", "RED")
self.leds.set_color("RIGHT", "RED")
if len(colorArray) == 9:
break
self.leds.set_color("LEFT", "ORANGE")
self.leds.set_color("RIGHT", "AMBER")
time.sleep(waitingTime)
if debug: print(max(set(colorArray), key=colorArray.count), end="\n")
return max(set(colorArray), key=colorArray.count)
def turnWithGyro2(self,
degreesToTurn=90,
speedToTurnAt=20,
debug=False): # Deprecated turn with gyro
gyroPast = self.gs.value()
while gyroPast - self.gs.value() <= degreesToTurn:
self.ms.on(90, ((degreesToTurn - speedToTurnAt) + abs(
(degreesToTurn - speedToTurnAt))) / 2)
if debug: print(gyroPast - self.gs.value())
def startRun(self,
toExec,
resetGyro=True,
colorArray=[3]): # Starts run using magic wand
self.waitForColor([3], 1, True)
self.gyroReset()
exec(toExec)
def gyroSlowLinearly(
self,
rotations,
startSpeed,
multiplier,
minSpeed=5,
debug=False,
startValue=None,
stopAtEnd=False
): # Goes with gyro and slows down as it nears to destination
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = self.m1.position
steerDiff = -1 if startSpeed < 0 else 1
while abs((currentRot - self.m1.position) / 360) < rotations:
currSpeed = 1 - (
(int(abs((currentRot - self.m1.position) / 360) * 100) / 100) /
rotations)
if debug:
print(
currSpeed,
round(
(startSpeed * currSpeed + minSpeed) * self.motorDiff))
self.ms.on(
(self.gs.value() - pastGyro) * multiplier * steerDiff,
round((startSpeed * currSpeed + minSpeed) * self.motorDiff))
if stopAtEnd: self.ms.stop(brake=True)
def goWithGyroRot2(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on((self.gs.value() - pastGyro) * multiplier * steerDiff,
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def returnButtons(self, pack2=False): # Returns pressed buttons for menu
if pack2:
packable = list(sys.stdin.readline().replace("\x1b[", "").rstrip())
return [''.join(x) for x in zip(packable[0::2], packable[1::2])]
else:
return sys.stdin.readline().replace("\x1b[", "").rstrip()
def goWithGyroForLevel(
self,
startRots,
speed,
multiplier,
debug=False,
startValue=None,
levelNeeded=5,
overrideDegrees=0
): # Goes forward with gyro until water-level is detected
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
self.goWithGyroRot2(startRots, speed, multiplier, False, startValue,
False)
print(abs(self.gs2.value()))
while abs(self.gs2.value()) > levelNeeded:
if debug:
print(
(abs(self.gs2.value()), self.gs.value(),
((self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees))
self.ms.on(
self.max100((
(self.gs.value() - pastGyro) * multiplier * steerDiff) +
overrideDegrees), speed * self.motorDiff)
def playSound(self, mscCommand): # Plays a sound
try:
os.system(mscCommand)
except:
pass
def max100(self, value): # Sets value to a max of 100
if value > 100:
return 100
elif value < -100:
return -100
else:
return value
def sleep(self, seconds): # sleep.
time.sleep(seconds)
def stopAtLine(
self,
speed,
color,
correctionSpeed=5): # Goes forward and stops when detecting lines
pastGyro = self.gs.value()
while True:
if self.csLeft.value() == color and self.csRight.value() == color:
break
elif self.csRight.value() != color and self.csLeft.value(
) == color:
self.mt.on(0, correctionSpeed * self.motorDiff)
elif self.csLeft.value() != color and self.csRight.value(
) == color:
self.mt.on(correctionSpeed * self.motorDiff, 0)
elif self.csLeft.value() != color and self.csRight.value(
) != color:
self.ms.on((self.gs.value() - pastGyro),
speed * self.motorDiff)
print({
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
print(
"final", {
"right": self.csRight.value(),
"left": self.csLeft.value(),
"gyro": self.gs.value()
})
def stayInPlace(self, multiplier
): # Using gyro and motor values, keeps the robot in place
motor1 = self.m1.position
motor2 = self.m2.position
while True:
corrig1 = -(self.m1.position - motor1) * multiplier
corrig2 = -(self.m2.position - motor2) * multiplier
if round(corrig1, 2) == 0 and round(corrig2, 2) == 0:
self.mt.stop(brake=False)
else:
self.mt.on(corrig1, corrig2)
print(corrig1, corrig2)
# print(corrig1, corrig2)
def moveModuleRepeating(self, raiseTime, downTime, speed1, speed2,
iterations): # Module motor access
for i in range(iterations):
self.moveModule(raiseTime, speed1, False)
self.moveModule(downTime, speed2, True)
def section(self, sectionName, resetTimer=False):
"""
Provides timing functionality, and makes it easier to see sections of runs in logs.
`resetTimer` parameter optional, it will reset the main runTimer.
"""
if resetTimer:
self.timerStart = time.time()
print(sectionName)
else:
print("%s, current run time: %i" %
(sectionName, self.timerStart - time.time()))
def goWithGyroRot2Maxed(
self,
rotations,
speed,
multiplier,
debug=False,
startValue=None,
stopAtEnd=False,
timeout=100000): # Goes forward using both motor's rotation value
pastGyro = self.gs.value() if startValue == None else startValue
currentRot = (self.m1.position + self.m1.position) / 2
steerDiff = -1 if speed < 0 else 1
startTime = time.time()
while abs((currentRot - (self.m1.position + self.m1.position) / 2) /
360) < rotations or time.time() - startTime >= timeout:
if debug:
print(
int(abs(
(currentRot - self.m1.position) / 360) * 100) / 100,
self.gs.value())
self.ms.on(
self.max100(
(self.gs.value() - pastGyro) * multiplier * steerDiff),
speed * self.motorDiff)
if stopAtEnd: self.ms.stop(brake=True)
def cor_dir(self):
return ColorSensor(self.ent_sc_dir)
