def main(motors_id: tuple):
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
ser.read()
for motorx in protocol.Motors:
ser.write(
protocol.generate_manual_speed_command(
motorx, 1, protocol.MotorsDirection.FORWARD))
consigne = []
retroaction = []
if ROTATION_TEST == True:
rotation_test(motors_id, consigne, retroaction)
else:
direction_test(motors_id, consigne, retroaction)
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.ON))
for motorx in protocol.Motors:
ser.write(
protocol.generate_manual_speed_command(
motorx, 1, protocol.MotorsDirection.FORWARD))
with open(fname + '.csv', 'w') as f:
writer = csv.writer(f, delimiter=',')
writer.writerow(consigne)
writer.writerow(retroaction)
print("Finish")
def main():
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
ser.read(1)
motorx = protocol.Motors.FRONT_X
pwm = 50
motor_dir = protocol.MotorsDirection.FORWARD
ser.write(protocol.generate_manual_speed_command(motorx, pwm, motor_dir))
print_code(HEADER_W, ser)
print_code(PAYLOAD_W, ser)
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
ser.read(ser.in_waiting())
def deinit():
cmd_reset_move = protocol.generate_move_command(0, 0, 0)
ser.write(cmd_reset_move)
cmd_enable_pid = protocol.generate_set_pid_mode(protocol.PIDStatus.ON)
ser.write(cmd_enable_pid)
def init():
ser.read(ser.inWaiting())
cmd_disable_pid = protocol.generate_set_pid_mode(protocol.PIDStatus.OFF)
ser.write(cmd_disable_pid)
set_pid_constants()
def deinit():
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.ON))
ser.write(protocol.generate_led_command(protocol.Leds.DOWN_GREEN))
def init():
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
ser.write(protocol.generate_led_command(protocol.Leds.UP_GREEN))
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
import serial
import protocol
from util import *
ser = serial.Serial("/dev/ttySTM32")
if __name__ == "__main__":
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.OFF))
print_code("cmd status: ", ser)
ser.write(protocol.generate_set_pid_mode(protocol.PIDStatus.ON))
print_code("cmd status: ", ser)
ser.close()