print('Initializing')
from time import sleep, time
from crossDetection import crossDetection
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, SpeedPercent, LargeMotor, SpeedRPM
from ev3dev2.sensor import INPUT_4, INPUT_2, INPUT_3
from ev3dev2.sensor.lego import ColorSensor, TouchSensor
from ev3dev2.wheel import EV3Tire
from params import *
from planner import Plan
from robot import Robot
# Loading motor and sensor objects
# MoveDifferential(left_motor_port, right_motor_port, wheel_class, wheel_distance_mm)
leftMotor = LargeMotor(OUTPUT_B)
rightMotor = LargeMotor(OUTPUT_A)
leftMotor.command = leftMotor.COMMAND_RUN_DIRECT
rightMotor.command = rightMotor.COMMAND_RUN_DIRECT
colSFollower = ColorSensor(INPUT_4)
colSCrossDetect = ColorSensor(INPUT_2)
stopButton = TouchSensor(INPUT_3)
robot = Robot(leftMotor, rightMotor, colSFollower, colSCrossDetect, baseSpeed)
move_times = {'f': [], 'b': [], 'l': [], 'r': [], 'p': []}
action = None
t = 0.0
# Initializing variables and behavior objects
state = lineFollowing
plan = Plan(planstring)
import time # TODO: implement fast read/write from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_D from ev3dev2.sensor.lego import GyroSensor, TouchSensor touch = TouchSensor() gyro = GyroSensor() gyro.mode = gyro.MODE_GYRO_G_A offset = 0 left = LargeMotor(OUTPUT_D) left.duty_cycle_sp = 0 left.command = left.COMMAND_RUN_DIRECT right = LargeMotor(OUTPUT_A) right.duty_cycle_sp = 0 right.command = right.COMMAND_RUN_DIRECT alpha = 0.8 dc_gain = 1.0 p_gain = 1 i_gain = 0.029 d_gain = 0.049 #dt = 30 / 1000 dt = 250 / 1000 def angle():
