128 lines
5.4 KiB
Python
128 lines
5.4 KiB
Python
#!/usr/bin/env python
|
|
import rospy
|
|
from std_msgs.msg import String
|
|
|
|
import time
|
|
import numpy as np
|
|
|
|
import Adafruit_PCA9685
|
|
|
|
|
|
class Motors():
|
|
'''
|
|
Motors class contains all functions to control the steering and driving
|
|
'''
|
|
|
|
# Define wheel names
|
|
FL, FR, CL, CR, RL, RR = range(0, 6)
|
|
|
|
# Motor commands are assuming positiv=driving_forward, negative=driving_backwards.
|
|
# The driving direction of the left side has to be inverted for this to apply to all wheels.
|
|
wheel_directions = [-1, 1, -1, 1, -1, 1]
|
|
|
|
# 1 fl-||-fr 2
|
|
# ||
|
|
# 3 cl-||-cr 4
|
|
# 5 rl====rr 6
|
|
|
|
def __init__(self):
|
|
|
|
# Dictionary containing the pins of all motors
|
|
self.pins = {
|
|
'drive': {},
|
|
'steer': {}
|
|
}
|
|
|
|
# Set variables for the GPIO motor pins
|
|
self.pins['drive'][self.FL] = rospy.get_param("pin_drive_fl")
|
|
self.pins['steer'][self.FL] = rospy.get_param("pin_steer_fl")
|
|
|
|
self.pins['drive'][self.FR] = rospy.get_param("pin_drive_fr")
|
|
self.pins['steer'][self.FR] = rospy.get_param("pin_steer_fr")
|
|
|
|
self.pins['drive'][self.CL] = rospy.get_param("pin_drive_cl")
|
|
self.pins['steer'][self.CL] = rospy.get_param("pin_steer_cl")
|
|
|
|
self.pins['drive'][self.CR] = rospy.get_param("pin_drive_cr")
|
|
self.pins['steer'][self.CR] = rospy.get_param("pin_steer_cr")
|
|
|
|
self.pins['drive'][self.RL] = rospy.get_param("pin_drive_rl")
|
|
self.pins['steer'][self.RL] = rospy.get_param("pin_steer_rl")
|
|
|
|
self.pins['drive'][self.RR] = rospy.get_param("pin_drive_rr")
|
|
self.pins['steer'][self.RR] = rospy.get_param("pin_steer_rr")
|
|
|
|
# PWM characteristics
|
|
self.pwm = Adafruit_PCA9685.PCA9685(busnum=1)
|
|
self.pwm.set_pwm_freq(50) # Hz
|
|
|
|
self.steering_pwm_neutral = [None] * 6
|
|
|
|
self.steering_pwm_neutral[self.FL] = rospy.get_param("steer_pwm_neutral_fl")
|
|
self.steering_pwm_neutral[self.FR] = rospy.get_param("steer_pwm_neutral_fr")
|
|
self.steering_pwm_neutral[self.CL] = rospy.get_param("steer_pwm_neutral_cl")
|
|
self.steering_pwm_neutral[self.CR] = rospy.get_param("steer_pwm_neutral_cr")
|
|
self.steering_pwm_neutral[self.RL] = rospy.get_param("steer_pwm_neutral_rl")
|
|
self.steering_pwm_neutral[self.RR] = rospy.get_param("steer_pwm_neutral_rr")
|
|
self.steering_pwm_range = rospy.get_param("steer_pwm_range")
|
|
|
|
self.driving_pwm_low_limit = 100
|
|
self.driving_pwm_neutral = [None] * 6
|
|
self.driving_pwm_neutral[self.FL] = rospy.get_param("drive_pwm_neutral_fl")
|
|
self.driving_pwm_neutral[self.FR] = rospy.get_param("drive_pwm_neutral_fr")
|
|
self.driving_pwm_neutral[self.CL] = rospy.get_param("drive_pwm_neutral_cl")
|
|
self.driving_pwm_neutral[self.CR] = rospy.get_param("drive_pwm_neutral_cr")
|
|
self.driving_pwm_neutral[self.RL] = rospy.get_param("drive_pwm_neutral_rl")
|
|
self.driving_pwm_neutral[self.RR] = rospy.get_param("drive_pwm_neutral_rr")
|
|
self.driving_pwm_upper_limit = 500
|
|
self.driving_pwm_range = rospy.get_param("drive_pwm_range")
|
|
|
|
# Set steering motors to neutral values (straight)
|
|
for wheel_name, motor_pin in self.pins['steer'].items():
|
|
self.pwm.set_pwm(motor_pin, 0,
|
|
self.steering_pwm_neutral[wheel_name])
|
|
time.sleep(0.1)
|
|
|
|
self.wiggle()
|
|
|
|
def wiggle(self):
|
|
time.sleep(0.1)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
|
|
int(self.steering_pwm_neutral[self.FL] + self.steering_pwm_range * 0.3))
|
|
time.sleep(0.1)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
|
|
int(self.steering_pwm_neutral[self.FR] + self.steering_pwm_range * 0.3))
|
|
time.sleep(0.3)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
|
|
int(self.steering_pwm_neutral[self.FL] - self.steering_pwm_range * 0.3))
|
|
time.sleep(0.1)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
|
|
int(self.steering_pwm_neutral[self.FR] - self.steering_pwm_range * 0.3))
|
|
time.sleep(0.3)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
|
|
int(self.steering_pwm_neutral[self.FL]))
|
|
time.sleep(0.1)
|
|
self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
|
|
int(self.steering_pwm_neutral[self.FR]))
|
|
time.sleep(0.3)
|
|
|
|
def setSteering(self, steering_command):
|
|
# Loop through pin dictionary. The items key is the wheel_name and the value the pin.
|
|
for wheel_name, motor_pin in self.pins['steer'].items():
|
|
duty_cycle = int(
|
|
self.steering_pwm_neutral[wheel_name] + steering_command[wheel_name]/90.0 * self.steering_pwm_range)
|
|
|
|
self.pwm.set_pwm(motor_pin, 0, duty_cycle)
|
|
|
|
def setDriving(self, driving_command):
|
|
# Loop through pin dictionary. The items key is the wheel_name and the value the pin.
|
|
for wheel_name, motor_pin in self.pins['drive'].items():
|
|
duty_cycle = int(self.driving_pwm_neutral[wheel_name] +
|
|
driving_command[wheel_name]/100.0 * self.driving_pwm_range * self.wheel_directions[wheel_name])
|
|
|
|
self.pwm.set_pwm(motor_pin, 0, duty_cycle)
|
|
|
|
def stopMotors(self):
|
|
for wheel_name, motor_pin in self.pins['drive'].items():
|
|
self.pwm.set_pwm(motor_pin, 0, int(self.driving_pwm_neutral[wheel_name]))
|