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():