def direction_test(motors_id: tuple, consigne, retroaction):
main_id, companion_id = motors_id
dir_sign = '+'
for direction in protocol.MotorsDirection:
if direction == protocol.MotorsDirection.FORWARD:
dir_sign = '+'
else:
dir_sign = '-'
for cmd in COMMANDS:
set_consigne(cmd, companion_id, direction, main_id, direction)
begin = time.time()
id_time = time.time() - begin
last_tick = time.time()
while id_time < IDENTIFICATION_TIME:
now = time.time()
delta_t = now - last_tick
if delta_t > STEP:
last_tick = now
ser.read(ser.inWaiting())
speed = read_encoder(main_id)
if dir_sign == '+':
consigne.append(cmd)
else:
consigne.append(-cmd)
retroaction.append(speed)
print("({}) val: {}".format(delta_t, speed))
id_time = now - begin
set_consigne(0, companion_id, protocol.MotorsDirection.FORWARD, main_id,
protocol.MotorsDirection.FORWARD)
print("{}\n{}".format(consigne, retroaction))
def draw_all_motor_menu(direction, screen, speed):
screen.addstr(
0, 0, "Commande de {} avec une direction {}".format(speed, direction))
for idx, motor_id in enumerate(protocol.Motors):
time.sleep(DEFAULT_COMM_SLEEP)
motor_speed = read_encoder(motor_id, ser)
ser.read(1)
screen.addstr(idx * 2 + 1, 0, "Moteur {}: {}".format(idx, motor_speed))
screen.addstr(9, 0, "Appuyer sur 's' pour changer la vitesse.")
screen.addstr(10, 0, "Appuyer sur 'd' pour changer la direction.")
screen.addstr(11, 0, "Appuyer sur 'q' pour revenir au menu principal")
display_busy_wait(screen, 8)
screen.move(12, 0)
def motor(screen):
global directions
screen.nodelay(False)
screen.clear()
curses.echo()
screen.addstr("ID du moteur [1-4]: ")
motor_id = int(screen.getkey())
motor_id = motors_id[motor_id]
screen.addstr(1, 0, "Vitesse du moteur [0-100]: ")
try:
speed = int(screen.getstr(3))
except ValueError:
speed = DEFAULT_SPEED
screen.addstr(2, 0, "Direction [f|b]: ")
try:
dir_key = screen.getkey()
direction = directions[dir_key]
except KeyError:
direction = DEFAULT_DIRECTION
curses.noecho()
screen.clear()
screen.nodelay(True)
ser.write(
protocol.generate_manual_speed_command(motor_id, speed, direction))
sub_run = True
while sub_run:
ser.read(ser.inWaiting())
time.sleep(0.050)
screen.clear()
motor_speed = read_encoder(motor_id, ser)
draw_motor_menu(direction, motor_speed, screen, speed)
try:
user_key = screen.getkey()
except curses.error:
user_key = -1
if user_key in ['c', 'C', 'q', 'Q']:
sub_run = False
elif user_key in ['s', 'S']:
screen.nodelay(False)
screen.clear()
curses.echo()
try:
screen.move(0, 0)
speed = int(screen.getstr(3))
except ValueError:
screen.addstr(4, 0, "La vitesse doit etre un nombre valide.")
curses.noecho()
screen.nodelay(True)
elif user_key in ['d', 'D']:
screen.nodelay(False)
screen.clear()
curses.echo()
try:
screen.move(0, 0)
dir_key = screen.getkey()
direction = directions[dir_key]
except KeyError:
screen.addstr(4, 0,
"La direction doit etre soit 'c' ou soit 'cc'.")
curses.noecho()
screen.nodelay(True)
ser.write(protocol.generate_manual_speed_command(motor_id, 0, direction))
return None
def keyboard_speed(screen):
screen.clear()
screen.nodelay(True)
set_pid_constants()
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.ON))
speed_x = 0
speed_y = 0
speed_theta = 0
target_changed = False
sub_run = True
while sub_run:
time.sleep(0.060)
ser.read(ser.inWaiting())
try:
user_key = screen.getkey()
except curses.error:
user_key = -1
set_pid_to_keyboard_speed(speed_x, speed_y, speed_theta)
if user_key in ['q', 'Q']:
sub_run = False
elif user_key == 'w':
speed_x += 10
elif user_key == 's':
speed_x -= 10
elif user_key == 'd':
speed_y += 10
elif user_key == 'a':
speed_y -= 10
elif user_key in ['c']:
speed_x = 0
speed_y = 0
speed_theta = 0
elif user_key in ['e']:
speed_theta -= 0.05
elif user_key in ['r']:
speed_theta += 0.05
speed_x, speed_y, speed_theta = cap_pid_speed(speed_x, speed_y,
speed_theta)
screen.clear()
screen.addstr(
0, 0, "Speed in x: {}mm/s ({:0.0f} tick/s)".format(
speed_x, convert_to_tick(speed_x)))
screen.addstr(
1, 0, "Speed in y: {}mm/s ({:0.0f} tick/s)".format(
speed_y, convert_to_tick(speed_y)))
screen.addstr(2, 0, "Angular speed: {}".format(speed_theta))
front_x = read_encoder(protocol.Motors.FRONT_X, ser)
rear_x = read_encoder(protocol.Motors.REAR_X, ser)
front_y = read_encoder(protocol.Motors.FRONT_Y, ser)
rear_y = read_encoder(protocol.Motors.REAR_Y, ser)
last_cmd_front_x = read_pid_last_cmd(protocol.Motors.FRONT_X, ser)
last_cmd_rear_x = read_pid_last_cmd(protocol.Motors.REAR_X, ser)
last_cmd_front_y = read_pid_last_cmd(protocol.Motors.FRONT_Y, ser)
last_cmd_rear_y = read_pid_last_cmd(protocol.Motors.REAR_Y, ser)
screen.addstr(3, 0, "Moteur FRONT_X: {} tick/s".format(front_x))
screen.addstr(4, 0, "Moteur REAR_X: {} tick/s".format(rear_x))
screen.addstr(5, 0, "Moteur FRONT_Y: {} tick/s".format(front_y))
screen.addstr(6, 0, "Moteur REAR_Y: {} tick/s".format(rear_y))
screen.addstr(7, 0,
"Moteur FRONT_X last cmd: {} ".format(last_cmd_front_x))
screen.addstr(8, 0,
"Moteur REAR_X last cmd: {} ".format(last_cmd_rear_x))
screen.addstr(9, 0,
"Moteur FRONT_Y last cmd: {} ".format(last_cmd_front_y))
screen.addstr(10, 0,
"Moteur REAR_Y last cmd: {} ".format(last_cmd_rear_y))
#adc_val = adc.read_last_adc(protocol.Adc.ADC_PENCIL)
#adc_volt = adc.convert_adc_value_to_voltage(adc_val)
#rfsr = adc.convert_voltage_to_force_sensor_resistance(adc_volt)
#screen.addstr(10, 0, "ADC pencil value: ADC={}, Vadc={:0.2f}, Rfsr={:0.2f}".format(adc_val, adc_volt, rfsr))
display_busy_wait(screen, 11)
screen.move(3, 0)
screen.nodelay(False)
set_pid_to_keyboard_speed(0, 0, 0)
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
return None