Start
This commit is contained in:
@@ -0,0 +1,93 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
from sensor_msgs.msg import Joy
|
||||
from exomy.msg import RoverCommand
|
||||
from locomotion_modes import LocomotionMode
|
||||
import math
|
||||
|
||||
# Define locomotion modes
|
||||
global locomotion_mode
|
||||
global motors_enabled
|
||||
|
||||
locomotion_mode = LocomotionMode.ACKERMANN.value
|
||||
motors_enabled = True
|
||||
|
||||
|
||||
def callback(data):
|
||||
|
||||
global locomotion_mode
|
||||
global motors_enabled
|
||||
|
||||
rover_cmd = RoverCommand()
|
||||
|
||||
# Function map for the Logitech F710 joystick
|
||||
# Button on pad | function
|
||||
# --------------|----------------------
|
||||
# A | Ackermann mode
|
||||
# X | Point turn mode
|
||||
# Y | Crabbing mode
|
||||
# Left Stick | Control speed and direction
|
||||
# START Button | Enable and disable motors
|
||||
|
||||
# Reading out joystick data
|
||||
y = data.axes[1]
|
||||
x = data.axes[0]
|
||||
|
||||
# Reading out button data to set locomotion mode
|
||||
# X Button
|
||||
if (data.buttons[0] == 1):
|
||||
locomotion_mode = LocomotionMode.POINT_TURN.value
|
||||
# A Button
|
||||
if (data.buttons[1] == 1):
|
||||
locomotion_mode = LocomotionMode.ACKERMANN.value
|
||||
# B Button
|
||||
if (data.buttons[2] == 1):
|
||||
pass
|
||||
# Y Button
|
||||
if (data.buttons[3] == 1):
|
||||
locomotion_mode = LocomotionMode.CRABBING.value
|
||||
|
||||
rover_cmd.locomotion_mode = locomotion_mode
|
||||
|
||||
# Enable and disable motors
|
||||
# START Button
|
||||
if (data.buttons[9] == 1):
|
||||
if motors_enabled is True:
|
||||
motors_enabled = False
|
||||
rospy.loginfo("Motors disabled!")
|
||||
elif motors_enabled is False:
|
||||
motors_enabled = True
|
||||
rospy.loginfo("Motors enabled!")
|
||||
else:
|
||||
rospy.logerr(
|
||||
"Exceptional value for [motors_enabled] = {}".format(motors_enabled))
|
||||
motors_enabled = False
|
||||
|
||||
rover_cmd.motors_enabled = motors_enabled
|
||||
|
||||
# The velocity is decoded as value between 0...100
|
||||
rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y), 1.0)
|
||||
|
||||
# The steering is described as an angle between -180...180
|
||||
# Which describe the joystick position as follows:
|
||||
# +90
|
||||
# 0 +-180
|
||||
# -90
|
||||
#
|
||||
rover_cmd.steering = math.atan2(y, x)*180.0/math.pi
|
||||
|
||||
rover_cmd.connected = True
|
||||
|
||||
pub.publish(rover_cmd)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
global pub
|
||||
|
||||
rospy.init_node('joystick_parser_node')
|
||||
rospy.loginfo('joystick_parser_node started')
|
||||
|
||||
sub = rospy.Subscriber("/joy", Joy, callback, queue_size=1)
|
||||
pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
|
||||
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,10 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
import enum
|
||||
|
||||
|
||||
class LocomotionMode(enum.Enum):
|
||||
FAKE_ACKERMANN = 0
|
||||
ACKERMANN = 1
|
||||
POINT_TURN = 2
|
||||
CRABBING = 3
|
||||
@@ -0,0 +1,46 @@
|
||||
#!/usr/bin/env python
|
||||
import time
|
||||
import rospy
|
||||
|
||||
from exomy.msg import MotorCommands
|
||||
from motors import Motors
|
||||
|
||||
motors = Motors()
|
||||
global watchdog_timer
|
||||
|
||||
|
||||
def callback(cmds):
|
||||
motors.setSteering(cmds.motor_angles)
|
||||
motors.setDriving(cmds.motor_speeds)
|
||||
|
||||
global watchdog_timer
|
||||
watchdog_timer.shutdown()
|
||||
# If this timer runs longer than the duration specified,
|
||||
# then watchdog() is called stopping the driving motors.
|
||||
watchdog_timer = rospy.Timer(rospy.Duration(5.0), watchdog, oneshot=True)
|
||||
|
||||
|
||||
def shutdown():
|
||||
motors.stopMotors()
|
||||
|
||||
|
||||
def watchdog(event):
|
||||
rospy.loginfo("Watchdog fired. Stopping driving motors.")
|
||||
motors.stopMotors()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# This node waits for commands from the robot and sets the motors accordingly
|
||||
rospy.init_node("motors")
|
||||
rospy.loginfo("Starting the motors node")
|
||||
rospy.on_shutdown(shutdown)
|
||||
|
||||
global watchdog_timer
|
||||
watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True)
|
||||
|
||||
sub = rospy.Subscriber(
|
||||
"/motor_commands", MotorCommands, callback, queue_size=1)
|
||||
|
||||
rate = rospy.Rate(10)
|
||||
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,124 @@
|
||||
#!/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()
|
||||
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 = rospy.get_param("drive_pwm_neutral")
|
||||
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 +
|
||||
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):
|
||||
# Set driving wheels to neutral position to stop them
|
||||
duty_cycle = int(self.driving_pwm_neutral)
|
||||
|
||||
for wheel_name, motor_pin in self.pins['drive'].items():
|
||||
self.pwm.set_pwm(motor_pin, 0, duty_cycle)
|
||||
@@ -0,0 +1,41 @@
|
||||
#!/usr/bin/env python
|
||||
import time
|
||||
from exomy.msg import RoverCommand, MotorCommands, Screen
|
||||
import rospy
|
||||
from rover import Rover
|
||||
import message_filters
|
||||
|
||||
|
||||
global exomy
|
||||
exomy = Rover()
|
||||
|
||||
|
||||
def joy_callback(message):
|
||||
cmds = MotorCommands()
|
||||
|
||||
if message.motors_enabled is True:
|
||||
exomy.setLocomotionMode(message.locomotion_mode)
|
||||
|
||||
cmds.motor_angles = exomy.joystickToSteeringAngle(
|
||||
message.vel, message.steering)
|
||||
cmds.motor_speeds = exomy.joystickToVelocity(
|
||||
message.vel, message.steering)
|
||||
else:
|
||||
cmds.motor_angles = exomy.joystickToSteeringAngle(0, 0)
|
||||
cmds.motor_speeds = exomy.joystickToVelocity(0, 0)
|
||||
|
||||
robot_pub.publish(cmds)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('robot_node')
|
||||
rospy.loginfo("Starting the robot node")
|
||||
global robot_pub
|
||||
joy_sub = rospy.Subscriber(
|
||||
"/rover_command", RoverCommand, joy_callback, queue_size=1)
|
||||
|
||||
rate = rospy.Rate(10)
|
||||
|
||||
robot_pub = rospy.Publisher("/motor_commands", MotorCommands, queue_size=1)
|
||||
|
||||
rospy.spin()
|
||||
@@ -0,0 +1,298 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import time
|
||||
import math
|
||||
import enum
|
||||
from locomotion_modes import LocomotionMode
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Rover():
|
||||
'''
|
||||
Rover class contains all the math and motor control algorithms to move the rover
|
||||
'''
|
||||
|
||||
# Defining wheel names
|
||||
FL, FR, CL, CR, RL, RR = range(0, 6)
|
||||
|
||||
# Defining locomotion modes
|
||||
FAKE_ACKERMANN, ACKERMANN, POINT_TURN, CRABBING = range(0, 4)
|
||||
|
||||
def __init__(self):
|
||||
self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN
|
||||
|
||||
self.wheel_x = 12.0
|
||||
self.wheel_y = 20.0
|
||||
|
||||
max_steering_angle = 45
|
||||
self.ackermann_r_min = abs(
|
||||
self.wheel_y) / math.tan(max_steering_angle * math.pi / 180.0) + self.wheel_x
|
||||
|
||||
self.ackermann_r_max = 250
|
||||
|
||||
def setLocomotionMode(self, locomotion_mode_command):
|
||||
'''
|
||||
Sets the locomotion mode
|
||||
'''
|
||||
if(self.locomotion_mode != locomotion_mode_command):
|
||||
self.locomotion_mode = locomotion_mode_command
|
||||
rospy.loginfo('Set locomotion mode to: %s',
|
||||
LocomotionMode(locomotion_mode_command).name)
|
||||
|
||||
def joystickToSteeringAngle(self, driving_command, steering_command):
|
||||
'''
|
||||
Converts the steering command [angle of joystick] to angles for the different motors
|
||||
|
||||
:param int driving_command: Drive speed command range from -100 to 100
|
||||
:param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right)
|
||||
'''
|
||||
|
||||
steering_angles = [0]*6
|
||||
deg = steering_command
|
||||
|
||||
if(self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value):
|
||||
if (driving_command == 0):
|
||||
# Stop
|
||||
steering_angles[self.FL] = 0
|
||||
steering_angles[self.FR] = 0
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = 0
|
||||
steering_angles[self.RR] = 0
|
||||
return steering_angles
|
||||
|
||||
if(80 < deg < 100):
|
||||
# Drive straight forward
|
||||
steering_angles[self.FL] = 0
|
||||
steering_angles[self.FR] = 0
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = 0
|
||||
steering_angles[self.RR] = 0
|
||||
elif(-80 < deg < -100):
|
||||
# Drive straight backwards
|
||||
steering_angles[self.FL] = 0
|
||||
steering_angles[self.FR] = 0
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = 0
|
||||
steering_angles[self.RR] = 0
|
||||
elif(100 < deg <= 180):
|
||||
# Drive right forwards
|
||||
steering_angles[self.FL] = 45
|
||||
steering_angles[self.FR] = 45
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = -45
|
||||
steering_angles[self.RR] = -45
|
||||
elif(-100 > deg >= -180):
|
||||
# Drive right backwards
|
||||
steering_angles[self.FL] = 45
|
||||
steering_angles[self.FR] = 45
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = -45
|
||||
steering_angles[self.RR] = -45
|
||||
elif(80 > deg >= 0):
|
||||
# Drive left forwards
|
||||
steering_angles[self.FL] = -45
|
||||
steering_angles[self.FR] = -45
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = 45
|
||||
steering_angles[self.RR] = 45
|
||||
elif(0 > deg > -80):
|
||||
# Drive left backwards
|
||||
steering_angles[self.FL] = -45
|
||||
steering_angles[self.FR] = -45
|
||||
steering_angles[self.CR] = 0
|
||||
steering_angles[self.CL] = 0
|
||||
steering_angles[self.RL] = 45
|
||||
steering_angles[self.RR] = 45
|
||||
|
||||
return steering_angles
|
||||
if(self.locomotion_mode == LocomotionMode.ACKERMANN.value):
|
||||
|
||||
# No steering if robot is not driving
|
||||
if(driving_command is 0):
|
||||
return steering_angles
|
||||
|
||||
# Scale between min and max Ackermann radius
|
||||
if math.cos(math.radians(steering_command)) == 0:
|
||||
r = self.ackermann_r_max
|
||||
else:
|
||||
r = self.ackermann_r_max - \
|
||||
abs(math.cos(math.radians(steering_command))) * \
|
||||
((self.ackermann_r_max-self.ackermann_r_min))
|
||||
|
||||
# No steering
|
||||
if r == self.ackermann_r_max:
|
||||
return steering_angles
|
||||
|
||||
inner_angle = int(math.degrees(
|
||||
math.atan(self.wheel_x/(abs(r)-self.wheel_y))))
|
||||
outer_angle = int(math.degrees(
|
||||
math.atan(self.wheel_x/(abs(r)+self.wheel_y))))
|
||||
|
||||
if steering_command > 90 or steering_command < -90:
|
||||
# Steering to the right
|
||||
steering_angles[self.FL] = outer_angle
|
||||
steering_angles[self.FR] = inner_angle
|
||||
steering_angles[self.RL] = -outer_angle
|
||||
steering_angles[self.RR] = -inner_angle
|
||||
else:
|
||||
# Steering to the left
|
||||
steering_angles[self.FL] = -inner_angle
|
||||
steering_angles[self.FR] = -outer_angle
|
||||
steering_angles[self.RL] = inner_angle
|
||||
steering_angles[self.RR] = outer_angle
|
||||
|
||||
return steering_angles
|
||||
|
||||
if(self.locomotion_mode == LocomotionMode.POINT_TURN.value):
|
||||
steering_angles[self.FL] = 45
|
||||
steering_angles[self.FR] = -45
|
||||
steering_angles[self.RL] = -45
|
||||
steering_angles[self.RR] = 45
|
||||
|
||||
return steering_angles
|
||||
if(self.locomotion_mode == LocomotionMode.CRABBING.value):
|
||||
if(driving_command != 0):
|
||||
wheel_direction = 0
|
||||
if(steering_command > 0):
|
||||
wheel_direction = steering_command - 90
|
||||
|
||||
elif(steering_command <= 0):
|
||||
wheel_direction = steering_command + 90
|
||||
|
||||
wheel_direction = np.clip(wheel_direction, -75, 75)
|
||||
|
||||
steering_angles[self.FL] = wheel_direction
|
||||
steering_angles[self.FR] = wheel_direction
|
||||
steering_angles[self.CL] = wheel_direction
|
||||
steering_angles[self.CR] = wheel_direction
|
||||
steering_angles[self.RL] = wheel_direction
|
||||
steering_angles[self.RR] = wheel_direction
|
||||
|
||||
return steering_angles
|
||||
|
||||
def joystickToVelocity(self, driving_command, steering_command):
|
||||
'''
|
||||
Converts the steering and drive command to the speeds of the individual motors
|
||||
|
||||
:param int driving_command: Drive speed command range from -100 to 100
|
||||
:param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right)
|
||||
'''
|
||||
|
||||
motor_speeds = [0]*6
|
||||
if (self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value):
|
||||
if(driving_command > 0 and steering_command >= 0):
|
||||
motor_speeds[self.FL] = 50
|
||||
motor_speeds[self.FR] = 50
|
||||
motor_speeds[self.CR] = 50
|
||||
motor_speeds[self.CL] = 50
|
||||
motor_speeds[self.RL] = 50
|
||||
motor_speeds[self.RR] = 50
|
||||
|
||||
elif(driving_command > 0 and steering_command <= 0):
|
||||
motor_speeds[self.FL] = -50
|
||||
motor_speeds[self.FR] = -50
|
||||
motor_speeds[self.CR] = -50
|
||||
motor_speeds[self.CL] = -50
|
||||
motor_speeds[self.RL] = -50
|
||||
motor_speeds[self.RR] = -50
|
||||
|
||||
return motor_speeds
|
||||
|
||||
if (self.locomotion_mode == LocomotionMode.ACKERMANN.value):
|
||||
v = driving_command
|
||||
if(steering_command < 0):
|
||||
v *= -1
|
||||
|
||||
# Scale between min and max Ackermann radius
|
||||
radius = self.ackermann_r_max - \
|
||||
abs(math.cos(math.radians(steering_command))) * \
|
||||
((self.ackermann_r_max-self.ackermann_r_min))
|
||||
|
||||
if (v == 0):
|
||||
return motor_speeds
|
||||
|
||||
if (radius == self.ackermann_r_max):
|
||||
return [v] * 6
|
||||
else:
|
||||
rmax = radius + self.wheel_x
|
||||
|
||||
a = math.pow(self.wheel_y, 2)
|
||||
b = math.pow(abs(radius) + self.wheel_x, 2)
|
||||
c = math.pow(abs(radius) - self.wheel_x, 2)
|
||||
rmax_float = float(rmax)
|
||||
|
||||
r1 = math.sqrt(a+b)
|
||||
r2 = rmax_float
|
||||
r3 = r1
|
||||
r4 = math.sqrt(a+c)
|
||||
r5 = abs(radius) - self.wheel_x
|
||||
r6 = r4
|
||||
|
||||
v1 = int(v)
|
||||
v2 = int(v*r2/r1)
|
||||
v3 = v1
|
||||
v4 = int(v*r4/r1)
|
||||
v5 = int(v*r5/r1)
|
||||
v6 = v4
|
||||
|
||||
if (steering_command > 90 or steering_command < -90):
|
||||
motor_speeds = [v1, v2, v3, v4, v5, v6]
|
||||
else:
|
||||
motor_speeds = [v6, v5, v4, v3, v2, v1]
|
||||
|
||||
return motor_speeds
|
||||
|
||||
if (self.locomotion_mode == LocomotionMode.POINT_TURN.value):
|
||||
deg = steering_command
|
||||
if(driving_command is not 0):
|
||||
# Left turn
|
||||
if(deg < 85 and deg > -85):
|
||||
motor_speeds[self.FL] = -50
|
||||
motor_speeds[self.FR] = 50
|
||||
motor_speeds[self.CL] = -50
|
||||
motor_speeds[self.CR] = 50
|
||||
motor_speeds[self.RL] = -50
|
||||
motor_speeds[self.RR] = 50
|
||||
# Right turn
|
||||
elif(deg > 95 or deg < -95):
|
||||
motor_speeds[self.FL] = 50
|
||||
motor_speeds[self.FR] = -50
|
||||
motor_speeds[self.CL] = 50
|
||||
motor_speeds[self.CR] = -50
|
||||
motor_speeds[self.RL] = 50
|
||||
motor_speeds[self.RR] = -50
|
||||
else:
|
||||
# Stop
|
||||
motor_speeds[self.FL] = 0
|
||||
motor_speeds[self.FR] = 0
|
||||
motor_speeds[self.CL] = 0
|
||||
motor_speeds[self.CR] = 0
|
||||
motor_speeds[self.RL] = 0
|
||||
motor_speeds[self.RR] = 0
|
||||
|
||||
return motor_speeds
|
||||
|
||||
if(self.locomotion_mode == LocomotionMode.CRABBING.value):
|
||||
if(driving_command > 0):
|
||||
if(steering_command > 0):
|
||||
motor_speeds[self.FL] = 50
|
||||
motor_speeds[self.FR] = 50
|
||||
motor_speeds[self.CL] = 50
|
||||
motor_speeds[self.CR] = 50
|
||||
motor_speeds[self.RL] = 50
|
||||
motor_speeds[self.RR] = 50
|
||||
elif(steering_command <= 0):
|
||||
motor_speeds[self.FL] = -50
|
||||
motor_speeds[self.FR] = -50
|
||||
motor_speeds[self.CL] = -50
|
||||
motor_speeds[self.CR] = -50
|
||||
motor_speeds[self.RL] = -50
|
||||
motor_speeds[self.RR] = -50
|
||||
|
||||
return motor_speeds
|
||||
Reference in New Issue
Block a user