From cf248b1e3d261d895278af5bda837e64635c7f69 Mon Sep 17 00:00:00 2001
From: Paul Winpenny
<plw1g21@soton.ac.ukgit config --global user.email plw1g21@soton.ac.uk>
Date: Thu, 21 Nov 2024 15:31:00 +0000
Subject: [PATCH] Added scripts from motor controller pi
---
Motor_controller_test_pi/Encoder/Encoder.py | 60 +++++++
.../Encoder/Test_encoder_pulse.py | 42 +++++
Motor_controller_test_pi/Encoder_testing.py | 104 +++++++++++
Motor_controller_test_pi/Encoder_testing_2.py | 83 +++++++++
.../IMU /Boasch/New_IMU.py | 45 +++++
.../IMU /Boasch/New_IMU_calibration.py | 65 +++++++
.../IMU /IMU_Calibration.py | 164 ++++++++++++++++++
Motor_controller_test_pi/IMU /IMU_Library.py | 47 +++++
Motor_controller_test_pi/IMU /IMU_smbus.py | 99 +++++++++++
Motor_controller_test_pi/IMU /ahrs_library.py | 118 +++++++++++++
.../Motor_control/Motor_control.py | 132 ++++++++++++++
.../Motor_control/Ros2_Motor_control.py | 68 ++++++++
.../Test_encoder_motor.py | 104 +++++++++++
Motor_controller_test_pi/pigpio.py | 135 ++++++++++++++
14 files changed, 1266 insertions(+)
create mode 100644 Motor_controller_test_pi/Encoder/Encoder.py
create mode 100644 Motor_controller_test_pi/Encoder/Test_encoder_pulse.py
create mode 100644 Motor_controller_test_pi/Encoder_testing.py
create mode 100644 Motor_controller_test_pi/Encoder_testing_2.py
create mode 100644 Motor_controller_test_pi/IMU /Boasch/New_IMU.py
create mode 100644 Motor_controller_test_pi/IMU /Boasch/New_IMU_calibration.py
create mode 100644 Motor_controller_test_pi/IMU /IMU_Calibration.py
create mode 100644 Motor_controller_test_pi/IMU /IMU_Library.py
create mode 100644 Motor_controller_test_pi/IMU /IMU_smbus.py
create mode 100644 Motor_controller_test_pi/IMU /ahrs_library.py
create mode 100644 Motor_controller_test_pi/Motor_control/Motor_control.py
create mode 100644 Motor_controller_test_pi/Motor_control/Ros2_Motor_control.py
create mode 100644 Motor_controller_test_pi/Test_encoder_motor.py
create mode 100644 Motor_controller_test_pi/pigpio.py
diff --git a/Motor_controller_test_pi/Encoder/Encoder.py b/Motor_controller_test_pi/Encoder/Encoder.py
new file mode 100644
index 00000000..6c949677
--- /dev/null
+++ b/Motor_controller_test_pi/Encoder/Encoder.py
@@ -0,0 +1,60 @@
+import RPi.GPIO as GPIO
+import time
+
+# Pin configuration
+channel_a = 17 # GPIO pin for Channel A
+channel_b = 27 # GPIO pin for Channel B
+
+# Encoder specifications
+PPR = 5 # Pulses per revolution
+time_interval = 1.0 # Time interval for speed calculation in seconds
+
+# Variables
+pulse_count = 0
+direction = None
+last_time = time.time()
+
+def setup():
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(channel_a, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+ GPIO.setup(channel_b, GPIO.IN, pull_up_down=GPIO.PUD_UP)
+ GPIO.add_event_detect(channel_a, GPIO.RISING, callback=encoder_callback)
+
+def encoder_callback(channel):
+ global pulse_count, direction
+ channel_b_state = GPIO.input(channel_b)
+ if channel_b_state == 0:
+ direction = "Clockwise"
+ pulse_count += 1
+ else:
+ direction = "Counterclockwise"
+ pulse_count -= 1
+
+def calculate_speed():
+ global pulse_count, last_time
+ current_time = time.time()
+ elapsed_time = current_time - last_time
+ last_time = current_time
+
+ # Calculate revolutions
+ revolutions = pulse_count / PPR
+ pulse_count = 0 # Reset pulse count after calculation
+
+ # Calculate RPM
+ rpm = (revolutions / elapsed_time) * 60
+ return rpm
+
+def main():
+ try:
+ setup()
+ print("Reading encoder data and calculating speed...")
+ while True:
+ time.sleep(time_interval) # Wait for the interval
+ speed = calculate_speed()
+ print(f"Speed: {speed:.2f} RPM, Direction: {direction}")
+ except KeyboardInterrupt:
+ print("\nExiting...")
+ GPIO.cleanup()
+
+if __name__ == "__main__":
+ main()
diff --git a/Motor_controller_test_pi/Encoder/Test_encoder_pulse.py b/Motor_controller_test_pi/Encoder/Test_encoder_pulse.py
new file mode 100644
index 00000000..a7b76224
--- /dev/null
+++ b/Motor_controller_test_pi/Encoder/Test_encoder_pulse.py
@@ -0,0 +1,42 @@
+from gpiozero import Button
+from time import sleep
+
+# Define pins for the Hall sensors
+MOTOR1_HALL_A = 17 # Update with actual GPIO pin for Motor 1, Channel A
+MOTOR1_HALL_B = 27 # Update with actual GPIO pin for Motor 1, Channel B
+MOTOR2_HALL_A = 22 # Update with actual GPIO pin for Motor 2, Channel A
+MOTOR2_HALL_B = 23 # Update with actual GPIO pin for Motor 2, Channel B
+
+# Create Button objects for sensors
+motor1_hall_a = Button(MOTOR1_HALL_A, pull_up=True)
+motor1_hall_b = Button(MOTOR1_HALL_B, pull_up=True)
+motor2_hall_a = Button(MOTOR2_HALL_A, pull_up=True)
+motor2_hall_b = Button(MOTOR2_HALL_B, pull_up=True)
+
+# Callback functions for state changes
+def motor1_channel_a_detected():
+ print("Motor 1: Channel A pulse detected")
+
+def motor1_channel_b_detected():
+ print("Motor 1: Channel B pulse detected")
+
+def motor2_channel_a_detected():
+ print("Motor 2: Channel A pulse detected")
+
+def motor2_channel_b_detected():
+ print("Motor 2: Channel B pulse detected")
+
+# Attach callbacks to each sensor
+motor1_hall_a.when_pressed = motor1_channel_a_detected
+motor1_hall_b.when_pressed = motor1_channel_b_detected
+motor2_hall_a.when_pressed = motor2_channel_a_detected
+motor2_hall_b.when_pressed = motor2_channel_b_detected
+
+# Main loop
+try:
+ print("Monitoring Hall sensors for both motors. Press Ctrl+C to exit.")
+ while True:
+ sleep(0.1) # Prevent excessive CPU usage
+
+except KeyboardInterrupt:
+ print("\nExiting program...")
diff --git a/Motor_controller_test_pi/Encoder_testing.py b/Motor_controller_test_pi/Encoder_testing.py
new file mode 100644
index 00000000..8c071e48
--- /dev/null
+++ b/Motor_controller_test_pi/Encoder_testing.py
@@ -0,0 +1,104 @@
+from gpiozero import PWMOutputDevice, RotaryEncoder
+from time import sleep, time
+
+class Motor():
+ def __init__(self, EnaA, In1A, In2A, EnaB, In1B, In2B, encA_pins=None, encB_pins=None):
+ # Initialize motor control pins
+ self.pwmA = PWMOutputDevice(EnaA)
+ self.in1A = PWMOutputDevice(In1A)
+ self.in2A = PWMOutputDevice(In2A)
+ self.pwmB = PWMOutputDevice(EnaB)
+ self.in1B = PWMOutputDevice(In1B)
+ self.in2B = PWMOutputDevice(In2B)
+
+ # Initialize encoders if pins are provided
+ if encA_pins:
+ self.encoderA = RotaryEncoder(a=encA_pins[0], b=encA_pins[1], max_steps=0)
+ self.encoderA.steps = 0 # Reset steps
+ self.encoderA.when_rotated = self.on_rotate_left
+ else:
+ self.encoderA = None
+
+ if encB_pins:
+ self.encoderB = RotaryEncoder(a=encB_pins[0], b=encB_pins[1], max_steps=0)
+ self.encoderB.steps = 0 # Reset steps
+ self.encoderB.when_rotated = self.on_rotate_right
+ else:
+ self.encoderB = None
+
+ # Callback for left encoder rotation
+ def on_rotate_left(self):
+ steps = self.encoderA.steps
+ print(f"Left Encoder Steps: {steps} trigger")
+
+ # Callback for right encoder rotation
+ def on_rotate_right(self):
+ steps = self.encoderB.steps
+ #print(f"Right Encoder Steps: {steps}")
+
+ def move(self, speed=0.5, turn=0, t=0):
+ # Ensure speed and turn are within bounds
+ speed = max(-1, min(1, speed)) # Clamp speed to [-1, 1]
+ turn = max(-1, min(1, turn)) # Clamp turn to [-1, 1]
+
+ # Calculate speed for each motor
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+ # Clamp motor speeds to [-1, 1]
+ leftSpeed = max(-1, min(1, leftSpeed))
+ rightSpeed = max(-1, min(1, rightSpeed))
+
+ # Set left motor
+ self.pwmA.value = abs(leftSpeed)
+ self.in1A.value = leftSpeed > 0
+ self.in2A.value = leftSpeed <= 0
+
+ # Set right motor
+ self.pwmB.value = abs(rightSpeed)
+ self.in1B.value = rightSpeed > 0
+ self.in2B.value = rightSpeed <= 0
+
+ # Print debugging info
+ print(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+
+ # Run the motors for time 't' while monitoring encoder steps
+ start_time = time()
+ while time() - start_time < t:
+ # Optionally, read and print encoder values
+ if self.encoderA:
+ left_steps = self.encoderA.steps
+ print(f"Left Encoder Steps: {left_steps}")
+ if self.encoderB:
+ right_steps = self.encoderB.steps
+# print(f"Right Encoder Steps: {right_steps}")
+ sleep(0.1) # Adjust sleep time as needed
+
+ def stop(self, t=0):
+ self.pwmA.value = 0
+ self.pwmB.value = 0
+ sleep(t)
+
+def main():
+ # Define encoder GPIO pins for left and right encoders
+ # Replace these with your actual GPIO pins
+ left_encoder_pins = (5, 6) # GPIO pins for left encoder channels A and B
+ right_encoder_pins = (13, 19) # GPIO pins for right encoder channels A and B
+
+ # Initialize motor with encoder pins
+ motor1 = Motor(2, 3, 4, 17, 22, 27, encA_pins=left_encoder_pins, encB_pins=right_encoder_pins)
+
+ # Run motor commands
+ motor1.move(0.5, 0, 10) # Move forward for 3 seconds
+ motor1.stop(1)
+ '''
+ motor1.move(-0.5, 0, 3) # Move backward for 3 seconds
+ motor1.stop(1)
+ motor1.move(0.5, 0.5, 3) # Turn right for 3 seconds
+ motor1.stop(1)
+ motor1.move(0.5, -0.5, 3) # Turn left for 3 seconds
+ motor1.stop(1)
+ '''
+
+if __name__ == '__main__':
+ main()
diff --git a/Motor_controller_test_pi/Encoder_testing_2.py b/Motor_controller_test_pi/Encoder_testing_2.py
new file mode 100644
index 00000000..50f65f46
--- /dev/null
+++ b/Motor_controller_test_pi/Encoder_testing_2.py
@@ -0,0 +1,83 @@
+from gpiozero import RotaryEncoder, PWMOutputDevice
+from time import sleep
+
+class Motor():
+ def __init__(self, EnaA, In1A, In2A, EnaB, In1B, In2B):
+ self.pwmA = PWMOutputDevice(EnaA)
+ self.in1A = PWMOutputDevice(In1A)
+ self.in2A = PWMOutputDevice(In2A)
+ self.pwmB = PWMOutputDevice(EnaB)
+ self.in1B = PWMOutputDevice(In1B)
+ self.in2B = PWMOutputDevice(In2B)
+
+ def move(self, speed=0.5, turn=0, t=0):
+ # Ensure speed and turn are within bounds
+ speed = max(-1, min(1, speed)) # Clamp speed to [-1, 1]
+ turn = max(-1, min(1, turn)) # Clamp turn to [-1, 1]
+
+ # Calculate speed for each motor
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+ # Clamp motor speeds to [-1, 1]
+ leftSpeed = max(-1, min(1, leftSpeed))
+ rightSpeed = max(-1, min(1, rightSpeed))
+
+ # Set left motor
+ self.pwmA.value = abs(leftSpeed)
+ self.in1A.value = leftSpeed > 0
+ self.in2A.value = leftSpeed <= 0
+
+ # Set right motor
+ self.pwmB.value = abs(rightSpeed)
+ self.in1B.value = rightSpeed > 0
+ self.in2B.value = rightSpeed <= 0
+
+ # Print debugging info
+ print(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+
+ sleep(t)
+
+ def stop(self, t=0):
+ self.pwmA.value = 0
+ self.pwmB.value = 0
+ sleep(t)
+
+
+# Encoder GPIO pins (updated to avoid conflicts)
+MOTOR1_HALL_A = 5
+MOTOR1_HALL_B = 6
+MOTOR2_HALL_A = 13
+MOTOR2_HALL_B = 19
+
+# Create RotaryEncoder objects for encoders
+motor1_encoder = RotaryEncoder(MOTOR1_HALL_A, MOTOR1_HALL_B, max_steps=0)
+motor2_encoder = RotaryEncoder(MOTOR2_HALL_A, MOTOR2_HALL_B, max_steps=0)
+
+# Callback functions for rotation events
+def motor1_rotated():
+ print(f"Motor 1: Steps={motor1_encoder.steps}, Direction={'Clockwise' if motor1_encoder.value > 0 else 'Counter-Clockwise'}")
+
+def motor2_rotated():
+ print(f"Motor 2: Steps={motor2_encoder.steps}, Direction={'Clockwise' if motor2_encoder.value > 0 else 'Counter-Clockwise'}")
+
+# Attach callbacks to each encoder
+motor1_encoder.when_rotated = motor1_rotated
+motor2_encoder.when_rotated = motor2_rotated
+
+def main():
+ print("Starting motor and monitoring encoder outputs...")
+ motor1.move(0.1, 0, 3) # Move forward for 3 seconds
+ motor1.stop(1)
+ # You can add more motor movements here
+ print("Test complete.")
+ # Keep the program running to continue monitoring encoders
+ while True:
+ sleep(1)
+
+if __name__ == '__main__':
+ motor1 = Motor(2, 3, 4, 17, 22, 27)
+ try:
+ main()
+ except KeyboardInterrupt:
+ print("\nExiting program...")
diff --git a/Motor_controller_test_pi/IMU /Boasch/New_IMU.py b/Motor_controller_test_pi/IMU /Boasch/New_IMU.py
new file mode 100644
index 00000000..b31f56b3
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /Boasch/New_IMU.py
@@ -0,0 +1,45 @@
+import smbus
+import time
+
+class BNO055:
+ def __init__(self, i2c_bus=1, address=0x28):
+ self.bus = smbus.SMBus(i2c_bus)
+ self.address = address
+ self.init_bno055()
+
+ def write_register(self, register, value):
+ self.bus.write_byte_data(self.address, register, value)
+
+ def read_register(self, register, length=1):
+ if length == 1:
+ return self.bus.read_byte_data(self.address, register)
+ else:
+ return self.bus.read_i2c_block_data(self.address, register, length)
+
+ def init_bno055(self):
+ # Switch to CONFIG mode to configure sensor
+ self.write_register(0x3D, 0x00)
+ time.sleep(0.05)
+
+ # Set power mode to Normal
+ self.write_register(0x3E, 0x00)
+
+ # Set to NDOF mode (Full sensor fusion for orientation)
+ self.write_register(0x3D, 0x0C)
+ time.sleep(0.05)
+
+ def read_euler_angles(self):
+ # Euler angles (yaw, pitch, roll) registers
+ data = self.read_register(0x1A, 6)
+ yaw = (data[1] << 8 | data[0]) / 16.0
+ pitch = (data[3] << 8 | data[2]) / 16.0
+ roll = (data[5] << 8 | data[4]) / 16.0
+ return yaw, pitch, roll
+
+if __name__ == "__main__":
+ sensor = BNO055()
+ print("Starting BNO055 data collection...")
+ while True:
+ yaw, pitch, roll = sensor.read_euler_angles()
+ print(f"Yaw: {yaw:.2f}, Pitch: {pitch:.2f}, Roll: {roll:.2f}")
+ time.sleep(0.5)
diff --git a/Motor_controller_test_pi/IMU /Boasch/New_IMU_calibration.py b/Motor_controller_test_pi/IMU /Boasch/New_IMU_calibration.py
new file mode 100644
index 00000000..982c0a85
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /Boasch/New_IMU_calibration.py
@@ -0,0 +1,65 @@
+import smbus
+import time
+
+class BNO055:
+ def __init__(self, i2c_bus=1, address=0x28):
+ self.bus = smbus.SMBus(i2c_bus)
+ self.address = address
+ self.init_bno055()
+
+ def write_register(self, register, value):
+ self.bus.write_byte_data(self.address, register, value)
+
+ def read_register(self, register, length=1):
+ if length == 1:
+ return self.bus.read_byte_data(self.address, register)
+ else:
+ return self.bus.read_i2c_block_data(self.address, register, length)
+
+ def init_bno055(self):
+ # Switch to CONFIG mode to configure sensor
+ self.write_register(0x3D, 0x00)
+ time.sleep(0.05)
+
+ # Set power mode to Normal
+ self.write_register(0x3E, 0x00)
+
+ # Set to NDOF mode (Full sensor fusion for orientation)
+ self.write_register(0x3D, 0x0C)
+ time.sleep(0.05)
+
+ def read_calibration_status(self):
+ # Read the calibration status byte (CALIB_STAT register: 0x35)
+ calib = self.read_register(0x35)
+ sys = (calib >> 6) & 0x03 # System calibration status (bits 6-7)
+ gyro = (calib >> 4) & 0x03 # Gyroscope calibration status (bits 4-5)
+ accel = (calib >> 2) & 0x03 # Accelerometer calibration status (bits 2-3)
+ mag = calib & 0x03 # Magnetometer calibration status (bits 0-1)
+ return sys, gyro, accel, mag
+
+if __name__ == "__main__":
+ sensor = BNO055()
+ print("Starting BNO055 calibration process...")
+
+ while True:
+ sys, gyro, accel, mag = sensor.read_calibration_status()
+ print(f"System: {sys}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}")
+
+ if sys == 3 and gyro == 3 and accel == 3 and mag == 3:
+ print("Calibration Complete!")
+ break
+
+ print("Follow these steps to calibrate:")
+ print("- Keep sensor still to calibrate gyroscope.")
+ print("- Move sensor in all axes to calibrate accelerometer.")
+ print("- Perform figure-8 motion for magnetometer.")
+
+ time.sleep(1)
+'''
+def read_calibration_data(self):
+ return self.read_register(0x55, 22) # 22 bytes of calibration data
+
+def write_calibration_data(self, data):
+ for i in range(22):
+ self.write_register(0x55 + i, data[i])
+'''
\ No newline at end of file
diff --git a/Motor_controller_test_pi/IMU /IMU_Calibration.py b/Motor_controller_test_pi/IMU /IMU_Calibration.py
new file mode 100644
index 00000000..1f8bb91c
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /IMU_Calibration.py
@@ -0,0 +1,164 @@
+import smbus
+import time
+
+# MPU6050 Registers and Addresses
+DEVICE_ADDRESS = 0x68 # MPU6050 device address
+
+PWR_MGMT_1 = 0x6B
+SMPLRT_DIV = 0x19
+CONFIG = 0x1A
+GYRO_CONFIG = 0x1B
+ACCEL_CONFIG = 0x1C
+INT_ENABLE = 0x38
+
+ACCEL_XOUT_H = 0x3B
+ACCEL_YOUT_H = 0x3D
+ACCEL_ZOUT_H = 0x3F
+
+GYRO_XOUT_H = 0x43
+GYRO_YOUT_H = 0x45
+GYRO_ZOUT_H = 0x47
+
+def calibrate_gyroscope(samples=1000):
+ print("Calibrating gyroscope... Please keep the sensor stationary.")
+ gyro_x_offset = 0
+ gyro_y_offset = 0
+ gyro_z_offset = 0
+
+ for _ in range(samples):
+ gyro_x = read_raw_data(GYRO_XOUT_H)
+ gyro_y = read_raw_data(GYRO_YOUT_H)
+ gyro_z = read_raw_data(GYRO_ZOUT_H)
+
+ gyro_x_offset += gyro_x
+ gyro_y_offset += gyro_y
+ gyro_z_offset += gyro_z
+
+ time.sleep(0.00001) # Adjust the delay as per your sampling rate
+
+ gyro_x_offset /= samples
+ gyro_y_offset /= samples
+ gyro_z_offset /= samples
+
+ print("Gyroscope calibration complete.")
+ return gyro_x_offset, gyro_y_offset, gyro_z_offset
+
+def calibrate_accelerometer(samples=1000):
+ print("Calibrating gyroscope... Please keep the sensor stationary.")
+ acc_x_offset = 0
+ acc_y_offset = 0
+ acc_z_offset = 0
+
+ for _ in range(samples):
+ acc_x = read_raw_data(ACCEL_XOUT_H)
+ acc_y = read_raw_data(ACCEL_YOUT_H)
+ acc_z = read_raw_data(ACCEL_ZOUT_H)
+
+ acc_x_offset += acc_x
+ acc_y_offset += acc_y
+ acc_z_offset += acc_z
+
+ time.sleep(0.00001) # Adjust the delay as per your sampling rate
+
+ acc_x_offset /= samples
+ acc_y_offset /= samples
+ acc_z_offset /= samples
+
+
+ print("Gyroscope calibration complete.")
+ return acc_x_offset, acc_y_offset, acc_z_offset
+
+
+
+
+
+def MPU_Init():
+ # Write to sample rate register
+ bus.write_byte_data(DEVICE_ADDRESS, SMPLRT_DIV, 7)
+ # Write to power management register to wake up the MPU6050
+ bus.write_byte_data(DEVICE_ADDRESS, PWR_MGMT_1, 1)
+ # Write to configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, CONFIG, 0)
+ # Write to gyroscope configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, GYRO_CONFIG, 24)
+ # Write to accelerometer configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, ACCEL_CONFIG, 0)
+ # Enable interrupts
+ bus.write_byte_data(DEVICE_ADDRESS, INT_ENABLE, 1)
+
+
+
+
+def read_raw_data(addr):
+ # Read two bytes of data from the given address
+ high = bus.read_byte_data(DEVICE_ADDRESS, addr)
+ low = bus.read_byte_data(DEVICE_ADDRESS, addr+1)
+ # Combine higher and lower bytes
+ value = (high << 8) | low
+ # Convert to signed integer
+ if value > 32768:
+ value = value - 65536
+ return value
+
+
+
+# Initialize the I2C bus
+bus = smbus.SMBus(1) # Use bus number 1 for Raspberry Pi
+
+# Initialize MPU6050
+MPU_Init()
+
+print("Reading data from MPU6050...")
+
+gyro_x_offset, gyro_y_offset, gyro_z_offset = calibrate_gyroscope()
+acc_x_offset, acc_y_offset, acc_z_offset = calibrate_accelerometer()
+
+try:
+# while True:
+ # Read accelerometer data
+ acc_x = read_raw_data(ACCEL_XOUT_H)
+ acc_y = read_raw_data(ACCEL_YOUT_H)
+ acc_z = read_raw_data(ACCEL_ZOUT_H)
+
+ # Read gyroscope data
+ gyro_x = read_raw_data(GYRO_XOUT_H)
+ gyro_y = read_raw_data(GYRO_YOUT_H)
+ gyro_z = read_raw_data(GYRO_ZOUT_H)
+
+
+ # Convert raw data to meaningful units
+ Ax = acc_x / 16384.0 # Accelerometer sensitivity scale factor
+ Ay = acc_y / 16384.0
+ Az = acc_z / 16384.0
+
+ Gx = gyro_x / 131.0 # Gyroscope sensitivity scale factor
+ Gy = gyro_y / 131.0
+ Gz = gyro_z / 131.0
+
+
+ Aox = acc_x_offset / 16384.0 # Accelerometer sensitivity scale factor
+ Aoy = acc_y_offset / 16384.0
+ Aoz = (acc_z_offset / 16384.0)-1
+
+
+ Gox = gyro_x_offset / 131.0
+ Goy = gyro_y_offset / 131.0
+ Goz = gyro_z_offset / 131.0
+
+ # Print the results
+ #print(f"Accelerometer (g): X={Ax:.8f}, Y={Ay:.8f}, Z={Az:.8f}")
+ #print(f"Gyroscope (°/s): X={Gx:.8f}, Y={Gy:.8f}, Z={Gz:.8f}")
+ print("-" * 50)
+
+ # Print the offset
+ print(f"Accelerometer Offset (g): X={Aox:.8f}, Y={Aoy:.8f}, Z={Aoz:.8f}")
+ print(f"Gyroscope Offset (°/s): X={Gox:.8f}, Y={Goy:.8f}, Z={Goz:.8f}")
+ print("-" * 50)
+
+
+ # Delay for readability
+ # time.sleep(0.1)
+
+except KeyboardInterrupt:
+ print("\nTerminated by User")
+
\ No newline at end of file
diff --git a/Motor_controller_test_pi/IMU /IMU_Library.py b/Motor_controller_test_pi/IMU /IMU_Library.py
new file mode 100644
index 00000000..db740111
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /IMU_Library.py
@@ -0,0 +1,47 @@
+import mpu6050
+import time
+
+# Create a new Mpu6050 object
+mpu6050 = mpu6050.mpu6050(0x68)
+
+# Define a function to read the sensor data
+def read_sensor_data():
+ # Read the accelerometer values
+ accelerometer_data = mpu6050.get_accel_data()
+
+ # Read the gyroscope values
+ gyroscope_data = mpu6050.get_gyro_data()
+
+ # Read temp
+ temperature = mpu6050.get_temp()
+
+ return accelerometer_data, gyroscope_data, temperature
+
+# Start a while loop to continuously read the sensor data
+while True:
+
+ # Read the sensor data
+ accelerometer_data, gyroscope_data, temperature = read_sensor_data()
+
+ Ax = accelerometer_data['x']
+ Ay = accelerometer_data['y']
+ Az = accelerometer_data['z']
+
+ Gx = gyroscope_data['x']
+ Gy = gyroscope_data['y']
+ Gz = gyroscope_data['z']
+
+ # Print the results
+ print(f"Accelerometer (g): X={Ax:.8f}, Y={Ay:.8f}, Z={Az:.8f}")
+ print(f"Gyroscope (°/s): X={Gx:.8f}, Y={Gy:.8f}, Z={Gz:.8f}")
+ print("-" * 50)
+
+
+ # Print the sensor data
+# print("Accelerometer data:", accelerometer_data)
+# print("Gyroscope data:", gyroscope_data)
+# print("--------------------")
+ #print("Temp:", temperature)
+
+ # Wait for 1 second
+ time.sleep(0.1)
\ No newline at end of file
diff --git a/Motor_controller_test_pi/IMU /IMU_smbus.py b/Motor_controller_test_pi/IMU /IMU_smbus.py
new file mode 100644
index 00000000..3aa441fe
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /IMU_smbus.py
@@ -0,0 +1,99 @@
+import smbus
+import time
+import math
+
+# MPU6050 Registers and Addresses
+DEVICE_ADDRESS = 0x68 # MPU6050 device address
+
+PWR_MGMT_1 = 0x6B
+SMPLRT_DIV = 0x19
+CONFIG = 0x1A
+GYRO_CONFIG = 0x1B
+ACCEL_CONFIG = 0x1C
+INT_ENABLE = 0x38
+
+ACCEL_XOUT_H = 0x3B
+ACCEL_YOUT_H = 0x3D
+ACCEL_ZOUT_H = 0x3F
+
+GYRO_XOUT_H = 0x43
+GYRO_YOUT_H = 0x45
+GYRO_ZOUT_H = 0x47
+
+def MPU_Init():
+ # Write to sample rate register
+ bus.write_byte_data(DEVICE_ADDRESS, SMPLRT_DIV, 7)
+ # Write to power management register to wake up the MPU6050
+ bus.write_byte_data(DEVICE_ADDRESS, PWR_MGMT_1, 1)
+ # Write to configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, CONFIG, 0)
+ # Write to gyroscope configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, GYRO_CONFIG, 24)
+ # Write to accelerometer configuration register
+ bus.write_byte_data(DEVICE_ADDRESS, ACCEL_CONFIG, 0)
+ # Enable interrupts
+ bus.write_byte_data(DEVICE_ADDRESS, INT_ENABLE, 1)
+
+def read_raw_data(addr):
+ # Read two bytes of data from the given address
+ high = bus.read_byte_data(DEVICE_ADDRESS, addr)
+ low = bus.read_byte_data(DEVICE_ADDRESS, addr+1)
+ # Combine higher and lower bytes
+ value = (high << 8) | low
+ # Convert to signed integer
+ if value > 32768:
+ value = value - 65536
+ return value
+
+# Initialize the I2C bus
+bus = smbus.SMBus(1) # Use bus number 1 for Raspberry Pi
+
+# Initialize MPU6050
+MPU_Init()
+
+print("Reading data from MPU6050...")
+
+try:
+ while True:
+ # Read accelerometer data
+ acc_x = read_raw_data(ACCEL_XOUT_H)
+ acc_y = read_raw_data(ACCEL_YOUT_H)
+ acc_z = read_raw_data(ACCEL_ZOUT_H)
+
+ # Read gyroscope data
+ gyro_x = read_raw_data(GYRO_XOUT_H)
+ gyro_y = read_raw_data(GYRO_YOUT_H)
+ gyro_z = read_raw_data(GYRO_ZOUT_H)
+
+ # Convert raw data to meaningful units
+ Ax = (acc_x / 16384.0 - 0.01467432) # Accelerometer sensitivity scale factor
+ Ay = (acc_y / 16384.0 - -0.00603101)
+ Az = (acc_z / 16384.0 - 0.06171924)
+
+ Ax_m = 9.81*Ax
+ Ay_m = 9.81*Ay
+ Az_m = 9.81*Az
+
+ Gx = gyro_x / 131.0 --0.17447328# Gyroscope sensitivity scale factor
+ Gy = gyro_y / 131.0 -0.08720611
+ Gz = gyro_z / 131.0 -0.16423664
+
+ Gx_r = Gx/180 *math.pi
+ Gy_r = Gy/180 *math.pi
+ Gz_r = Gz/180 *math.pi
+
+ # Print the results
+ print(f"Accelerometer (g): X={Ax:.3f}, Y={Ay:.3f}, Z={Az:.3f}")
+ print(f"Gyroscope (°/s): X={Gx:.3f}, Y={Gy:.3f}, Z={Gz:.3f}")
+
+ print(f"Accelerometer (m/s^2): X={Ax_m:.3f}, Y={Ay_m:.3f}, Z={Az_m:.3f}")
+ print(f"Gyroscope (rad/s): X={Gx_r:.3f}, Y={Gy_r:.3f}, Z={Gz_r:.3f}")
+ print("-" * 50)
+
+ # Delay for readability
+ time.sleep(0.1)
+
+except KeyboardInterrupt:
+ print("\nTerminated by User")
+
+
diff --git a/Motor_controller_test_pi/IMU /ahrs_library.py b/Motor_controller_test_pi/IMU /ahrs_library.py
new file mode 100644
index 00000000..9d98799e
--- /dev/null
+++ b/Motor_controller_test_pi/IMU /ahrs_library.py
@@ -0,0 +1,118 @@
+import smbus
+import time
+import math
+from ahrs.filters import Madgwick
+
+class IMUReader:
+ def __init__(self):
+ self.bus = smbus.SMBus(1) # I2C bus
+ self.DEVICE_ADDRESS = 0x68 # MPU6050 device address
+ self.mpu_init()
+
+ self.filter = Madgwick()
+ self.orientation = [0.0, 0.0, 0.0, 1.0] # Initial quaternion
+
+ def mpu_init(self):
+ PWR_MGMT_1 = 0x6B
+ SMPLRT_DIV = 0x19
+ CONFIG = 0x1A
+ GYRO_CONFIG = 0x1B
+ ACCEL_CONFIG = 0x1C
+ INT_ENABLE = 0x38
+
+ # Write to sample rate register
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, SMPLRT_DIV, 7)
+ # Write to power management register to wake up the MPU6050
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, PWR_MGMT_1, 1)
+ # Write to configuration register
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, CONFIG, 0)
+ # Write to gyroscope configuration register
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, GYRO_CONFIG, 24)
+ # Write to accelerometer configuration register
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, ACCEL_CONFIG, 0)
+ # Enable interrupts
+ self.bus.write_byte_data(self.DEVICE_ADDRESS, INT_ENABLE, 1)
+
+ def read_raw_data(self, addr):
+ # Read two bytes of data from the given address
+ high = self.bus.read_byte_data(self.DEVICE_ADDRESS, addr)
+ low = self.bus.read_byte_data(self.DEVICE_ADDRESS, addr + 1)
+ value = (high << 8) | low
+ if value > 32768:
+ value = value - 65536
+ return value
+
+ def read_data(self):
+ ACCEL_XOUT_H = 0x3B
+ ACCEL_YOUT_H = 0x3D
+ ACCEL_ZOUT_H = 0x3F
+ GYRO_XOUT_H = 0x43
+ GYRO_YOUT_H = 0x45
+ GYRO_ZOUT_H = 0x47
+
+ # Read accelerometer data
+ acc_x = self.read_raw_data(ACCEL_XOUT_H)
+ acc_y = self.read_raw_data(ACCEL_YOUT_H)
+ acc_z = self.read_raw_data(ACCEL_ZOUT_H)
+
+ # Read gyroscope data
+ gyro_x = self.read_raw_data(GYRO_XOUT_H)
+ gyro_y = self.read_raw_data(GYRO_YOUT_H)
+ gyro_z = self.read_raw_data(GYRO_ZOUT_H)
+
+ ax_offset = 0.01467432
+ ay_offset = -0.00603101
+ az_offset = 0.06171924
+
+ gx_offset = -0.17447328
+ gy_offset = 0.08720611
+ gz_offset = 0.16423664
+
+ Ax = (acc_x / 16384.0) - ax_offset
+ Ay = (acc_y / 16384.0) - ay_offset
+ Az = (acc_z / 16384.0) - az_offset
+ Ax = 9.81 * Ax
+ Ay = 9.81 * Ay
+ Az = 9.81 * Az
+
+ Gx = (gyro_x / 131.0) - gx_offset
+ Gy = (gyro_y / 131.0) - gy_offset
+ Gz = (gyro_z / 131.0) - gz_offset
+
+ Gx = Gx / 180 * math.pi
+ Gy = Gy / 180 * math.pi
+ Gz = Gz / 180 * math.pi
+
+ self.orientation = self.filter.updateIMU(self.orientation,
+ [Gx, Gy, Gz],
+ [Ax / 9.81, Ay / 9.81, Az / 9.81])
+ return Ax, Ay, Az, Gx, Gy, Gz, self.orientation
+
+ def start(self):
+ print("Starting IMU data collection...")
+ while True:
+ Ax, Ay, Az, Gx, Gy, Gz, orientation = self.read_data()
+
+ # Debugging: Raw data
+ print(f"Raw Accelerometer: Ax={Ax:.2f}, Ay={Ay:.2f}, Az={Az:.2f}")
+ print(f"Raw Gyroscope: Gx={Gx:.2f}, Gy={Gy:.2f}, Gz={Gz:.2f}")
+
+ # Debugging: Orientation
+ print(f"Orientation (quaternion): x={orientation[0]:.2f}, y={orientation[1]:.2f}, "
+ f"z={orientation[2]:.2f}, w={orientation[3]:.2f}")
+
+ # Test stability
+ print("Orientation (Euler angles):")
+ roll = math.atan2(2.0 * (orientation[3] * orientation[0] + orientation[1] * orientation[2]),
+ 1.0 - 2.0 * (orientation[0]**2 + orientation[1]**2))
+ pitch = math.asin(2.0 * (orientation[3] * orientation[1] - orientation[2] * orientation[0]))
+ yaw = math.atan2(2.0 * (orientation[3] * orientation[2] + orientation[0] * orientation[1]),
+ 1.0 - 2.0 * (orientation[1]**2 + orientation[2]**2))
+ print(f"Roll={roll:.2f}, Pitch={pitch:.2f}, Yaw={yaw:.2f}")
+
+ time.sleep(0.5)
+
+
+if __name__ == "__main__":
+ imu_reader = IMUReader()
+ imu_reader.start()
diff --git a/Motor_controller_test_pi/Motor_control/Motor_control.py b/Motor_controller_test_pi/Motor_control/Motor_control.py
new file mode 100644
index 00000000..425372b0
--- /dev/null
+++ b/Motor_controller_test_pi/Motor_control/Motor_control.py
@@ -0,0 +1,132 @@
+import RPi.GPIO as GPIO
+from time import sleep
+
+GPIO.setmode(GPIO.BCM)
+GPIO.setwarnings(False)
+
+class Motor():
+ def __init__(self, EnaA, In1A, In2A,EnaB, In1B, In2B):
+ self.EnaA = EnaA
+ self.In1A = In1A
+ self.In2A = In2A
+ self.EnaB = EnaB
+ self.In1B = In1B
+ self.In2B = In2B
+ print(EnaB,In1B,In2B)
+ GPIO.setup(EnaA, GPIO.OUT)
+ GPIO.setup(In1A, GPIO.OUT)
+ GPIO.setup(In2A, GPIO.OUT)
+ GPIO.setup(EnaB, GPIO.OUT)
+ GPIO.setup(In1B, GPIO.OUT)
+ GPIO.setup(In2B, GPIO.OUT)
+ self.pwmA = GPIO.PWM(EnaA, 100)
+ self.pwmA.start(0)
+ self.pwmB = GPIO.PWM(EnaB, 100)
+ self.pwmB.start(0)
+
+ def move(self,speed=0.5,turn = 0,t = 0):
+
+ if turn > 1:
+ turn = 1
+ elif turn < -1:
+ turn = -1
+
+ if turn >0.2:
+ print("Turnning Left.. (",turn,")")
+ elif turn < -0.2:
+ print("Turnning Right.. (",turn,")")
+ else:
+ if speed > 0:
+ print("Going Straight")
+ else:
+ print("Going backward")
+
+
+ #scale
+ speed *= 100
+ turn *= 100
+
+ #turn : 1 -> right, 0 -> left (-100 to 200)
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+
+ if leftSpeed > 100:
+ leftSpeed = 100
+ elif leftSpeed < -100:
+ leftSpeed = -100
+ if rightSpeed > 100:
+ rightSpeed = 100
+ elif rightSpeed < -100:
+ rightSpeed = -100
+
+ self.pwmA.ChangeDutyCycle(abs(leftSpeed))
+ self.pwmB.ChangeDutyCycle(abs(rightSpeed))
+
+
+
+ if leftSpeed>0:
+ GPIO.output(self.In1A,GPIO.HIGH) #Left
+ GPIO.output(self.In2A,GPIO.LOW) #Right
+ else:
+ # leftSpeed = 0
+ GPIO.output(self.In1A,GPIO.LOW)
+ GPIO.output(self.In2A,GPIO.HIGH)
+
+ if rightSpeed>0:
+ GPIO.output(self.In1B,GPIO.HIGH)
+ GPIO.output(self.In2B,GPIO.LOW)
+ else:
+ # rifhtSpeed = 0
+ GPIO.output(self.In1B,GPIO.LOW)
+ GPIO.output(self.In2B,GPIO.HIGH)
+
+ #print(f"Speed: {speed:.2f} ,Left: {leftSpeed:.2f} Right: {rightSpeed:.2f} TURN:{turn:.6f}")
+
+
+ sleep(t)
+
+
+ def stop(self,t=0):
+ self.pwmA.ChangeDutyCycle(0)
+ self.pwmB.ChangeDutyCycle(0)
+ sleep(t)
+
+
+
+
+def main():
+
+ motor1.stop(1)
+ motor1.move(0.5,0,10) #turn right
+ motor1.stop(1)
+ '''
+ motor1.move(-1,0,0.5) #turn right
+ motor1.stop(1)
+ motor1.move(1,1,0.5) #turn right
+ motor1.stop(1)
+ motor1.move(-1,-1,0.5) #turn right
+ motor1.stop(1)
+ motor1.move(1,-1,0.5) #turn right
+ motor1.stop(1)
+ motor1.move(-1,1,0.5) #turn right
+ motor1.stop(1)
+
+ motor1.move(0.1 , 0,3) #turn right
+ motor1.stop(1)
+ motor1.move(-0.1,0,3) #turn right
+ motor1.stop(1)
+ '''
+ #motor1.move(0.33,0.8,5) #turn right
+ motor1.stop(1)
+
+
+import sys
+if __name__ == '__main__' :
+ print(sys.version)
+
+
+ #motor1 = Motor(2,3,4,17,22,27)
+ motor1 = Motor(14, 15, 18, 17, 22, 27)
+ main()
+
\ No newline at end of file
diff --git a/Motor_controller_test_pi/Motor_control/Ros2_Motor_control.py b/Motor_controller_test_pi/Motor_control/Ros2_Motor_control.py
new file mode 100644
index 00000000..716afa3f
--- /dev/null
+++ b/Motor_controller_test_pi/Motor_control/Ros2_Motor_control.py
@@ -0,0 +1,68 @@
+from gpiozero import PWMOutputDevice
+from time import sleep
+
+class Motor():
+ def __init__(self, EnaA, In1A, In2A, EnaB, In1B, In2B):
+ self.pwmA = PWMOutputDevice(EnaA)
+ self.in1A = PWMOutputDevice(In1A)
+ self.in2A = PWMOutputDevice(In2A)
+ self.pwmB = PWMOutputDevice(EnaB)
+ self.in1B = PWMOutputDevice(In1B)
+ self.in2B = PWMOutputDevice(In2B)
+
+ def move(self, speed=0.5, turn=0, t=0):
+ # Ensure speed and turn are within bounds
+ speed = max(-1, min(1, speed)) # Clamp speed to [-1, 1]
+ turn = max(-1, min(1, turn)) # Clamp turn to [-1, 1]
+
+ # Calculate speed for each motor
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+ wheel_base = 0
+ '''
+ left_speed = speed - (turn* wheel_base / 2)
+ right_speed = peed + (turn* wheel_base / 2)
+ '''
+
+ # Clamp motor speeds to [-1, 1]
+ leftSpeed = max(-1, min(1, leftSpeed))
+ rightSpeed = max(-1, min(1, rightSpeed))
+
+ # Set left motor
+ self.pwmA.value = abs(leftSpeed)
+ self.in1A.value = leftSpeed > 0
+ self.in2A.value = leftSpeed <= 0
+
+ # Set right motor
+ self.pwmB.value = abs(rightSpeed)
+ self.in1B.value = rightSpeed > 0
+ self.in2B.value = rightSpeed <= 0
+
+ # Print debugging info
+ print(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+
+ sleep(t)
+
+ def stop(self, t=0):
+ self.pwmA.value = 0
+ self.pwmB.value = 0
+ sleep(t)
+
+def main():
+ #motor1.move(0.5, 0, 0.865) # Move forward
+ motor1.move(0.5, 0, 3) # Move forward
+ motor1.stop(1)
+ '''
+ motor1.move(-0.5, 0, 3) # Move backward
+ motor1.stop(1)
+ motor1.move(0.5, 0.5, 3) # Turn right
+ motor1.stop(1)
+ motor1.move(0.5, -0.5, 3) # Turn left
+ motor1.stop(1)
+ '''
+
+if __name__ == '__main__':
+ motor1 = Motor(14, 15, 18,17 ,22 ,27 )
+ main()
+
\ No newline at end of file
diff --git a/Motor_controller_test_pi/Test_encoder_motor.py b/Motor_controller_test_pi/Test_encoder_motor.py
new file mode 100644
index 00000000..7e658535
--- /dev/null
+++ b/Motor_controller_test_pi/Test_encoder_motor.py
@@ -0,0 +1,104 @@
+from gpiozero import PWMOutputDevice, Button
+from time import sleep
+
+class Motor():
+ def __init__(self, EnaA, In1A, In2A, EnaB, In1B, In2B, HallA_A, HallA_B, HallB_A, HallB_B):
+ # Motor control initialization
+ self.pwmA = PWMOutputDevice(EnaA)
+ self.in1A = PWMOutputDevice(In1A)
+ self.in2A = PWMOutputDevice(In2A)
+ self.pwmB = PWMOutputDevice(EnaB)
+ self.in1B = PWMOutputDevice(In1B)
+ self.in2B = PWMOutputDevice(In2B)
+
+ # Encoder initialization
+ self.motor1_hall_a = Button(HallA_A, pull_up=True)
+ self.motor1_hall_b = Button(HallA_B, pull_up=True)
+ self.motor2_hall_a = Button(HallB_A, pull_up=True)
+ self.motor2_hall_b = Button(HallB_B, pull_up=True)
+
+ # Attach callbacks
+ self.motor1_hall_a.when_pressed = self.motor1_channel_a_detected
+ self.motor1_hall_b.when_pressed = self.motor1_channel_b_detected
+ self.motor2_hall_a.when_pressed = self.motor2_channel_a_detected
+ self.motor2_hall_b.when_pressed = self.motor2_channel_b_detected
+
+ # Encoder counters
+ self.motor1_count = 0
+ self.motor2_count = 0
+
+ def move(self, speed=0.5, turn=0, t=0):
+ # Ensure speed and turn are within bounds
+ speed = max(-1, min(1, speed)) # Clamp speed to [-1, 1]
+ turn = max(-1, min(1, turn)) # Clamp turn to [-1, 1]
+
+ # Calculate speed for each motor
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+ # Clamp motor speeds to [-1, 1]
+ leftSpeed = max(-1, min(1, leftSpeed))
+ rightSpeed = max(-1, min(1, rightSpeed))
+
+ # Set left motor
+ self.pwmA.value = abs(leftSpeed)
+ self.in1A.value = leftSpeed > 0
+ self.in2A.value = leftSpeed <= 0
+
+ # Set right motor
+ self.pwmB.value = abs(rightSpeed)
+ self.in1B.value = rightSpeed > 0
+ self.in2B.value = rightSpeed <= 0
+
+ # Print debugging info
+ print(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+ sleep(t)
+
+
+
+ def stop(self, t=0):
+ self.pwmA.value = 0
+ self.pwmB.value = 0
+ sleep(t)
+
+
+
+
+ # Encoder callback functions
+ def motor1_channel_a_detected(self):
+ self.motor1_count += 1
+ print(f"Motor 1: Channel A pulse detected, Count: {self.motor1_count}")
+
+ def motor1_channel_b_detected(self):
+ print(f"Motor 1: Channel B pulse detected")
+
+
+ def motor2_channel_a_detected(self):
+ self.motor2_count += 1
+ print(f"Motor 2: Channel A pulse detected, Count: {self.motor2_count}")
+
+ def motor2_channel_b_detected(self):
+ print(f"Motor 2: Channel B pulse detected")
+
+
+
+
+def main():
+
+ try:
+ print("Starting motor movement and encoder monitoring. Press Ctrl+C to exit.")
+
+ motor1.move(0.1, 0, 10) # Move forward
+ motor1.stop(1)
+
+
+
+ except KeyboardInterrupt:
+ print("\nExiting program...")
+
+
+
+if __name__ == '__main__':
+ motor1 = Motor(2, 3, 4, 17, 22, 27, 5,6,20,21 )
+ main()
+
diff --git a/Motor_controller_test_pi/pigpio.py b/Motor_controller_test_pi/pigpio.py
new file mode 100644
index 00000000..2e47c7b8
--- /dev/null
+++ b/Motor_controller_test_pi/pigpio.py
@@ -0,0 +1,135 @@
+import pigpio
+from gpiozero import PWMOutputDevice
+from time import sleep, time
+import threading
+
+# Motor control class
+class Motor:
+ def __init__(self, EnaA, In1A, In2A, EnaB, In1B, In2B):
+ # Initialize PWM and GPIO pins for both motors
+ self.pwmA = PWMOutputDevice(EnaA)
+ self.in1A = PWMOutputDevice(In1A)
+ self.in2A = PWMOutputDevice(In2A)
+ self.pwmB = PWMOutputDevice(EnaB)
+ self.in1B = PWMOutputDevice(In1B)
+ self.in2B = PWMOutputDevice(In2B)
+
+ def move(self, speed=0.5, turn=0, t=0):
+ # Ensure speed and turn are within bounds
+ speed = max(-1, min(1, speed)) # Clamp speed to [-1, 1]
+ turn = max(-1, min(1, turn)) # Clamp turn to [-1, 1]
+
+ # Calculate speed for each motor
+ leftSpeed = speed - turn
+ rightSpeed = speed + turn
+
+ # Clamp motor speeds to [-1, 1]
+ leftSpeed = max(-1, min(1, leftSpeed))
+ rightSpeed = max(-1, min(1, rightSpeed))
+
+ # Set left motor
+ self.pwmA.value = abs(leftSpeed)
+ self.in1A.value = leftSpeed > 0
+ self.in2A.value = leftSpeed <= 0
+
+ # Set right motor
+ self.pwmB.value = abs(rightSpeed)
+ self.in1B.value = rightSpeed > 0
+ self.in2B.value = rightSpeed <= 0
+
+ # Print debugging info
+ print(f"Left Motor: Speed={leftSpeed}, Right Motor: Speed={rightSpeed}")
+
+ sleep(t)
+
+ def stop(self, t=0):
+ self.pwmA.value = 0
+ self.pwmB.value = 0
+ sleep(t)
+
+# Encoder class using pigpio
+class Encoder:
+ def __init__(self, gpioA, gpioB):
+ self.pi = pigpio.pi()
+ if not self.pi.connected:
+ raise IOError("Could not connect to pigpio daemon!")
+ self.gpioA = gpioA
+ self.gpioB = gpioB
+ self.position = 0
+
+ self.pi.set_mode(gpioA, pigpio.INPUT)
+ self.pi.set_mode(gpioB, pigpio.INPUT)
+
+ self.pi.set_pull_up_down(gpioA, pigpio.PUD_UP)
+ self.pi.set_pull_up_down(gpioB, pigpio.PUD_UP)
+
+ self.lastGpio = None
+
+ self.callbackA = self.pi.callback(gpioA, pigpio.EITHER_EDGE, self._pulse)
+ self.callbackB = self.pi.callback(gpioB, pigpio.EITHER_EDGE, self._pulse)
+
+ def _pulse(self, gpio, level, tick):
+ if gpio == self.gpioA:
+ levelB = self.pi.read(self.gpioB)
+ if level == levelB:
+ self.position += 1
+ else:
+ self.position -= 1
+ else:
+ levelA = self.pi.read(self.gpioA)
+ if level == levelA:
+ self.position -= 1
+ else:
+ self.position += 1
+
+ def get_position(self):
+ return self.position
+
+ def cancel(self):
+ self.callbackA.cancel()
+ self.callbackB.cancel()
+ self.pi.stop()
+
+def main():
+ # Initialize motor and encoder
+ motor = Motor(2, 3, 4, 17, 22, 27)
+ encoder1 = Encoder(5, 6)
+ encoder2 = Encoder(13, 19)
+
+ try:
+ # Start motor movement in a separate thread
+ def move_motor():
+ print("Starting motor...")
+ motor.move(0.5, 0) # Start moving forward
+ sleep(3) # Move for 3 seconds
+ motor.stop()
+ print("Motor stopped.")
+
+ motor_thread = threading.Thread(target=move_motor)
+ motor_thread.start()
+
+ # Read encoder values while motor is running
+ start_time = time()
+ while motor_thread.is_alive():
+ pos1 = encoder1.get_position()
+ pos2 = encoder2.get_position()
+ elapsed_time = time() - start_time
+ print(f"Time: {elapsed_time:.2f}s, Motor 1 Position: {pos1}, Motor 2 Position: {pos2}")
+ sleep(0.1) # Adjust the sleep time as needed
+
+ # Final encoder positions
+ pos1 = encoder1.get_position()
+ pos2 = encoder2.get_position()
+ print(f"Final Positions - Motor 1: {pos1}, Motor 2: {pos2}")
+
+ except KeyboardInterrupt:
+ print("Program interrupted by user.")
+ finally:
+ # Clean up
+ encoder1.cancel()
+ encoder2.cancel()
+ motor.stop()
+ print("Program terminated.")
+
+if __name__ == '__main__':
+ main()
--
GitLab