from ev3.lego import Motor
c = Motor(port = Motor.PORT.C)
from arrays import *
while btn.any():
initialMotorLocation= c.read_value("position")
# while can not found
#code in choosing the direction
if direction == forward:
wayBack[movementNumber] = forward();
elif direction == left:
wayBack[movementNumber] = turn(right);
elif direction == right:
wayBack[movementNumber] = turn(left);
#code in moving the bot
def headingback(movementNumber,initialMotorLocation,wayBack[]):
turn(left);
turn(left);
i = movementNumber;
while c.read_value("position") != initialMotorLocation or i != 0:
wayBack[i];
i = i - 1;
value_on_red = 100.0
value_not_on_red = 35.0
target = ((value_on_red - value_not_on_red) / 2) + value_not_on_red
tp = 70 # target power (speed)
kp = 0.65 # how fast we try to correct the 'out of target'
ki = 0.05 # smooth the correction by storing historical errors
kd = 0.09 # controls the overshooting
print "target power: " + str(tp)
print "proportional constant: " + str(kp)
print "proportional range: +" + str(target) + " / -" + str(target)
color_sensor = ColorSensor()
ultra_sensor = UltrasonicSensor()
motor_left = Motor(port=Motor.PORT.A)
motor_right = Motor(port=Motor.PORT.D)
tone = Tone()
def avg(lst):
if len(lst) == 0:
return 0
return reduce(lambda x, y: x + y, lst) / len(lst)
def stop():
motor_left.stop()
motor_right.stop()
def victory():
motor_left.run_forever(-100)
motor_right.run_forever(-100)
class Sim_Sensor:
def __init__(self, port):
self.port = port
def read_value(self, value_name):
return 0
# Global varibales for the motors and sensors
if not simulation:
from ev3.lego import Motor
from ev3.lego import Msensor
sonar_sensor = Msensor(port=2)
infrared_sensor = Msensor(port=3)
color_sensor = Msensor(port=4)
forward_motor = Motor(port=Motor.PORT.C)
turn_motor = Motor(port=Motor.PORT.B)
turret_motor = Motor(port=Motor.PORT.A)
else:
forward_motor = Sim_Motor(port="C")
turn_motor = Sim_Motor(port="B")
turret_motor = Sim_Motor(port="A")
sonar_sensor = Sim_Sensor(port=2)
infrared_sensor = Sim_Sensor(port=3)
color_sensor = Sim_Sensor(port=4)
# Config values
forward_speed = 100
turn_speed = 100
turret_speed = 100
turn_max_clicks = 350
if motion_noise_on:
steering_noise = base_steering_noise
distance_noise = base_distance_noise
turning_noise = base_turning_noise
else:
steering_noise = 0.0
distance_noise = 0.0
turning_noise = 0.0
if real_robot_mode:
from ev3.lego import Motor
from ev3.lego import UltrasonicSensor
from ev3.lego import ColorSensor
from ev3.ev3dev import Tone
a = Motor(port=Motor.PORT.A) # left large motor
d = Motor(port=Motor.PORT.D) # right large motor
a.reset()
d.reset()
a.position_mode=Motor.POSITION_MODE.RELATIVE
d.position_mode=Motor.POSITION_MODE.RELATIVE
sonar_left = UltrasonicSensor(port=2)
sonar_front = UltrasonicSensor(port=3)
color_left = ColorSensor(port=1)
color_right = ColorSensor(port=4)
sound = Tone()
# Occupancy grid - checks for available positions
class Grid:
if motion_noise_on:
steering_noise = base_steering_noise
distance_noise = base_distance_noise
turning_noise = base_turning_noise
else:
steering_noise = 0.0
distance_noise = 0.0
turning_noise = 0.0
if real_robot_mode:
from ev3.lego import Motor
from ev3.lego import UltrasonicSensor
from ev3.lego import ColorSensor
from ev3.ev3dev import Tone
a = Motor(port=Motor.PORT.A) # left large motor
d = Motor(port=Motor.PORT.D) # right large motor
a.reset()
d.reset()
a.position_mode = Motor.POSITION_MODE.RELATIVE
d.position_mode = Motor.POSITION_MODE.RELATIVE
sonar_left = UltrasonicSensor(port=2)
sonar_front = UltrasonicSensor(port=3)
color_left = ColorSensor(port=1)
color_right = ColorSensor(port=4)
sound = Tone()
# Occupancy grid - checks for available positions
class Grid:
def __init__(self, rows, cols):
def run_motor(seconds):
milliseconds = seconds * 1000
print "start running motor"
d = Motor(port=Motor.PORT.A)
d.run_time_limited(time_sp=milliseconds, speed_sp=80)
print "done running motor"
import time
from ev3.lego import Motor
from ev3.lego import UltrasonicSensor
a = Motor(port=Motor.PORT.A)
d = Motor(port=Motor.PORT.D)
a.reset()
d.reset()
a.position_mode = Motor.POSITION_MODE.RELATIVE
d.position_mode = Motor.POSITION_MODE.RELATIVE
Kp = 0.03
Kd = 0.02
base_power = 30
power_multiplier = 0.20
target_wall_dist = 100
sonar_left = UltrasonicSensor(port=2)
sonar_front = UltrasonicSensor(port=3)
def sense_front():
reading1 = sonar_front.dist_in
reading2 = sonar_front.dist_in
return min(reading1, reading2)
def sense_left():
reading1 = sonar_left.dist_in
reading2 = sonar_left.dist_in
return min(reading1, reading2)