def motor(test_method):
if test_method == "move_to_encoder_pose_with_dur":
target_time = 1.2
target_dist = -500
encoder = RotaryEncoder(pigpio.pi(), ENCODER_B_PLUS_PIN,
ENCODER_A_PLUS_PIN)
motor = Motor(encoder)
def go_to_target():
start = time.time()
result = False
while result is not True:
result, val = motor.move_to_encoder_pose_with_dur(
target_pose, target_dist, target_time)
end = time.time()
motor.stop()
print("Target time: " + str(target_time))
print("Elapsed Time: " + str(end - start))
target_pose = motor.encoder_position() + target_dist
go_to_target()
target_pose = motor.encoder_position() - target_dist
go_to_target()
def config_speed_pwm_ratio(source, port, real_world, debug):
"""
Determines the ratio between pwm and rotation speed in inc/s
"""
clear_cmd()
print('INCREMENTAL ENCODER UTILITY')
print(
'Place the slider next to the motor (which is the origin position and) \
press enter'
)
input()
arduino_console = serial.Serial(port, 230400, timeout=1, write_timeout=2)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(1)
EXP_DURATION = 1 # s
SPEED_VALUE = 255 # pwm
speeds = []
times = []
t_start = time.clock()
t = t_start
while t - t_start < EXP_DURATION:
motor.speed = SPEED_VALUE
times.append(t - t_start)
speeds.append(motor.speed)
t = time.clock()
motor.speed = 0
response_t, lower, upper = response_time(
times,
speeds,
percentage=0,
nb_points_mean=50
)
print('ratio = {} pwm/(inc/s)'.format(255/lower))
real_world.pwm_incs_ratio = 255/lower
real_world.save()
def __init__(self):
# start logger
logger = LoggerInit("ventilator")
# instantiate sensors
self.encoder = RotaryEncoder(pigpio.pi(), ENCODER_B_PLUS_PIN, ENCODER_A_PLUS_PIN)
self.contact_switch = LimitSwitch(CONTACT_SWITCH_PIN)
self.absolute_switch = LimitSwitch(ABSOLUTE_SWITCH_PIN)
self.power_switch = PowerSwitch(POWER_SWITCH_PIN)
# self.pressure_sensor = PressureSensor()
# self.flow_sensor = FlowSensor(FLOW_SENSOR_PIN)
# instantiate actuators
self.motor = Motor(self.encoder)
# instantiate controller
self.controller = VentilatorController(self.motor,
None,
self.absolute_switch,
self.contact_switch,
self.power_switch)
# instantiate ui
self.ui = UI()
self.ui_controller_interface = UIControllerInterface(self.ui, self.controller)
def closed_loop_pos_step_input(source, port, real_world, debug):
arduino_console = serial.Serial(port, 230400, timeout=1, write_timeout=2)
experiment = Experiment.new(real_world.data_folder)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(1)
context = ask_context()
experiment.add_data(['context'], context)
EXP_DURATION = 3 # s
POS_VALUE = int((real_world.rail_length/2)*real_world.inc_m_ratio) # inc
positions = []
speeds = []
times = []
t_start = time.clock()
t = t_start
while t - t_start < EXP_DURATION:
motor.position = POS_VALUE
times.append(t - t_start)
positions.append(motor.position)
speeds.append(motor.speed)
t = time.clock()
motor.speed = 0
experiment.add_data(
['entree_echelon'],
{
'name': 'Réponse temporelle du système en boucle fermée à une entrée en echelon de position',
'pos_order': POS_VALUE,
'times': times,
'speeds': speeds,
'positions': positions
}
)
experiment.save()
pl.plot(times, positions)
pl.legend(['position'])
pl.show()
def open_loop_step_input(source, port, real_world, debug):
arduino_console = serial.Serial(port, 230400, timeout=1, write_timeout=2)
experiment = Experiment.new(real_world.data_folder)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(1)
context = ask_context()
experiment.add_data(['context'], context)
EXP_DURATION = 1.7 # s
SPEED_VALUE = 255 # pwm
positions = []
speeds = []
times = []
t_start = time.clock()
t = t_start
while t - t_start < EXP_DURATION:
motor.speed = SPEED_VALUE
times.append(t - t_start)
positions.append(motor.position)
speeds.append(motor.speed)
t = time.clock()
motor.speed = 0
experiment.add_data(
['entree_echelon'],
{
'name': 'Réponse temporelle du système en boucle ouverte à une entrée en echelon',
'speed_order': SPEED_VALUE,
'times': times,
'speeds': speeds,
'positions': positions
}
)
experiment.save()
def __init__(self):
gpio = pigpio.pi()
self.Color = Color(7)
self.Distance = [Distance(2), Distance(3), Distance(0), Distance(1)]
self.Gyroscope = Gyroscope('/dev/ttyAMA0')
self.Temperature = [Temperature(i) for i in range(85, 89)]
self.Camera = Camera()
self.LedRed = Led(gpio, 20)
self.LedYellow = Led(gpio, 21)
self.LedRed.off()
self.LedYellow.off()
self.ButtonStart = Button(gpio, 9)
self.ButtonStop = Button(gpio, 10)
self.Motor = [Motor(19, 11, 26), Motor(13, 6, 5)]
self.Servo = Servo(gpio, 12)
self.Sensors = [
self.Color, self.Distance, self.Gyroscope, self.Temperature
]
self.Actuators = [self.LedRed, self.Servo]
self.Maze = Maze(self.Sensors, self.Actuators, self.Camera)
self.ActualX = self.Maze.StartX
self.ActualY = self.Maze.StartY
self.ActualZ = self.Maze.StartZ
self.StartTime = 0
self.StartHeading = 0
self._stop()
def __init__(self):
self.Serial = ArduinoSerial('/dev/ttyACM0', 115200) # TODO: Riconoscere seriale
self.Color = Color(self.Serial)
self.Distance = [Distance(self.Serial, i) for i in range(1, 4)]
self.Gyroscope = Gyroscope(self.Serial)
self.Temperature = [Temperature(self.Serial, i) for i in range(1, 4)]
self.Camera = Camera()
self.Motor = Motor(self.Serial)
self.Servo = Servo(self.Serial)
self.Sensors = [self.Color, self.Distance, self.Gyroscope, self.Temperature]
self.Maze = Maze(self.Sensors, self.Servo, self.Camera)
self.ActualX = self.Maze.StartX
self.ActualY = self.Maze.StartY
self.ActualZ = self.Maze.StartZ
self.StartTime = 0
self.StartHeading = 0
self.stop()
def config_inc_distance_ratio(source, port, real_world, debug):
"""
Determines the ratio between distance in m and distance in inc
"""
clear_cmd()
print('INCREMENTAL ENCODER UTILITY')
print(
'Place the slider next to the motor (which is the origin position), \
TURN OFF THE POWER SOURCE OF THE CHOPPER CONTROLLER then press enter'
)
input()
print()
arduino_console = serial.Serial(port, 230400, timeout=1, write_timeout=2)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(1)
print('[INITIAL POSITION] ', motor.position)
print(
'Now, move the basket to the furthest position possible and press enter'
)
input('Ready ?')
while 1:
print(motor.position)
if msvcrt.kbhit():
break
end_position = motor.position
print('[END POSITION] ', end_position)
ratio = abs(end_position)/real_world.rail_length
print('[RATIO] ', ratio)
real_world.inc_m_ratio = ratio
real_world.save()
def main(source, port, real_world, debug):
############################
# MOTOR INITIALISATION #
############################
arduino_console = serial.Serial(port, 230400, timeout=0.01)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(3)
###########################
# CAMERA INITIALISATION #
###########################
color_range = real_world.color_range
ball = Ball(source, real_world.color_range, max_retries=100, debug=debug)
ball.start_positionning()
rail_origin = real_world.dist_origin_rails
px_m_ratio = real_world.px_m_ratio
inc_m_ratio = real_world.inc_m_ratio
###################
# BALL TRACKING #
###################
i = 0
positions = []
models = []
x_falls = []
t = time.clock()
bgr = cv2.cvtColor(ball._last_frame, cv2.COLOR_RGB2BGR)
#fourcc = cv2.VideoWriter_fourcc(*'MP4V')
#out = cv2.VideoWriter('test.mp4', fourcc, 20.0, (len(bgr), len(bgr[0])))
# Wait for the ball to appear
while not ball.is_in_range:
dt = time.clock() - t
t = time.clock()
bgr = cv2.cvtColor(ball._last_frame, cv2.COLOR_RGB2BGR)
#out.write(ball._last_frame)
display_info(bgr, str(1/dt), color_range)
cv2.imshow('camera', bgr)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
#out.release()
positions.append(ball.position)
while ball.is_in_range:
dt = time.clock() - t
t = time.clock()
positions.append(ball.position)
f = free_fall(
[positions[-1], positions[-2]],
real_world.px_m_ratio
)
models.append(f)
path = Path(f)
x_fall = path.falling_point(ball.window, ball.window['width']//2)
x_falls.append((x_fall, time.clock()))
if rail_origin - x_fall >= 0:
motor.position = int((inc_m_ratio/px_m_ratio)*(rail_origin - x_fall))
print('FOUND')
else:
print('NO')
frame = ball._last_frame
bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
fps = str(1/dt)
display_info(bgr, fps, color_range)
cv2.imshow('camera', bgr)
cv2.imshow('mask', ball._mask)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
height = len(bgr)
width = len(bgr[0])
##########################
# TEMP: DRAW POSITIONS #
##########################
print("Number of positions:", len(positions))
print(positions)
positions = array(positions)
X = positions[:,0]
Y = positions[:,1]
# for point in x_falls:
# pl.plot(point[0], 0, 'ro')
for model in models:
pl.plot(X, [model(x) for x in X])
pl.plot(X, Y)
#pl.plot(X, [f(x) for x in X])
pl.legend(['position']+[str(i) for i in range(len(models))])
pl.show()
# while 1:
# pos = ball.position
# print(pos)
# # WARNING ! DO NOT DELETE
# if cv2.waitKey(1) & 0xFF == ord('q'):
# break
ball.stop_positionning()
#!/usr/bin/python
#
# TicDevice and PID test
# VentCU - An open source ventilator
#
# (c) VentCU, 2020. All Rights Reserved.
#
import matplotlib.pyplot as plt
from time import sleep
from actuators.motor import Motor
import pigpio
from sensors.rotary_encoder import RotaryEncoder
import numpy as np
motor = Motor(RotaryEncoder(pigpio.pi(), 18, 16))
encoder_u_limit = 400
encoder_l_limit = 0
# initialize the goal of the motor
goal = encoder_u_limit
i = 0
if __name__ == "__main__":
while i < 10:
result, vel = motor.move_to_encoder_pose(goal)
raise TypeError("window_size is too small for the polynomials order")
order_range = range(order + 1)
half_window = (window_size - 1) // 2
# precompute coefficients
b = np.mat([[k**i for i in order_range]
for k in range(-half_window, half_window + 1)])
m = np.linalg.pinv(b).A[deriv] * rate**deriv * factorial(deriv)
# pad the signal at the extremes with
# values taken from the signal itself
firstvals = y[0] - np.abs(y[1:half_window + 1][::-1] - y[0])
lastvals = y[-1] + np.abs(y[-half_window - 1:-1][::-1] - y[-1])
y = np.concatenate((firstvals, y, lastvals))
return np.convolve(m[::-1], y, mode='valid')
motor = Motor(RotaryEncoder(pigpio.pi(), 18, 16))
encoder_u_limit = 400
encoder_l_limit = 0
vel_arr = []
en_pose_arr = []
tic_pose_arr = []
x = []
zeros = []
setpoints_en = []
setpoints_tic = []
i = 0
# initialize the goal of the motor
goal = encoder_u_limit
def open_loop_sine_input(source, port, real_world, debug):
arduino_console = serial.Serial(port, 230400, timeout=1, write_timeout=2)
experiment = Experiment.new(real_world.data_folder)
motor = Motor(
arduino_console,
debug=debug
)
# avoids some bugs with serial
time.sleep(1)
context = ask_context()
experiment.add_data(['context'], context)
print('Move the basket next to the motor (the origin)')
basket = int(input('Basket mounted ? 0: No, 1: Yes '))
############
# MEASURES #
############
START_FREQ = 0.05 # Hz
END_FREQ = 50 # Hz
NB_POINTS = 25
SINE_AMPLITUDE = 200 # pwm
EXP_DURATION = 15 # s
CENTER = int(
(real_world.rail_length/2)*real_world.inc_m_ratio
)# inc
if basket:
motor.position = CENTER
time.sleep(2)
exp_freqs = np.geomspace(
START_FREQ,
END_FREQ,
NB_POINTS
)
results = []
for sine_freq in exp_freqs:
if basket:
motor.position = CENTER
time.sleep(5)
sine_freq += 0.1
speed_measures = []
pos_measures = []
consignes = []
times = []
t_start = time.clock()
t = t_start
while t - t_start < EXP_DURATION:
try:
speed_measures.append(motor.speed)
except:
t = time.clock()
continue
try:
pos_measures.append(motor.position)
except:
speed_measures.pop()
t = time.clock()
continue
times.append(t - t_start)
order = int(SINE_AMPLITUDE*np.sin(2*np.pi*sine_freq*(t - t_start)))
# limit the amplitude of the movement
if basket and (pos_measures[-1] <= 1500 and order <= 0):
motor.speed = 0
consignes.append(0)
elif basket and (pos_measures[-1] >= 2*CENTER - 1500 and order >= 0):
motor.speed = 0
consignes.append(0)
else:
motor.speed = order
consignes.append(order)
t = time.clock()
motor.speed = 0
# pos_measures = np.array(pos_measures)
# pos_spectrum = np.fft.rfft(pos_measures)
# speed_measures = np.array(speed_measures)
# speed_spectrum = np.fft.rfft(speed_measures)
results.append({
"order_freq": sine_freq,
"order_amplitude": SINE_AMPLITUDE,
"times": times,
"speed_orders": consignes,
"speed_measures": speed_measures,
"pos_measures": pos_measures
})
############
# ANALYSIS #
############
experiment.add_data(['entree_sinus'], {'measures': results})
experiment.save()
n_experiments = len(results)
# plot all the experiments measures
fig, axes = pl.subplots(n_experiments)
for i in range(n_experiments):
axes[i].plot(results[i]['times'], results[i]['pos_measures'], color='r')
axes[i].legend(['position mesurée (inc) '])
ax2 = axes[i].twinx()
ax2.plot(results[i]['times'], results[i]['speed_measures'], color='b')
ax2.legend(['vitesse mesurée (inc/ms)'])
# ax1.semilogx(
# results[:,0],
# 20*np.log10(results[:,1])
# )
# ax1.semilogx(
# results[:,0],
# results[:,2]
# )
# fig, ax1 = pl.subplots()
# ax2 = ax1.twinx()
# ax1.plot(times, pos_measures, color='r')
# ax1.plot(times, consignes)
# ax1.set_xlabel('time (s)')
# ax1.set_ylabel('position (inc)')
# ax1.legend(['position (inc)', 'consigne'], loc=1)
# ax2.plot(times, speed_measures, color='b')
# ax2.set_ylabel('speed (inc/s)')
# ax2.legend(['speed (inc/s)'], loc=4)
#pl.title('Réponse du moteur à un sinus de vitesse')
pl.show()
class Robot:
ReturnTime = 360
def __init__(self):
self.Serial = ArduinoSerial('/dev/ttyACM0', 115200) # TODO: Riconoscere seriale
self.Color = Color(self.Serial)
self.Distance = [Distance(self.Serial, i) for i in range(1, 4)]
self.Gyroscope = Gyroscope(self.Serial)
self.Temperature = [Temperature(self.Serial, i) for i in range(1, 4)]
self.Camera = Camera()
self.Motor = Motor(self.Serial)
self.Servo = Servo(self.Serial)
self.Sensors = [self.Color, self.Distance, self.Gyroscope, self.Temperature]
self.Maze = Maze(self.Sensors, self.Servo, self.Camera)
self.ActualX = self.Maze.StartX
self.ActualY = self.Maze.StartY
self.ActualZ = self.Maze.StartZ
self.StartTime = 0
self.StartHeading = 0
self.stop()
def start(self):
print("Robot ready")
"""
while True:
pass # TODO: Attendere pressione pulsante
"""
self.StartTime = time.time()
print("Start Time = " + str(self.StartTime))
returnList = [-1, -1]
movementCounter = 0
while True:
area = self.Maze.Areas[self.ActualX][self.ActualY][self.ActualZ]
if not area.Scanned:
area.scan()
if self.Gyroscope.getPitch() == 0:
for i in range(4):
if area.Walls[i]:
self.turn(i)
area.findVisualVictim(i)
area.Victims[i].save()
if movementCounter > 10:
orientation = self.Gyroscope.getOrientation()
if orientation == 0:
orientation = 2
elif orientation == 1:
orientation = 3
elif orientation == 2:
orientation = 0
elif orientation == 3:
orientation = 1
if area.Walls[orientation] and not area.Victims[orientation].Present:
self.reset()
movementCounter = 0
tmp = self.move(self.Maze.findPath(self.ActualX, self.ActualY, self.ActualZ))
returnList.append(tmp)
if tmp == 8:
print("Stop Pressed")
self.stop()
"""
while True:
if self.ButtonStart.pressed(): # TODO: Attendere pressione pulsante
self.StartHeading = self.Gyroscope.getHeading()
break
"""
else:
print("Moved to X = " + str(self.ActualX) + " Y = " + str(self.ActualY) + " Z = " + str(self.ActualZ))
movementCounter += 1
if returnList[-1] == 9 and returnList[-2] == 9:
self.turnRight()
if time.time() - self.StartTime > self.ReturnTime:
break
print("Searching for a returning home path")
movementList = self.Maze.findReturnPath(self.ActualX, self.ActualY, self.ActualZ)
if len(movementList) > 0:
print("Returning home")
print("Movement list = " + str(movementList))
for movement in movementList:
self.move(movement)
else:
print("Path not found")
self.stop()
def move(self, direction):
"""
Move the robot to absolute direction
0 is X+
1 is Y+
2 is X-
3 is Y-
"""
tmp = 0
xy = direction
orientation = self.Gyroscope.getOrientation()
if orientation == -1:
return -1
else:
direction -= orientation
if direction == -3:
if self.turnRight() == 8:
return 8
tmp = self.moveForward()
elif direction == -2:
tmp = self.moveBackward()
elif direction == -1:
if self.turnLeft() == 8:
return 8
tmp = self.moveForward()
elif direction == 0:
tmp = self.moveForward()
elif direction == 1:
if self.turnRight() == 8:
return 8
tmp = self.moveForward()
elif direction == 2:
tmp = self.moveBackward()
elif direction == 3:
if self.turnLeft() == 8:
return 8
tmp = self.moveForward()
if tmp == 8:
return 8
elif tmp == 9:
print("Movement Timeout")
return 9
elif tmp == 4:
self.Maze.Areas[self.ActualX][self.ActualY][self.ActualZ].Ramps[xy] = True
self.ActualZ = 1 if self.ActualZ == 0 else 2
if xy == 0:
xy = 2
elif xy == 1:
xy = 3
elif xy == 2:
xy = 0
elif xy == 3:
xy = 1
self.Maze.Areas[self.ActualX][self.ActualY][self.ActualZ].Ramps[xy] = True
self.Maze.RampPassages += 1
return 4
elif tmp == 5:
self.Maze.Areas[self.ActualX][self.ActualY][self.ActualZ].Ramps[xy] = True
self.ActualZ = 0 if self.ActualZ == 1 else 1
if xy == 0:
xy = 2
elif xy == 1:
xy = 3
elif xy == 2:
xy = 0
elif xy == 3:
xy = 1
self.Maze.Areas[self.ActualX][self.ActualY][self.ActualZ].Ramps[xy] = True
self.Maze.RampPassages += 1
return 5
elif xy == 0:
if tmp == 3:
self.Maze.Areas[self.ActualX + 1][self.ActualY][self.ActualZ].Type = Area.AreaType.NoGo
else:
self.ActualX += 1
return 0
elif xy == 1:
if tmp == 3:
self.Maze.Areas[self.ActualX][self.ActualY + 1][self.ActualZ].Type = Area.AreaType.NoGo
else:
self.ActualY += 1
return 1
elif xy == 2:
if tmp == 3:
self.Maze.Areas[self.ActualX - 1][self.ActualY][self.ActualZ].Type = Area.AreaType.NoGo
else:
self.ActualX -= 1
return 2
elif xy == 3:
if tmp == 3:
self.Maze.Areas[self.ActualX][self.ActualY - 1][self.ActualZ].Type = Area.AreaType.NoGo
else:
self.ActualY -= 1
return 3
def turn(self, direction):
"""
Turn the robot to absolute direction
0 is powerOn front
1 is powerOn right
2 is powerOn back
3 is powerOn left
"""
orientation = self.Gyroscope.getOrientation()
if orientation == -1:
return -1
else:
direction -= orientation
if direction == -3:
if self.turnRight() == 8:
return 8
elif direction == -2:
if self.turnRight() == 8:
return 8
if self.turnRight() == 8:
return 8
elif direction == -1:
if self.turnLeft() == 8:
return 8
elif direction == 0:
pass
elif direction == 1:
if self.turnRight() == 8:
return 8
elif direction == 2:
if self.turnLeft() == 8:
return 8
if self.turnLeft() == 8:
return 8
elif direction == 3:
if self.turnLeft() == 8:
return 8
def moveForward(self):
return self.Motor.forward()
def turnRight(self):
return self.Motor.right()
def moveBackward(self):
return self.Motor.reverse()
def turnLeft(self):
return self.Motor.left()
def stop(self):
self.ActualX = self.Maze.LastCheckPoint[0]
self.ActualY = self.Maze.LastCheckPoint[1]
self.ActualZ = self.Maze.LastCheckPoint[2]
print("Return to X = " + str(self.ActualX) + " Y = " + str(self.ActualY) + " Z = " + str(self.ActualZ))
def reset(self):
self.Motor.reset()
# unit testing script for the ventilator
# controller class
#
import pigpio
from configs.gpio_map import *
from actuators.motor import Motor
from sensors.rotary_encoder import RotaryEncoder
from sensors.limit_switch import LimitSwitch
from sensors.power_switch import PowerSwitch
from ventilator_controller import VentilatorController
if __name__ == "__main__":
encoder = RotaryEncoder(pigpio.pi(), ENCODER_B_PLUS_PIN,
ENCODER_A_PLUS_PIN)
contact_switch = LimitSwitch(CONTACT_SWITCH_PIN)
absolute_switch = LimitSwitch(ABSOLUTE_SWITCH_PIN)
power_switch = PowerSwitch(POWER_SWITCH_PIN)
# instantiate actuators
motor = Motor(encoder)
# instantiate controller
controller = VentilatorController(motor, None, absolute_switch,
contact_switch, power_switch)
controller.start_homing()
print("Start ventilation...")
controller.start_ventilation()