Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New Motors #98

Draft
wants to merge 21 commits into
base: develop
Choose a base branch
from
Draft

New Motors #98

Show file tree
Hide file tree
Changes from 20 commits
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
3fdd4cb
added modbus_test.ino
RobertJN64 Sep 17, 2024
64c6776
manual implementation
RobertJN64 Sep 18, 2024
a6e7b1f
basic functionality
RobertJN64 Sep 18, 2024
8776262
samples working
RobertJN64 Sep 18, 2024
c4a6561
setup motors
RobertJN64 Sep 18, 2024
22193df
rewrite with object oriented approach
RobertJN64 Sep 19, 2024
1743a85
enable RS485 code (it was disabled for testing locally)
RobertJN64 Sep 19, 2024
27d7cb1
code works on motor!
RobertJN64 Sep 21, 2024
cc45ea9
split out modbus into its own lib, add more read functions
RobertJN64 Sep 25, 2024
f44096e
fix motor stuff
RobertJN64 Oct 9, 2024
2543233
remove all the stuff that the new motors replace
RobertJN64 Oct 9, 2024
55017db
Revert "remove all the stuff that the new motors replace"
RobertJN64 Oct 15, 2024
2806cbc
added stepper motor code (without breaking linear actuators, removed …
RobertJN64 Oct 15, 2024
9e3c8be
flag TODO items
RobertJN64 Oct 15, 2024
55a3744
removed encoders and old current sensors
RobertJN64 Oct 16, 2024
fb9c0c5
update volts? -> RPM
RobertJN64 Oct 16, 2024
7771c1e
fix negative velocity
RobertJN64 Oct 23, 2024
1a8eafb
flag ROS messages that need to be updated, update differentialdrive
RobertJN64 Oct 23, 2024
bd89c01
update manual_controller.py to use 5000 RPM instead of -127 to 127
RobertJN64 Nov 6, 2024
935c236
use 1000 RPM to stand in for 127 effort
RobertJN64 Nov 6, 2024
01237b7
fix more 127 -> RPM thresholds
RobertJN64 Dec 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions lunabot_behavior/behavior.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
from nav_msgs.msg import Path, Odometry
from apriltag_ros.msg import AprilTagDetectionArray, AprilTagDetection
from lunabot_msgs.msg import RobotEffort, RobotSensors, RobotErrors, Behavior
from std_msgs.msg import Bool, Int8
from std_msgs.msg import Bool, Int8, Int32

import ascent
import find_apriltag
Expand Down Expand Up @@ -58,10 +58,10 @@ def __init__(self):
self.robot_odom: Odometry = Odometry()

self.lin_act_publisher = rospy.Publisher("/lin_act", Int8, queue_size=1, latch=True)
self.left_drive_publisher = rospy.Publisher("/left_drive", Int8, queue_size=1, latch=True)
self.right_drive_publisher = rospy.Publisher("/right_drive", Int8, queue_size=1, latch=True)
self.excavate_publisher = rospy.Publisher("/excavate", Int8, queue_size=1, latch=True)
self.deposition_publisher = rospy.Publisher("/deposition", Int8, queue_size=1, latch=True)
self.left_drive_publisher = rospy.Publisher("/left_drive", Int32, queue_size=1, latch=True)
self.right_drive_publisher = rospy.Publisher("/right_drive", Int32, queue_size=1, latch=True)
self.excavate_publisher = rospy.Publisher("/excavate", Int32, queue_size=1, latch=True)
self.deposition_publisher = rospy.Publisher("/deposition", Int32, queue_size=1, latch=True)

self.velocity_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=1, latch=True)
self.traversal_publisher = rospy.Publisher("/behavior/traversal_enabled", Bool, queue_size=1, latch=True)
Expand Down
10 changes: 5 additions & 5 deletions lunabot_behavior/deposition.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import rospy

from lunabot_msgs.msg import RobotSensors
from std_msgs.msg import Int8
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist

from homing_controller import HomingController
Expand All @@ -27,7 +27,7 @@ def __init__(self, deposition_publisher: rospy.Publisher = None):
"""

if deposition_publisher is None:
self.deposition_publisher = rospy.Publisher("/deposition", Int8, queue_size=1, latch=True)
self.deposition_publisher = rospy.Publisher("/deposition", Int32, queue_size=1, latch=True)
rospy.init_node('deposition_node')
else:
self.deposition_publisher = deposition_publisher
Expand All @@ -38,7 +38,7 @@ def __init__(self, deposition_publisher: rospy.Publisher = None):

self.rate = rospy.Rate(10) # 10hz

self.deposition_power = 127 # TODO change if needed
self.deposition_RPM = 1000

# self.load_cell_threshold = 1 # in kilograms, TODO test / verify WHEN LOAD CELLS EXIST

Expand All @@ -58,8 +58,8 @@ def deposit(self):

time.sleep(0.1)

deposition_msg = Int8()
deposition_msg.data = self.deposition_power
deposition_msg = Int32()
deposition_msg.data = self.deposition_RPM

start_time = rospy.get_time()

Expand Down
11 changes: 6 additions & 5 deletions lunabot_behavior/excavate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
from clamp_output import clamp_output
import ascent

from std_msgs.msg import Int8
from std_msgs.msg import Int8, Int32


class Excavate:
Expand All @@ -34,7 +34,7 @@ def __init__(
"""

if excavation_publisher is None:
self.excavation_publisher: rospy.Publisher = rospy.Publisher("/excavate", Int8, queue_size=1, latch=True)
self.excavation_publisher: rospy.Publisher = rospy.Publisher("/excavate", Int32, queue_size=1, latch=True)
rospy.init_node("excavation_node")
else:
self.excavation_publisher: rospy.Publisher = excavation_publisher
Expand All @@ -58,6 +58,7 @@ def __init__(
# 90 percent of max speed
TARGET_EXCAVATION_VELOCITY = 8 * 0.9

# TODO RJN - update this whole script to use RPM
self.excavation_pid_controller = VelocityPIDController(
TARGET_EXCAVATION_VELOCITY, 1, 0, 0, 127
) # TODO find values
Expand Down Expand Up @@ -111,7 +112,7 @@ def plunge(self):
time.sleep(3)
return True

excavation_message = Int8()
excavation_message = Int32()
lin_act_message = Int8()

excavation_ang = (
Expand Down Expand Up @@ -190,7 +191,7 @@ def trench(self):
time.sleep(3)
return True

excavation_message = Int8()
excavation_message = Int32()
cmd_vel_message = Twist()

excavation_ang = self.robot_sensors.exc_ang
Expand Down Expand Up @@ -276,7 +277,7 @@ def spin_excavation_backwards(self):
# 90% of max speed, backwards
EXCAVATION_SPEED = int(-127 * 0.9)

excavation_message = Int8()
excavation_message = Int32()
excavation_message.data = EXCAVATION_SPEED

self.excavation_publisher.publish(excavation_message)
Expand Down
10 changes: 5 additions & 5 deletions lunabot_behavior/no-sensor-deposition.py
View file
Open in desktop
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changes to this file (and no-sensor-excavate) aren't going to be super important as they will need to be re-written completely. Likely will need to make at least some of these changes again on the new version.

Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import rospy

from lunabot_msgs.msg import RobotSensors
from std_msgs.msg import Int8
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist

from homing_controller import HomingController
Expand All @@ -27,7 +27,7 @@ def __init__(self, deposition_publisher: rospy.Publisher = None):
"""

if deposition_publisher is None:
self.deposition_publisher = rospy.Publisher("/deposition", Int8, queue_size=1, latch=True)
self.deposition_publisher = rospy.Publisher("/deposition", Int32, queue_size=1, latch=True)
rospy.init_node('deposition_node')
else:
self.deposition_publisher = deposition_publisher
Expand All @@ -38,7 +38,7 @@ def __init__(self, deposition_publisher: rospy.Publisher = None):

self.rate = rospy.Rate(10) # 10hz

self.deposition_power = 127 # TODO change if needed
self.deposition_RPM = 1000 # RPM

# self.load_cell_threshold = 1 # in kilograms, TODO test / verify WHEN LOAD CELLS EXIST

Expand All @@ -58,8 +58,8 @@ def deposit(self):

time.sleep(0.1)

deposition_msg = Int8()
deposition_msg.data = self.deposition_power
deposition_msg = Int32()
deposition_msg.data = self.deposition_RPM

start_time = rospy.get_time()

Expand Down
16 changes: 8 additions & 8 deletions lunabot_behavior/no-sensor-excavate.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
from clamp_output import clamp_output
import ascent

from std_msgs.msg import Int8
from std_msgs.msg import Int8, Int32


class Excavate:
Expand All @@ -34,7 +34,7 @@ def __init__(
"""

if excavation_publisher is None:
self.excavation_publisher: rospy.Publisher = rospy.Publisher("/excavate", Int8, queue_size=1, latch=True)
self.excavation_publisher: rospy.Publisher = rospy.Publisher("/excavate", Int32, queue_size=1, latch=True)
rospy.init_node("excavation_node")
else:
self.excavation_publisher: rospy.Publisher = excavation_publisher
Expand Down Expand Up @@ -97,7 +97,7 @@ def plunge(self):
time.sleep(3)
return True

excavation_message = Int8()
excavation_message = Int32()
lin_act_message = Int8()

start_time = rospy.get_time()
Expand All @@ -113,7 +113,7 @@ def plunge(self):

dt = new_time - current_time

excavation_message.data = int(127 * 0.8)
excavation_message.data = int(1000 * 0.8)

lin_act_message.data = -110

Expand Down Expand Up @@ -143,7 +143,7 @@ def trench(self):
time.sleep(3)
return True

excavation_message = Int8()
excavation_message = Int32()
cmd_vel_message = Twist()

current_time = rospy.get_time()
Expand All @@ -169,7 +169,7 @@ def trench(self):

dt = new_time - current_time

excavation_message.data = int(127 * 0.8)
excavation_message.data = int(1000 * 0.8)

self.excavation_publisher.publish(excavation_message)

Expand Down Expand Up @@ -200,9 +200,9 @@ def spin_excavation_backwards(self):
print("Excavation: spinning backwards (", self.exc_failure_counter, ")")

# 90% of max speed, backwards
EXCAVATION_SPEED = int(-127 * 0.9)
EXCAVATION_SPEED = int(-1000 * 0.9) #RPM

excavation_message = Int8()
excavation_message = Int32()
excavation_message.data = EXCAVATION_SPEED

self.excavation_publisher.publish(excavation_message)
Expand Down
8 changes: 4 additions & 4 deletions lunabot_behavior/plunge.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import rospy

from lunabot_msgs.msg import RobotEffort, RobotSensors
from std_msgs.msg import Int8
from std_msgs.msg import Int8, Int32
import interrupts

import time
Expand All @@ -22,7 +22,7 @@ def __init__(self, excavation_publisher: rospy.Publisher = None, lin_act_publish
"""

if excavation_publisher is None:
self.excavation_publisher = rospy.Publisher("/excavate", Int8, queue_size=1, latch=True)
self.excavation_publisher = rospy.Publisher("/excavate", Int32, queue_size=1, latch=True)
rospy.init_node('plunge_node')
else:
self.excavation_publisher = excavation_publisher
Expand All @@ -44,7 +44,7 @@ def __init__(self, excavation_publisher: rospy.Publisher = None, lin_act_publish
# TODO: check and test this value
self.LOWERING_TIME = 23 #In seconds, how long it takes to lower the linear actuators 90% of the way

self.EXCAVATION_SPEED = 127
self.EXCAVATION_SPEED = 1000 #RPM
self.LIN_ACT_SPEED = -110

self.is_sim = rospy.get_param("is_sim")
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Github won't let me leave a comment in the right place, but two lines down (line 54) exc_message is not updated I don't think

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in fix threshold commit

Expand All @@ -68,7 +68,7 @@ def plunge(self):

time.sleep(0.1)

excavation_message = Int8()
excavation_message = Int32()
excavation_message.data = self.EXCAVATION_SPEED

lin_act_message = Int8()
Expand Down
2 changes: 1 addition & 1 deletion lunabot_behavior/stuck.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ def stuck(self):
error_publisher.publish(stuck_msg)

# check if trying to go somewhere
if self.robot_effort.left_drive >= 15 or self.robot_effort.right_drive >= 15:
if self.robot_effort.left_drive >= 15 or self.robot_effort.right_drive >= 15: #TODO RJN - these might need to be higher
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, you're almost certainly correct. That being said, I don't think this file ever gets used. There's a strong argument to be made for deleting it entirely.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Updated in fix threshold commit

# check if not going anywhere
if self.robot_sensors.drive_left_vel <= 0.1 and self.robot_sensors.drive_right_vel <= 0.1:
if not currently_stuck:
Expand Down
66 changes: 13 additions & 53 deletions lunabot_control/scripts/differential_drive_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
import numpy as np
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int8
from std_msgs.msg import Int32

from lunabot_msgs.msg import RobotSensors

Expand All @@ -13,20 +13,16 @@ def __init__(self):
# ROS Publishers and subsribers to get / send data

self._vel_sub = rospy.Subscriber("/cmd_vel", Twist, self._vel_cb)
self._right_drive_pub = rospy.Publisher("/right_drive", Int8, queue_size=1)
self._left_drive_pub = rospy.Publisher("/left_drive", Int8, queue_size=1)
self._right_drive_pub = rospy.Publisher("/right_drive", Int32, queue_size=1)
self._left_drive_pub = rospy.Publisher("/left_drive", Int32, queue_size=1)
self._state_sub = rospy.Subscriber("sensors", RobotSensors, self._robot_state_cb)

self.width = rospy.get_param("~width", 0.5588)
self.max_speed_percentage = rospy.get_param("~max_speed_percentage", 0.8)
self.hz = rospy.get_param("~hz", 20)

self._max_speed = rospy.get_param("~max_speed", 2.0) # In rad/s
self.p = rospy.get_param("~p", 3.9) # P gain for PID controller
self.i = rospy.get_param("~i", 0.0) # I gain for PID controller
self.d = rospy.get_param("~d", 0) # D gain for PID controller
self.i_sat = rospy.get_param("~i_saturate", 10) # Max for integral term

self._max_speed = rospy.get_param("~max_speed", 2.0) * 60 / 2 / np.pi # In rad/s converted to RPM
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a reason why this isn't in RPM directly? Also, to convert from linear to angular speed, don't you need wheel diameter (12 inches, if it's relevant)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Max RPM speed is 3000 at full torque and 5000 overall. I see very little downside to capping this at 3000 rpm


self.lin = 0
self.ang = 0

Expand Down Expand Up @@ -58,12 +54,8 @@ def _robot_state_cb(self, msg):
self._left_vel = msg.drive_left_vel

def _loop(self):
left_drive_msg = Int8()
right_drive_msg = Int8()

left_percent_estimate = np.clip(self._left_vel / self._max_speed, -1, 1)
right_percent_estimate = np.clip(self._right_vel / self._max_speed, -1, 1)

left_drive_msg = Int32()
right_drive_msg = Int32()

WEIGHT = 0.9

Expand All @@ -73,40 +65,8 @@ def _loop(self):
left_set = self.lin - self.ang * self.width / 2
right_set = self.lin + self.ang * self.width / 2

# Measured Velocity in m / s
left_measured = self._left_vel * self._meters_per_rad
right_measured = self._right_vel * self._meters_per_rad

# Calculating error
left_error = left_set - left_measured
right_error = right_set - right_measured

self.left_error_sum += left_error
self.right_error_sum += right_error

self.left_error_sum = np.clip(self.left_error_sum, -self.i_sat, self.i_sat)
self.right_error_sum = np.clip(self.right_error_sum, -self.i_sat, self.i_sat)

# Calculating motor velocities
left = (
left_percent_estimate
+ left_error * self.p
+ self.left_error_sum * self.i
+ (left_error - self._left_prev_error) / self.loop_dt * self.d
)
right = (
right_percent_estimate
+ right_error * self.p
+ self.right_error_sum * self.i
+ (right_error - self._right_prev_error) / self.loop_dt * self.d
)

# Calculating previous error
self._left_prev_error = left_error
self._right_prev_error = right_error

left_drive_msg.data = self.constrain(left)
right_drive_msg.data = self.constrain(right)
left_drive_msg.data = self.constrain(left_set * self._meters_per_rad)
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This goes and uses the self._meters_per_rad. Due to github, I can't leave a comment directly on that definition, but I think that number may be wrong. Even if it is correct, something more than a magic number would be appreciated.

right_drive_msg.data = self.constrain(right_set * self._meters_per_rad)

if self.lin == 0 and self.ang == 0:
left_drive_msg.data = 0
Expand All @@ -115,12 +75,12 @@ def _loop(self):
self._right_drive_pub.publish(right_drive_msg)

def constrain(self, val):
val = np.clip(-1, val, 1) # Clipping speed to not go over 100%
return np.int8(val * 127 * self.max_speed_percentage)
val = np.clip(-self._max_speed, val, self._max_speed) # Clipping speed to not go over 100%
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah looking at this context self._max_speed should really just be explicity defined as RPM. If needed the paramater is used elsewhere, it would be better to back-calculate it from the rpm value.

return np.int32(val)

def shutdown_hook(self):
left_drive_msg = Int8()
right_drive_msg = Int8()
left_drive_msg = Int32()
right_drive_msg = Int32()

left_drive_msg.data = 0
right_drive_msg.data = 0
Expand Down
10 changes: 5 additions & 5 deletions lunabot_control/scripts/effort_factory.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import rospy

from lunabot_msgs.msg import RobotEffort, RobotErrors
from std_msgs.msg import Int8
from std_msgs.msg import Int8, Int32


class EffortFactory:
Expand All @@ -26,10 +26,10 @@ def __init__(self):
)

self.lin_act_subscriber = rospy.Subscriber("/lin_act", Int8, self.set_lin_act)
self.left_drive_subscriber = rospy.Subscriber("/left_drive", Int8, self.set_left_drive)
self.right_drive_subscriber = rospy.Subscriber("/right_drive", Int8, self.set_right_drive)
self.excavate_subscriber = rospy.Subscriber("/excavate", Int8, self.set_excavate)
self.deposition_subscriber = rospy.Subscriber("/deposition", Int8, self.set_deposition)
self.left_drive_subscriber = rospy.Subscriber("/left_drive", Int32, self.set_left_drive)
self.right_drive_subscriber = rospy.Subscriber("/right_drive", Int32, self.set_right_drive)
self.excavate_subscriber = rospy.Subscriber("/excavate", Int32, self.set_excavate)
self.deposition_subscriber = rospy.Subscriber("/deposition", Int32, self.set_deposition)

self.error_subscriber = rospy.Subscriber("/errors", RobotErrors, self.error_callback)

Expand Down
Loading
Loading