diff options
Diffstat (limited to 'simulation/controllers')
| -rw-r--r-- | simulation/controllers/OMHI_test/OMHI_test.py | 84 |
1 files changed, 84 insertions, 0 deletions
diff --git a/simulation/controllers/OMHI_test/OMHI_test.py b/simulation/controllers/OMHI_test/OMHI_test.py new file mode 100644 index 0000000..ca8db64 --- /dev/null +++ b/simulation/controllers/OMHI_test/OMHI_test.py @@ -0,0 +1,84 @@ +from controller import Robot +import math + +# Create the Robot instance. +robot = Robot() + +# Get the time step of the current world. +timestep = int(robot.getBasicTimeStep()) + +# Print all available device names for debugging +device_count = robot.getNumberOfDevices() +print("Number of devices:", device_count) +for i in range(device_count): + device = robot.getDeviceByIndex(i) + print("Device", i, ":", device.getName()) + +# Get the wheel motors +wheel_1 = robot.getDevice("wheel1") +wheel_2 = robot.getDevice("wheel2") +wheel_3 = robot.getDevice("wheel3") + +# Check if the devices are found +if wheel_1 is None: + print("Device 'wheel1' not found") +if wheel_2 is None: + print("Device 'wheel2' not found") +if wheel_3 is None: + print("Device 'wheel3' not found") + +# Set the position to infinity to control velocity +wheel_1.setPosition(float('inf')) +wheel_2.setPosition(float('inf')) +wheel_3.setPosition(float('inf')) + +# Set the initial velocity to 0 +wheel_1.setVelocity(0) +wheel_2.setVelocity(0) +wheel_3.setVelocity(0) + + +def set_wheel_velocities(velocity1, velocity2, velocity3): + wheel_1.setVelocity(velocity1) + wheel_2.setVelocity(velocity2) + wheel_3.setVelocity(velocity3) + + +def omhidrive(V, D, R, radius): + """ + Calculate the speeds of three motors for a three-wheeled omnidirectional robot. + + :param V: Linear velocity magnitude + :param D: Direction of the velocity in degrees + :param R: Angular velocity (rotation speed) + :param radius: Distance from the center of the robot to the wheels + :return: Speeds of the three motors + """ + # Convert direction to radians + dirrad = math.radians(D) + + # Calculate the components of the velocity + V_x = V * math.cos(dirrad) + V_y = V * math.sin(dirrad) + print(f"V x speed: {V_x}") + print(f"V y speed: {V_y}") + + # Wheel angles (in radians) + angles = [math.radians(0), math.radians(120), math.radians(240)] + + # Calculate motor speeds using the provided formulas + spdeA = V_x * math.cos(angles[0]) + V_y * math.sin(angles[0]) + R * radius + spdeB = V_x * math.cos(angles[1]) + V_y * math.sin(angles[1]) + R * radius + spdeC = V_x * math.cos(angles[2]) + V_y * math.sin(angles[2]) + R * radius + + print(f"Motor A speed: {spdeA}") + print(f"Motor B speed: {spdeB}") + print(f"Motor C speed: {spdeC}") + + # Assuming set_wheel_velocities is a function to set the speeds of the motors + set_wheel_velocities(spdeA, spdeB, spdeC) + +# Main loop: +while robot.step(timestep) != -1: + + omhidrive(5, 0, 0, 0.188)
\ No newline at end of file |