class MyMotor(object): def __init__(self, motor, speed=0): self._motor = LargeMotor(motor) assert self._motor.connected self._motor.reset() self._motor.position = 0 self._motor.stop_action = 'brake' self._set_speed(speed) @property def speed(self): return int(self._speed/10) def _set_speed(self, speed): assert speed >= -100 and speed <= 100 self._speed = speed*10 def run_forever(self, speed): if speed is not None: self._set_speed(speed) self._motor.run_forever(speed_sp=self._speed) def run_timed(self, timed, speed): if speed is not None: self._set_speed(speed) runtimed = timed * 1000 self._motor.run_timed(speed_sp=self._speed, time_sp=runtimed) def run_position(self, postion, speed, lastpostion=False): if speed is not None: self._set_speed(speed) if lastpostion: self._motor.run_to_abs_pos(speed_sp=self._speed) else: self._motor.run_to_rel_pos(speed_sp=self._speed) def stop(self): self._motor.stop() def run_back(self, speed): if not self._speed: self.stop() self._set_speed(speed) self._motor.run_forever(speed_sp=self._speed)
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()
#!/usr/bin/python from ev3dev.ev3 import MediumMotor as MediumMotor from ev3dev.ev3 import LargeMotor as LargeMotor from time import sleep a = MediumMotor(address='outA') b = LargeMotor(address='outB') c = LargeMotor(address='outC') a.reset() b.reset() c.reset() a.position_sp = 50 a.duty_cycle_sp = 50 a.command = 'run-to-abs-pos' b.position_sp = -450 b.duty_cycle_sp = 50 b.command = 'run-to-abs-pos' c.position_sp = -450 b.duty_cycle_sp = 50 b.command = 'run-to-abs-pos' sleep(5)
#!/usr/bin/python from ev3dev.ev3 import LargeMotor as LargeMotor from ev3dev.ev3 import TouchSensor as TouchSensor from time import sleep power = -100 run_time = 3 motor_b = LargeMotor(address='outB') motor_c = LargeMotor(address='outC') motor_b.reset() motor_c.reset() motor_b.command = 'run-direct' motor_c.command = 'run-direct' trigger = TouchSensor() print("Fire when ready!") while True: if trigger.value() == True: break else: sleep(.01) motor_b.duty_cycle_sp = power motor_c.duty_cycle_sp = power
#!/usr/bin/python from ev3dev.ev3 import LargeMotor as LargeMotor from ev3dev.ev3 import TouchSensor as TouchSensor from time import sleep power = -100 run_time = 3 motor_b = LargeMotor(address='outB') motor_c = LargeMotor(address='outC') motor_b.reset() motor_c.reset() motor_b.command = 'run-direct' motor_c.command = 'run-direct' trigger = TouchSensor() print("Fire when ready!") while True: if trigger.value() == True: break else: sleep(.01)
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
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