modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

last up 2025

Browse Source
This commit is contained in:
acki
2025-10-04 11:20:56 +02:00
parent 8c9e6e67ad
commit 037e3c841a
17 changed files with 47 additions and 411 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
simulated_pcb/odo.py
View File

@@ -56,12 +56,12 @@ class SimulatedPCB:
self.waypoint_order.pop(0)
del self.waypoints[wp['id']]
else:
speed = min(300.0, distance * 2)
speed = min(100.0, distance / 2)
self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle)
self.vtheta = max(-9.0, min(9.0, angle_diff * 2))
else:
speed = min(400.0, distance * 3)
speed = min(150.0, distance / 2)
self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle)
self.vtheta = 0.0
@@ -80,9 +80,10 @@ class SimulatedPCB:
self.theta += self.vtheta * dt
self.theta = self.normalize_angle(self.theta)
if now - self.last_send > 0.1:
# self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
self.last_send = now
if now - self.last_send > 0.1:
# print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}')
self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
self.last_send = now
def run(self):
while self.running:
@@ -119,11 +120,26 @@ class SimulatedPCB:
print(f'[TX] OK;PID;1')
self.ser.write(f'OK;PID;1\n'.encode())
elif msg.startswith("SET;START"):
elif msg.startswith("SET;START;"):
self.start = msg.split(';')[2] == '1'
print(f'[TX] OK;START;1')
self.ser.write(f'OK;START;1\n'.encode())
elif msg.startswith("SET;WAYPOINT;"):
parts = msg.split(';')
if len(parts) >= 7:
wp = {
'id': int(parts[2]),
'type': int(parts[3]),
'x': float(parts[4]),
'y': float(parts[5]),
'theta': float(parts[6])
}
self.waypoints[wp['id']] = wp
if wp['id'] not in self.waypoint_order:
self.waypoint_order.append(wp['id'])
self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode())
elif msg.startswith("SET;WAYPOINTS;"):
parts = msg.split(';')
if len(parts) / 7 >= 1:
@@ -168,4 +184,4 @@ if __name__ == '__main__':
except KeyboardInterrupt:
if sim:
sim.stop()
print("Simulation stopped")
print("Simulation stopped")