OUTPUT_A, Sound, PowerSupply from time import time, sleep from math import pi, copysign N = 1000 file_name = "data_pc.txt" Kp = 20 pwr = 0 motor = LargeMotor(OUTPUT_A) battary = PowerSupply() Sound().beep() fout = open(file_name, "w") sleep(0.05) motor.reset() print(motor.position) start_time = time() for i in range(0, N): t = time() - start_time rotation = motor.position e = (360 - rotation) * pi / 180 pwr = Kp * e pwr = pwr / battary.measured_volts * 100 if abs(pwr) > 100: pwr = copysign(1, pwr) * 100 motor.run_direct(duty_cycle_sp=pwr) line = "%f\t%d\n" % (t, rotation) fout.write(line) fout.close()
class LegoCar: # Parameters of the car HARD_MAX_PHI = 0.61 SOFT_MAX_PHI = 0.3 R = 0.0432 / 2 # wheel radius L = 0.165 # wheelbase def __init__(self, init_pose, soc=None, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__velocity_controller = VelocityController(self, 0, 0, adaptation=False) self.s = soc # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__path_controller = PathController() self.__localization = Localization(self, init_pose) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Ready').wait() def reset(self): # reset encoders of motors self.__motor_rear.reset() self.__motor_steer.reset() # reset gyro sensor. The robot needs to be perfectly still! self.__sensor_gyro.mode = self.__sensor_gyro.modes[1] self.__sensor_gyro.mode = self.__sensor_gyro.modes[0] def getDevice(self, what): if what in ['MOTORS']: return self.__motor_rear, self.__motor_steer elif what in ['GYRO']: return self.__sensor_gyro def getState(self): return self.__robot_state def turnWheel(self, phi_desired): """ Turns front wheels on the desired angle 'phi_desired' """ pid_phi = PID(100.0, 500.0, 5.0, 100, -100) t = 0 clock = Clock() while t < 1: # FIXME: seems that need steady state condition t, dt = clock.getTandDT() phi_current = self.__motor_steer.position error_phi = radians(phi_desired - phi_current) u_phi = pid_phi.getControl(error_phi, dt) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) Sound.speak('Wheels were turned!') def velocity_move(self, vel_linear, vel_angular, time): # initialization for current mode self.__velocity_controller.setTargetVelocities(vel_linear, vel_angular) clock = Clock() fh = open("vel.txt", "w") while clock.getCurrentTime() <= time: try: t, dt = clock.getTandDT() theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle ] # returns ANGLE AND RATE x, y, dx, dy, v_r = self.__localization.getData( radians(theta), radians(self.__motor_rear.speed), dt) self.__robot_state = [x, y, dx, dy, theta, omega] # update state u_v, u_phi = self.__velocity_controller.getControls( radians(self.__motor_rear.speed), radians(self.__motor_steer.position), radians(omega), dt) fh.write("%f %f %f \n" % (t, v_r, radians(omega))) self.__motor_rear.run_direct(duty_cycle_sp=u_v) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) if self.__button_halt.enter: break except KeyboardInterrupt: break # off motors fh.close() self.__motor_rear.duty_cycle_sp = 0 self.__motor_steer.duty_cycle_sp = 0 # raise SystemExit def trajectory_move(self, trajectory): clock = Clock() fh = open("test.txt", "w") while not trajectory.is_end: try: t, dt = clock.getTandDT() theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle ] # !!! returns ANGLE AND RATE :) x, y, dx, dy, v_r, beta = self.__localization.getData( radians(theta), radians(self.__motor_rear.speed), dt) self.__robot_state = [x, y, dx, dy, theta, omega] # update state point = trajectory.getCoordinatesTime(t) v_des, omega_des = self.__trajectory_controller.getControls( point, x, y, dx, dy, beta, dt) self.__velocity_controller.setTargetVelocities( v_des, omega_des) u_v, u_phi = self.__velocity_controller.getControls( radians(self.__motor_rear.speed), radians(self.__motor_steer.position), radians(omega), dt) fh.write( "%f %f %f %f %f %f %f %f %f %f %f\n" % (t, x, y, point.x, point.y, v_r, v_des, theta, radians(omega), omega_des, self.__motor_steer.position)) self.__motor_rear.run_direct(duty_cycle_sp=u_v) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) #print(trajectory.is_end) if self.__button_halt.enter: break except KeyboardInterrupt: break # off motors fh.close() self.__motor_rear.duty_cycle_sp = 0 self.__motor_steer.duty_cycle_sp = 0 # raise SystemExit def path_move(self, trajectory, v_des=0.2): fh = open("test.txt", "w") clock = Clock() while not trajectory.is_end: try: t, dt = clock.getTandDT() theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle ] # !!! returns ANGLE AND RATE :) x, y, dx, dy, v_r = self.__localization.getData( radians(theta), radians(self.__motor_rear.speed), dt) self.__robot_state = [x, y, dx, dy, theta, omega] # update state x_ref, y_ref, kappa, t_hat = trajectory.getCoordinatesDistance( x, y) omega_des = self.__path_controller.getControls( v_r, x, y, x_ref, y_ref, radians(theta), kappa, t_hat) self.__velocity_controller.setTargetVelocities( v_des, omega_des) u_v, u_phi = self.__velocity_controller.getControls( radians(self.__motor_rear.speed), radians(self.__motor_steer.position), radians(omega), dt) fh.write("%f %f %f %f %f\n" % (t, x, y, x_ref, y_ref)) self.__motor_rear.run_direct(duty_cycle_sp=u_v) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) if self.__button_halt.enter: break except KeyboardInterrupt: break # off motors self.__motor_rear.duty_cycle_sp = 0 self.__motor_steer.duty_cycle_sp = 0
#!/usr/bin/python from ev3dev.ev3 import LargeMotor, Sound, OUTPUT_A from time import sleep, time from math import sin, pi N = 1000 file_name = "data_sin.txt" motor = LargeMotor(OUTPUT_A) Sound().beep("-f 440 -l 100") fout = open(file_name, "w") sleep(0.05) motor.reset() start_time = time() for i in range(0, N): t = (time() - start_time) * 1000 u_float = 100 * sin(4 * pi * t / N) rotation = motor.position motor.run_direct(duty_cycle_sp=u_float) s = "%f\t%d\n" % (t, rotation) fout.write(s) sleep(0.001) fout.close()
class LegoCar: # Parameters of the car HARD_MAX_PHI = 0.61 SOFT_MAX_PHI = 0.3 R = 0.0432 / 2 # wheel radius L = 0.21 - R # wheelbase D = 0.112 # distance between the axes of rotation of the front wheels # Ultrasonic Sensors positions respect of the middle of the rear axle US_FRONT = (L, 0.06) US_REAR = (-0.015, 0.06) def __init__(self, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1', port_sensor_us_front='in2', port_sensor_us_rear='in3'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__sensor_us_rear = UltrasonicSensor(port_sensor_us_rear) self.__sensor_us_front = UltrasonicSensor(port_sensor_us_front) self.__velocity_controller = VelocityController(self, 0, 0) # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__localization = Localization(self) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Initialization complete!').wait() def reset(self): # reset encoders of motors self.__motor_rear.reset() self.__motor_steer.reset() # reset gyro sensor. The robot needs to be perfectly still! self.__sensor_gyro.mode = self.__sensor_gyro.modes[1] self.__sensor_gyro.mode = self.__sensor_gyro.modes[0] def getDevice(self, what): if what in ['US_SENSORS']: return self.__sensor_us_front, self.__sensor_us_rear elif what in ['MOTORS']: return self.__motor_rear, self.__motor_steer elif what in ['GYRO']: return self.__sensor_gyro def getState(self): return self.__robot_state def turnWheel(self, phi_desired): """ Turned front wheels on the desired angle 'phi_desired '""" pid_phi = PID(100.0, 500.0, 5.0, 100, -100) t = 0 clock = Clock() while t < 1: # FIXME: seems that need steady state condition t, dt = clock.getTandDT() phi_current = self.__motor_steer.position error_phi = radians(phi_desired - phi_current) u_phi = pid_phi.getControl(error_phi, dt) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) Sound.speak('Wheels were turned!') def move(self, vel_linear=None, vel_angular=None, trajectory=None, mapping=None): """ Examples: mode_1. move(trajectory=given_trajectory) --- following given trajectory; mode_2. move(vel_linear=0.3, vel_angular=0.3) --- moving with given velocities; mode_3. move(`mode_1 or mode_2`, mapping=mapping) --- moving and mapping. """ # initialization for current mode if vel_linear and vel_angular not in [None]: self.__velocity_controller.setTargetVelocities( vel_linear, vel_angular) clock = Clock() while True: try: t, dt = clock.getTandDT() theta, omega = [-x for x in self.__sensor_gyro.rate_and_angle ] # !!! returns ANGLE AND RATE :) x, y, dx, dy = self.__localization.getData( radians(theta), radians(self.__motor_rear.speed), dt) self.__robot_state = [x, y, dx, dy, theta, omega] # update state if trajectory is not None: point = trajectory.getCoordinates(t) v_des, omega_des = self.__trajectory_controller.getControls( point, x, y, dx, dy, radians(theta), dt) self.__velocity_controller.setTargetVelocities( v_des, omega_des) u_v, u_phi = self.__velocity_controller.getControls( radians(self.__motor_rear.speed), radians(self.__motor_steer.position), radians(omega), dt) if mapping is not None: mapping.updateMap(x, y, radians(theta)) self.__motor_rear.run_direct(duty_cycle_sp=u_v) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) if self.__button_halt.enter: break except KeyboardInterrupt: break # off motors self.__motor_rear.duty_cycle_sp = 0 self.__motor_steer.duty_cycle_sp = 0 raise SystemExit