Skip to content
Snippets Groups Projects
Commit 6e74edb4 authored by Václav Jelínek's avatar Václav Jelínek
Browse files

Update segway to use the new ESP library

parent 810d519d
No related branches found
No related tags found
1 merge request!7Merge old hardware code with new hardware code
Hide whitespace changes
Inline Side-by-side
programs/gyro.py
+ 8
− 8
View file @ 6e74edb4
......@@ -12,7 +12,7 @@ from lib.robot_consts import Button, Port, Sensor, GyroAcc
# Gyro and accelerometer settings
offset_angle = 0.141
offset_angle = 0.134
angle_compl = offset_angle
Kcomp = 0.001
gyro_start_time = 0
......@@ -27,7 +27,7 @@ BINARY_HEADER = (203, 98, 116, 140, 15, 42, 254)
NUM_RECEIVED_BYTES = 96
# Length constants
WHEEL_RADIUS = 0.0425
WHEEL_RADIUS = 0.026
ROTATION_RADIUS = 0.08
ULTRA_MAX_RANGE = 2.55
......@@ -134,10 +134,10 @@ def main():
robot.display.show()
# Initialize regulator constants
KpP,KiP,KdP,KfdP,max_P,max_i_P = 0.4,0.01,0.0,0.0,0.22,0.05
KpP,KiP,KdP,KfdP,max_P,max_i_P = 0.2,0.01,0.0,0.0,0.14,0.05
KpS,KiS,KdS,KfdS,max_S,max_i_S = 0.25,0.0,-0.07,25,0.06,0.06
KpA,KiA,KdA,KfdA,max_A,max_i_A = 2000,0,-500,80,100,0
KpR,KiR,KdR,KfdR,max_R,max_i_R = 100,1,0,0,40,1
KpA,KiA,KdA,KfdA,max_A,max_i_A = 2400,0,-500,80,100,0
KpR,KiR,KdR,KfdR,max_R,max_i_R = 300,1,0,0,40,1
KpL,KiL,KdL,KfdL,max_L,max_i_L = 0,0,0,0,0.23,0
ref_position,ref_rotation,ref_speed = 0.0,0.0,0.0
......@@ -158,7 +158,7 @@ def main():
robot.init_sensor(sensor_type=Sensor.NXT_ULTRASONIC)
# Initialize ESP communication in binary mode
robot.esp.set_binary(BINARY_HEADER, NUM_RECEIVED_BYTES)
robot.esp.bt_set_binary(BINARY_HEADER, NUM_RECEIVED_BYTES)
# Initialize motors
robot.init_motor(Port.M2)
......@@ -201,7 +201,7 @@ def main():
robot.motors[Port.M3].set_power(motor_power+rot_power)
# Receive data from ESP and update regulator parameters
line = robot.esp.read()
line = robot.esp.bt_read()
if line:
read_values = struct.unpack('<ffffffffffffffffffffffff', line)
(KpP,KiP,max_P,
......@@ -216,7 +216,7 @@ def main():
ref_position, position, ref_speed, speed, ref_angle, e_angle,
ref_rotation, rotation, ref_dist, wall_dist,
motor_power, rot_power, ref_ultra_speed)
robot.esp.write(write_message)
robot.esp.bt_write(write_message)
# Exit program if left cube button is pressed
buttons = robot.buttons.pressed()
......
programs/gyro_speed.py
+ 62
− 54
View file @ 6e74edb4
......@@ -12,7 +12,7 @@ from lib.robot_consts import Button, Port, Sensor, GyroAcc
# Gyro and accelerometer settings
offset_angle = 0.137
offset_angle = 0.133
angle_compl = offset_angle
Kcomp = 0.001
gyro_start_time = 0
......@@ -22,20 +22,17 @@ regul_start_time = 0
REGUL_PERIOD_US = 10000
REGUL_PERIOD = REGUL_PERIOD_US/1000000
# ESP communication settings
STOP = 0
UP = 1
DOWN = 2
LEFT = 3
RIGHT = 4
#BINARY_HEADER = (203, 98, 116, 140, 15, 42, 254)
#NUM_RECEIVED_BYTES = 96
# Length constants
WHEEL_RADIUS = 0.0425
WHEEL_RADIUS = 0.025
ROTATION_RADIUS = 0.08
ULTRA_MAX_RANGE = 2.55
# Control settings
MAX_SPEED = 0.2
MAX_ROT_SPED = 1.3
SPEED_STEP = MAX_SPEED/150
ROT_SPEED_STEP = MAX_ROT_SPED/50
# Average for ultrasonic measurements
class StreamingMovingAverage:
def __init__(self, window_size):
......@@ -59,8 +56,8 @@ def gyro_callback(p):
gyro_start_time = new_time
gyro_acc_meas = robot.sensors.gyro_acc.read_value()
gy = gyro_acc_meas[GyroAcc.GY]
ax = gyro_acc_meas[GyroAcc.AX]
gy = -gyro_acc_meas[GyroAcc.GY]
ax = -gyro_acc_meas[GyroAcc.AX]
az = gyro_acc_meas[GyroAcc.AZ]
angle_acc = atan2(az, ax)
......@@ -140,10 +137,9 @@ def main():
robot.display.show()
# Initialize regulator constants
KpP,KiP,KdP,KfdP,max_P,max_i_P = 0.4,0.01,0.0,0.0,0.22,0.05
KpS,KiS,KdS,KfdS,max_S,max_i_S = 0.3,0.6,-0.07,25,0.05,0.2
KpA,KiA,KdA,KfdA,max_A,max_i_A = 1800,0,-700,90,100,0
KpR,KiR,KdR,KfdR,max_R,max_i_R = 5,10,0,0,30,30
KpS,KiS,KdS,KfdS,max_S,max_i_S = 0.3,0.55,-0.07,25,0.05,0.2
KpA,KiA,KdA,KfdA,max_A,max_i_A = 2500,0,-700,90,100,0
KpR,KiR,KdR,KfdR,max_R,max_i_R = 30,50,0,0,45,45
KpL,KiL,KdL,KfdL,max_L,max_i_L = 0,0,0,0,0.23,0
ref_position,ref_rotation,ref_speed = 0.0,0.0,0.0
......@@ -164,7 +160,17 @@ def main():
robot.init_sensor(sensor_type=Sensor.NXT_ULTRASONIC)
# Initialize ESP communication in binary mode
robot.init_esp_uart()
wifi = robot.esp.wifi()
print("Wifi: ", wifi)
if wifi:
return
print("Name: ", robot.esp.set_name("Open-Cube-Wifi"))
print("Password: ", robot.esp.set_password("01234567"))
robot.esp.wifi_set_indicators_labels(("", "", "", "", ""))
robot.esp.wifi_set_buttons_labels(("", "up", "", "left", "", "right", "", "down", "" ))
robot.esp.wifi_set_switches_labels(("", "", "speed", "", ""))
robot.esp.wifi_set_numbers_labels(("motorpwr:", "rot pwr:", "speed:", "rotation:", "ref s:", "ref r:"))
# Initialize motors
robot.init_motor(Port.M2)
......@@ -176,7 +182,7 @@ def main():
robot.init_sensor(sensor_type=Sensor.GYRO_ACC)
gyro_start_time = utime.ticks_us()
p0 = Pin(GyroAcc.IRQ_PIN, Pin.IN)
p0.irq(trigger=Pin.IRQ_RISING, handler=gyro_callback)
p0.irq(trigger=Pin.IRQ_FALLING, handler=gyro_callback)
counter = 0
......@@ -187,8 +193,8 @@ def main():
e_angle = angle_compl-offset_angle
position, speed, rotation, m1, m2, rot_speed = get_position(m1, m2, rotation)
if counter % 5 == 0:
wall_dist = ultra_avg.process(robot.sensors.ultra_nxt.distance()/100)
#if counter % 5 == 0:
# wall_dist = ultra_avg.process(robot.sensors.ultra_nxt.distance()/100)
# Regulators
#ref_speed, d_eP, last_eP, sum_eP = PID_regulator(position, ref_position, d_eP, last_eP, sum_eP, max_P, max_i_P, (KpP,KiP,KdP,KfdP))
......@@ -207,40 +213,42 @@ def main():
robot.motors[Port.M3].set_power(motor_power+rot_power)
# Receive data from ESP and update regulator parameters
line = robot.esp_uart.read()
if line:
direction = int(line)
print(direction)
ref_speed = 0
ref_rotation = 0
sum_eS = 0
sum_eR = 0
if direction == UP:
ref_speed = 0.16
if direction == DOWN:
ref_speed = -0.16
if direction == LEFT:
ref_rotation = 3
if direction == RIGHT:
ref_rotation = -3
'''
if line:
read_values = struct.unpack('<ffffffffffffffffffffffff', line)
(KpP,KiP,max_P,
KpS,KiS,KdS,KfdS,max_S,
KpA,KiA,KdA,KfdA,max_A,
KpR,KiR,max_R,
KpL,KiL,max_L,
ref_position,ref_rotation,ref_dist,offset_angle,Kcomp) = read_values
esp_buttons = robot.esp.wifi_get_buttons()
esp_switches = robot.esp.wifi_get_switches()
if esp_switches[2]:
MAX_SPEED = 0.2
MAX_ROT_SPED = 2
else:
MAX_SPEED = 0.15
MAX_ROT_SPED = 1
SPEED_STEP = MAX_SPEED/150
ROT_SPEED_STEP = MAX_ROT_SPED/50
if esp_buttons[1]:
if ref_speed < MAX_SPEED:
ref_speed += SPEED_STEP
elif esp_buttons[7]:
if ref_speed > MAX_SPEED*(-1):
ref_speed -= SPEED_STEP
else:
if ref_speed > 0:
ref_speed -= SPEED_STEP
elif ref_speed < 0:
ref_speed += SPEED_STEP
if esp_buttons[3]:
if ref_rotation < MAX_ROT_SPED:
ref_rotation += ROT_SPEED_STEP
elif esp_buttons[5]:
if ref_rotation > MAX_ROT_SPED*(-1):
ref_rotation -= ROT_SPEED_STEP
else:
if ref_rotation > 0:
ref_rotation -= ROT_SPEED_STEP
elif ref_rotation < 0:
ref_rotation += ROT_SPEED_STEP
# Send data to ESP
write_message = struct.pack('<fffffffffffff',
ref_position, position, ref_speed, speed, ref_angle, e_angle,
ref_rotation, rotation, ref_dist, wall_dist,
motor_power, rot_power, ref_ultra_speed)
robot.esp_uart.write(write_message)
'''
if counter % 20 == 0:
robot.esp.wifi_set_numbers((motor_power, rot_power, speed, rot_speed, ref_speed, ref_rotation))
# Exit program if left cube button is pressed
buttons = robot.buttons.pressed()
if buttons[Button.LEFT]:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment