def secwhile():
global switch1, switch2
lcolor1 = LightSensor(INPUT_2)
lcolor2 = LightSensor(INPUT_3)
Kp = 85
Ki = 0
Kd = 0
offset = d
Tp = 50
integral = 0
lastError = 0
dervitave = 0
while (switch2):
if ((lcolor1.reflected_light_intensity -
lcolor2.reflected_light_intensity) > d):
switch1 = True
switch2 = False
lightvalue2 = lcolor2.reflected_light_intensity
error = lightvalue2 - offset
integral = integral + error
dervitave = error - lastError
turn = Kp * error + Ki * integral + Kd * dervitave
turn = turn / 100
powerA = Tp - turn
powerC = Tp + turn
ml.on(SpeedPercent(powerA))
mr.on(SpeedPercent(powerC))
lastError = error
def firstwhile():
lcolor1 = LightSensor(INPUT_2)
lcolor2 = LightSensor(INPUT_3)
Kp = 138
Ki = 0
Kd = 0
offset = 27
Tp = 30
integral = 0
lastError = 0
dervitave = 0
while (switch1):
global switch1, switch2
if (lcolor1.reflected_light_intensity > 47
and lcolor2.reflected_light_intensity < 23):
switch1 = False
switch2 = True
lightvalue1 = lcolor1.reflected_light_intensity
error = lightvalue1 - offset
integral = integral + error
dervitave = error - lastError
turn = Kp * error + Ki * integral + Kd * dervitave
turn = turn / 100
powerA = Tp + turn
powerC = Tp - turn
ml.on(SpeedPercent(powerA))
mr.on(SpeedPercent(powerC))
lastError = error
if (lcolor1.reflected_light_intensity > 47
and lcolor2.reflected_light_intensity < 23):
switch1 = False
switch2 = True
def __init__(self):
self.btn = Button()
self.LLight = LightSensor(INPUT_1)
self.RLight = LightSensor(INPUT_4)
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
os.system('setfont Lat15-TerminusBold14')
thread = threading.Thread(target=self.run, args=())
thread.daemon = True
thread.start()
def __init__(self, *args_module_name):
super().__init__(*args_module_name)
self.m_left = OUTPUT_B # Motor left
self.m_right = OUTPUT_A # Motor right
self.s_us = INPUT_1 # Sensor Ultrasonic
self.sl_left = INPUT_3 # Sensor Light left
self.sl_right = INPUT_4 # Sensor Right left
self.mMT = MoveTank(self.m_left, self.m_right) # move with 2 motors
self.mMS = MoveSteering(self.m_left, self.m_right) # move on position
self.sUS = UltrasonicSensor(self.s_us)
self.sLS_left = LightSensor(INPUT_3)
self.sLS_right = LightSensor(INPUT_4)
self.thread_detect_danger = ControlThread()
self.thread_detect_light_intesity = ControlThread()
self.stop_detect_light_intesity = False
def __init__(self):
self.btn = Button()
self.LLight = LightSensor(INPUT_1)
self.RLight = LightSensor(INPUT_4)
self.cs = ColorSensor()
self.drive = MoveTank(OUTPUT_A, OUTPUT_D)
self.steer = MoveSteering(OUTPUT_A, OUTPUT_D)
self.heightmotor = LargeMotor(OUTPUT_B)
self.clawactuator = MediumMotor(OUTPUT_C)
os.system('setfont Lat15-TerminusBold14')
self.speed = 40
self.sectionCache = 0
self.orient = {'N': "1", 'E': "1", 'S': "1", 'W': "1"}
robotDir = 2
turnLeftIntersection()
#Go straight
elif path[i] - path[i + 1] == 1:
goStraightIntersection()
#Turn 180°
elif path[i] - path[i + 1] == -1:
robotDir = 1
AroundIntersection()
i = i + 1
if __name__ == '__main__':
# Connect light sensor to input 1 and 4
lsWh = LightSensor('in4')
lsBl = LightSensor('in1')
lsM = LightSensor('in2')
lsT = TouchSensor('in3')
mBl = LargeMotor('outB')
mWh = LargeMotor('outC')
# Put the Mode_reflect to "Reflect"
lsWh.MODE_REFLECT = 'REFLECT'
lsBl.MODE_REFLECT = 'REFLECT'
lsM.MODE_REFLECT = 'REFLECT'
StartTime = time()
OurTime = 0
i = 0
robotPos = 12
import time
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
from ev3dev2.sensor.lego import ColorSensor, LightSensor
'''Kp = 520
Ki = 0
Kd = 0
offset = 20
Tp = 30
integral = 0
lastError = 0
dervitave = 0'''
lcolor1 = LightSensor(INPUT_2)
lcolor2 = LightSensor(INPUT_3)
mr = LargeMotor(OUTPUT_A)
ml = LargeMotor(OUTPUT_B)
switch1 = True
switch2 = True
a = 3
def kalibrierung(a, l1, l2):
return (l2 - l1) * a
d = kalibrierung(a, lcolor1.reflected_light_intensity,
lcolor2.reflected_light_intensity)
#!/usr/bin/env python3
import time
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
from ev3dev2.sensor.lego import ColorSensor, LightSensor
line_black = False
mcolor = ColorSensor(INPUT_1)
lcolor = LightSensor(INPUT_2)
rcolor = LightSensor(INPUT_3)
mr = LargeMotor(OUTPUT_A)
ml = LargeMotor(OUTPUT_B)
if (mcolor.color == 1 and sensorValue(lcolor) < 45 and rcolor.color == 1):
line_black = True
drive()
if (line_black == False):
test()
def drive():
while (line_black):
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
tank_drive.on_for_rotations(SpeedPercent(80), SpeedPercent(80), 10)
if (lcolor.color != 1 or mcolor.color != 1 or rcolor.color != 1):
line_black == False
if (line_black == False):
test()
def test():
#import main from timeit import default_timer as timer from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, MoveTank, SpeedPercent, MoveSteering from ev3dev2.sensor.lego import ColorSensor, LightSensor from ev3dev2.sensor import INPUT_4, INPUT_1, INPUT_2 from ev3dev2.sound import Sound ############################################### ## I/O DEFINING ## ############################################### # Color sensor i_cs_r = ColorSensor(INPUT_4) i_cs_l = ColorSensor(INPUT_1) # Light sensor i_ls = LightSensor(INPUT_2) # Large motor o_wheel_l = LargeMotor(OUTPUT_A) o_wheel_r = LargeMotor(OUTPUT_B) o_both_wheel = MoveTank(OUTPUT_A, OUTPUT_B) o_both_steering = MoveSteering(OUTPUT_A, OUTPUT_B) #o_lift = MediumMotor(OUTPUT_B) ############################################### ## GLOBAL VARIABLES ## ############################################### # Follow line CS_SCALE = 1 # OPTIMAL SETTING 2 CS_BIAS = 20
#from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, MoveSteering
from ev3dev2.sensor.lego import LightSensor
#from ev3dev2.sensor import INPUT_4, INPUT_1, INPUT_2
from ev3fast import *
from ev3dev2.motor import MoveTank
from collections import deque
###############################################
## I/O DEFINING ##
###############################################
# Color sensor
i_cs_r = ColorSensor('in4')
i_cs_l = ColorSensor('in1')
# Light sensor
i_ls = LightSensor('in2')
# Large motor
o_wheel_l = LargeMotor('outA')
o_wheel_r = LargeMotor('outB')
o_both_wheel = MoveTank('outA', 'outB')
o_both_steering = MoveSteering('outA', 'outB')
###############################################
## GLOBAL VARIABLES ##
###############################################
# Follow line
# CS_SCALE = 1 # OPTIMAL SETTING 1.1
# CS_BIAS = 10
SPEED_REDUCTION = 45
MAX_SPEED = 90
import time, tty, sys
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import LightSensor
p1 = LegoPort(INPUT_1)
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/brickpi3.html#brickpi3-in-port-modes
p1.mode = 'nxt-analog'
# http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensors.html
p1.set_device = 'lego-nxt-light'
# Connect infrared to any sensor port
ls = LightSensor(INPUT_1)
# allow for some time to load the new drivers
time.sleep(0.5)
lsAmbVal = 0
prev_lsAmbVal = 0
lsReflVal = 0
prev_lsReflVal = 0
print(
"#######################################################################")
print(
"################# light sensor ambient mode #########################")
print(
from ev3dev2.sensor.lego import LightSensor from ev3dev2.sensor.lego import GyroSensor from ev3dev2.motor import Motor from ev3dev2.motor import MoveTank, follow_for_ms, follow_for_forever from ev3dev2.motor import SpeedPercent, speed_to_speedvalue, SpeedNativeUnits from ev3dev2.wheel import EV3Tire from aibot.constants import * # ---------------------------------------------------------------------- # sensor objects cs_l = ColorSensor(ADDR_CS_L) cs_r = ColorSensor(ADDR_CS_R) ls_f = LightSensor(ADDR_LS_F) gs = GyroSensor(ADDR_GS) # motor objects mot_r = Motor(ADDR_MOT_R) mot_l = Motor(ADDR_MOT_L) motors = MoveTank(ADDR_MOT_L, ADDR_MOT_R) # set object of motors motors.gyro = gs motors.cs_r = cs_r motors.cs_l = cs_l motors.ls_f = ls_f # ----------------------------------------------------------------------
from ev3dev2.led import Leds
os.system('setfont Lat15-TerminusBold14')
ultra = UltrasonicSensor(INPUT_1)
# mLT = LargeMotor(OUTPUT_A) #Silnik lewy tylny
# mLP = LargeMotor(OUTPUT_B) #Silnik lewy przedni
# mRT = LargeMotor(OUTPUT_C) #Silnik prawy tylny
# mRP = LargeMotor(OUTPUT_D) #Silnik prawy przedni
ramie = LargeMotor(OUTPUT_A);
napedLewy = LargeMotor(OUTPUT_C);
napedPrawy = LargeMotor(OUTPUT_D);
light = LightSensor(INPUT_2) #Czujnik koloru
leds = Leds()
button = Button()
def Rotate(POWER, TIME=.250):
mLT.run_forever(speed_sp=-POWER)
mLP.run_forever(speed_sp=(-POWER/1.667))
mRT.run_forever(speed_sp=POWER)
mRP.run_forever(speed_sp=(POWER/1.667))
sleep(TIME)
mLP.stop()
mLT.stop()
mRT.stop()
mRP.stop()
################################### ### load probot_empty.ttt scene ### ################################### from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C from ev3dev2.sensor.lego import LightSensor, GyroSensor from time import sleep, time tank_pair = MoveTank(OUTPUT_B, OUTPUT_C) light = LightSensor() intensity = light.reflected_light_intensity print(intensity, end=', ') g = GyroSensor() angle = g.value() print(angle) # tank_pair.on(20, -20) tank_pair.on(120, 120) # tank_pair.on_for_rotations(20, -20, 1) # # t = time() # # while time() - t < 1: # intensity = light.reflected_light_intensity # print(intensity, end=', ') # angle = g.value() # print(angle) # # tank_pair.off()
#!/usr/bin/env python3
from ev3dev.ev3 import *
from ev3dev2.sensor.lego import LightSensor
from time import sleep
# Connect light sensor to input 1 and 4
lsWh = LightSensor('in1')
lsBl = LightSensor('in4')
lsM = LightSensor('in2')
mB = LargeMotor('outB')
mC = LargeMotor('outC')
# Put the Mode_reflect to "Reflect"
lsWh.MODE_REFLECT = 'REFLECT'
lsBl.MODE_REFLECT = 'REFLECT'
Loop = 10000
for a in range(0, Loop):
valueWh = lsWh.value()
valueBl = lsBl.value()
valueM = lsM.value()
#mB.run_forever(speed_sp= 200)
#mC.run_forever(speed_sp= 200)
#if valueWh < 480 :
# mB.run_forever(speed_sp= -150)
