Skip to content

Commit 8f467ca

Browse files
authored
Merge pull request #67 from Open-STEM/turning_fixes2
turning fixes and run_robot
2 parents 411fbc4 + 8dbbf6c commit 8f467ca

File tree

5 files changed

+186
-10
lines changed

5 files changed

+186
-10
lines changed

.gitignore

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,4 +3,6 @@
33
docs/_build/
44
.micropico
55
.picowgo
6-
secrets.json
6+
secrets.json
7+
venv
8+
.temp_xrplib

XRPLib/differential_drive.py

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -240,20 +240,24 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
240240

241241
if main_controller is None:
242242
main_controller = PID(
243-
kp = 0.02,
244-
ki = 0.001,
245-
kd = 0.00165,
246-
min_output = 0.2,
243+
# kp = 0.2,
244+
# ki = 0.004,
245+
# kd = 0.0036,
246+
kd = 0.0036 + 0.0034 * (max(max_effort, 0.5) - 0.5) * 2,
247+
kp = 0.2,
248+
ki = 0.004,
249+
#kd = 0.007,
250+
min_output = 0.1,
247251
max_output = max_effort,
248-
max_integral = 75,
252+
max_integral = 30,
249253
tolerance = 1,
250254
tolerance_count = 3
251255
)
252256

253257
# Secondary controller to keep encoder values in sync
254258
if secondary_controller is None:
255259
secondary_controller = PID(
256-
kp = 0.5,
260+
kp = 0.25,
257261
)
258262

259263
if use_imu and (self.imu is not None):
@@ -274,8 +278,8 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
274278
turn_error = turn_degrees - ((right_delta-left_delta)/2)*360/(self.track_width*math.pi)
275279

276280
# Pass the turn error to the main controller to get a turn speed
277-
turn_speed = main_controller.update(turn_error)
278-
281+
turn_speed = main_controller.update(turn_error, True)
282+
279283
# exit if timeout or tolerance reached
280284
if main_controller.is_done() or time_out.is_done():
281285
break

XRPLib/pid.py

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ def _handle_exit_condition(self, error: float):
5757
# otherwise, reset times in tolerance, because we need to be in tolerance for numTimesInTolerance consecutive times
5858
self.times = 0
5959

60-
def update(self, error: float) -> float:
60+
def update(self, error: float, debug: bool = False) -> float:
6161
"""
6262
Handle a new update of this PID loop given an error.
6363
@@ -109,6 +109,9 @@ def update(self, error: float) -> float:
109109
# cache output for next update
110110
self.prev_output = output
111111

112+
if debug:
113+
print(f"{output}: ({self.kp * error}, {self.ki * integral}, {self.kd * derivative})")
114+
112115
return output
113116

114117
def is_done(self) -> bool:

mainprogram.py

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
2+
3+
from XRPLib.encoded_motor import EncodedMotor
4+
import time
5+
from XRPLib.differential_drive import DifferentialDrive
6+
7+
differentialDrive = DifferentialDrive.get_default_differential_drive()
8+
9+
def turn_test(angle, effort):
10+
for i in range(2):
11+
differentialDrive.turn(angle, effort)
12+
differentialDrive.turn(-angle, effort)
13+
differentialDrive.stop()
14+
15+
16+
turn_test(30, 0.5)
17+
turn_test(180, 0.5)
18+
turn_test(30, 0.75)
19+
turn_test(180, 0.75)
20+
turn_test(30, 1)
21+
turn_test(180, 1)

run_robot.py

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
import os
2+
import shutil
3+
from ampy.files import Files
4+
from ampy.pyboard import Pyboard
5+
import serial, threading, time, subprocess
6+
import serial.tools.list_ports
7+
import filecmp
8+
9+
10+
# Folder to store the last synced XRPLib files
11+
TEMP_FOLDER = ".temp_xrplib"
12+
13+
14+
def ensure_directory_exists(files, directory):
15+
"""Ensure that a directory exists on the Pico. If not, create it."""
16+
try:
17+
if directory != "/": # Root directory always exists
18+
files.mkdir(directory)
19+
print(f"Directory {directory} created on Pico.")
20+
except Exception as e:
21+
# Ignore the error if the directory already exists
22+
pass
23+
24+
def copy_file_to_pico(files, local_file_path, pico_destination_path):
25+
"""Copy a single file to the Pico, replacing it if it already exists."""
26+
try:
27+
with open(local_file_path, 'rb') as local_file:
28+
file_content = local_file.read()
29+
30+
# Write or replace the file on the Pico
31+
files.put(pico_destination_path, file_content)
32+
print(f"Successfully copied {local_file_path} to {pico_destination_path} on Pico.")
33+
34+
except Exception as e:
35+
print(f"Failed to copy file {local_file_path}. Error: {str(e)}")
36+
37+
def copy_directory_to_pico(files, local_directory, pico_destination_directory):
38+
"""Copy an entire directory to the Pico, including all subdirectories and files"""
39+
try:
40+
for root, dirs, files_list in os.walk(local_directory):
41+
relative_path = os.path.relpath(root, local_directory)
42+
pico_dir_path = os.path.join(pico_destination_directory, relative_path).replace("\\", "/")
43+
ensure_directory_exists(files, pico_dir_path)
44+
45+
for file_name in files_list:
46+
local_file_path = os.path.join(root, file_name)
47+
pico_file_path = os.path.join(pico_dir_path, file_name).replace("\\", "/")
48+
copy_file_to_pico(files, local_file_path, pico_file_path)
49+
50+
except Exception as e:
51+
print(f"Failed to copy directory {local_directory}. Error: {str(e)}")
52+
finally:
53+
pyb.close() # Ensure the connection to the Pico is closed
54+
55+
def create_temp_folder():
56+
"""Create a temporary folder to store the last synced XRPLib files."""
57+
if not os.path.exists(TEMP_FOLDER):
58+
os.makedirs(TEMP_FOLDER)
59+
60+
def compare_and_copy(files, local_directory, pico_destination_directory):
61+
"""Compare local XRPLib with the last synced version and copy only changed files."""
62+
create_temp_folder()
63+
for root, dirs, files_list in os.walk(local_directory):
64+
relative_path = os.path.relpath(root, local_directory)
65+
pico_dir_path = os.path.join(pico_destination_directory, relative_path).replace("\\", "/")
66+
temp_dir_path = os.path.join(TEMP_FOLDER, relative_path)
67+
68+
if not os.path.exists(temp_dir_path):
69+
os.makedirs(temp_dir_path)
70+
71+
for file_name in files_list:
72+
local_file_path = os.path.join(root, file_name)
73+
temp_file_path = os.path.join(temp_dir_path, file_name)
74+
pico_file_path = os.path.join(pico_dir_path, file_name).replace("\\", "/")
75+
76+
if not os.path.exists(temp_file_path) or not filecmp.cmp(local_file_path, temp_file_path, shallow=False):
77+
# If the file doesn't exist in the temp folder, or it has changed, copy it
78+
copy_file_to_pico(files, local_file_path, pico_file_path)
79+
# Update the temp folder with the new file
80+
shutil.copy2(local_file_path, temp_file_path)
81+
82+
def read_serial_output(port, baudrate=115200, timeout=1):
83+
"""Read and print serial output from the Pico W."""
84+
try:
85+
with serial.Serial(port, baudrate, timeout=timeout) as ser:
86+
print("Reading serial output...")
87+
while True:
88+
if ser.in_waiting > 0:
89+
output = ser.read(ser.in_waiting).decode('utf-8')
90+
print(output, end='') # Print without adding extra newlines
91+
time.sleep(0.1) # Small delay to prevent high CPU usage
92+
except Exception as e:
93+
print(f"Failed to read serial output. Error: {str(e)}")
94+
95+
def run_pico_script(port, script_name):
96+
"""Run a MicroPython script on the Pico using ampy."""
97+
try:
98+
subprocess.run(['ampy', '-p', port, 'run', script_name], check=True)
99+
print(f"Successfully ran {script_name} on Pico.")
100+
except subprocess.CalledProcessError as e:
101+
print(f"Failed to run script {script_name}. Error: {str(e)}")
102+
103+
def list_files_on_pico(port):
104+
"""List all files and folders on the Pico."""
105+
try:
106+
pyb = Pyboard(port)
107+
files = Files(pyb)
108+
file_list = files.ls('/')
109+
110+
print("Files and folders on Pico:")
111+
for file in file_list:
112+
print(file)
113+
114+
except Exception as e:
115+
print(f"Failed to list files. Error: {str(e)}")
116+
117+
def copy_all_files_to_pico(port, directory=True, main=True, telemetry=True):
118+
pyb = Pyboard(port)
119+
files = Files(pyb)
120+
121+
if directory:
122+
compare_and_copy(files, "XRPLib", "lib/XRPLib")
123+
if main:
124+
copy_file_to_pico(files, "mainprogram.py", "mainprogram.py")
125+
if telemetry:
126+
copy_file_to_pico(files, "telemetry.txt", "telemetry.txt")
127+
128+
def detect_pico_port():
129+
"""Auto-detect the Pico W's serial port."""
130+
ports = list(serial.tools.list_ports.comports())
131+
for port in ports:
132+
if "usbmodem" in port.device or "COM" in port.device: # Adjust this pattern if needed
133+
return port.device
134+
raise IOError("Pico W not found. Please check the connection.")
135+
136+
if __name__ == "__main__":
137+
138+
# Auto-detect the Pico W port
139+
pico_port = detect_pico_port()
140+
print(f"Pico W detected on port: {pico_port}")
141+
142+
# Copy any changed files to the Pico
143+
copy_all_files_to_pico(pico_port, telemetry=False)
144+
145+
# Run the mainprogram.py script on the Pico
146+
run_pico_script(pico_port, "mainprogram.py")

0 commit comments

Comments
 (0)