Merging Real motors with main

CMakeLists.txt

@@ -167,6 +167,10 @@ include_directories(
 catkin_install_python(PROGRAMS
   scripts/getModels.py
   scripts/setModels.py
+  scripts/models.py
+  scripts/cmd_vel2motor.py
+  scripts/odometry.py
+  scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

config/diffdrive.yaml

@@ -1,8 +1,8 @@
 type: "diff_drive_controller/DiffDriveController"
 publish_rate: 50
 
-left_wheel: ['left_front_wheel_joint', 'left_back_wheel_joint']
-right_wheel: ['right_front_wheel_joint', 'right_back_wheel_joint']
+left_wheel: ['left_front_motor', 'left_back_motor']
+right_wheel: ['right_front_motor', 'right_back_motor']
 
 wheel_separation: 0.2
 
@@ -19,12 +19,12 @@ base_frame_id: base_link
 linear:
   x:
     has_velocity_limits    : true
-    max_velocity           : 1.5   # m/s
+    max_velocity           : 100   # m/s
     has_acceleration_limits: true
-    max_acceleration       : 1.5  # m/s^2
+    max_acceleration       : 100  # m/s^2
 angular:
   z:
     has_velocity_limits    : true
-    max_velocity           : 6.28   # rad/s
+    max_velocity           : 100   # rad/s
     has_acceleration_limits: true
-    max_acceleration       : 6.28   # rad/s^2
+    max_acceleration       : 100   # rad/s^2

config/v5_motor.yaml

@@ -0,0 +1,21 @@
+# topics
+publish_velocity: true
+publish_encoder:  true
+publish_current:  true
+publish_motor_joint_state: true
+update_rate: 100.0
+# motor model
+motor_nominal_voltage: 14.0 # Volts
+# moment of inertia calculated by getting 17.5 inch / pounds of torque from a vex forum post and converting it to kgm https://www.vexforum.com/t/v5-motor-torque/80258/2
+moment_of_inertia: 0.4535924 # kgm^2
+armature_damping_ratio: 0.03 # Nm/(rad/s)
+electromotive_force_constant: 0.03 # Nm/A
+# ohms law 14V max batter 11W max power draw V/(W/V) = Ohms https://www.vexrobotics.com/276-4840.html#additional
+electric_resistance: 17.818182 # Ohm
+electric_inductance: 0.001 # Henry
+# transmission
+gear_ratio: 36.0
+joint_damping: 0.005
+joint_friction: 0.01
+# shaft encoder
+encoder_ppr: 900 #50*_gear_ratio

launch/changeup.launch

@@ -1,19 +1,4 @@
 <launch>
-    <arg name="x_start" default="1.0"/>
-    <arg name="y_start" default="-1.0"/>
-    <arg name="z_start" default="0.11"/>
-
-    <arg name="world_name" default="$(find simulation)/worlds/games/change_up.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
-    <arg name="model" default="$(find simulation)/urdf/robot.urdf.xacro"/>
-    <arg name="rvizconfig" default="$(find simulation)/rviz/changeup.rviz" />
-
-    <include file="$(find simulation)/launch/robot.launch">
-        <arg name="x_start" value="$(arg x_start)" />
-        <arg name="y_start" value="$(arg y_start)" />
-        <arg name="z_start" value="$(arg z_start)" />
-
-        <arg name="world_name" value="$(arg world_name)" />
-        <arg name="model" value="$(arg model)" />
-        <arg name="rvizconfig" value="$(arg rvizconfig)" />
-    </include>
+    <include file="$(find simulation)/launch/changeup_field_only.launch"/>
+    <include file="$(find simulation)/launch/robot.launch"/>
 </launch>

launch/changeup_field_only.launch

@@ -0,0 +1,8 @@
+<launch>
+    <arg name="world_name" default="$(find simulation)/worlds/games/change_up.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
+        <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+    <include file="$(find simulation)/launch/empty_world.launch">
+        <arg name="world_name" value="$(arg world_name)" />
+<!--        <arg name="physics" value="dart" />-->
+    </include>
+</launch>

launch/robot.launch

@@ -1,75 +1,76 @@
 <launch>
-    <arg name="x_start" default="1.0"/>
-    <arg name="y_start" default="-1.0"/>
+    <arg name="rvizconfig" default="$(find simulation)/rviz/robot.rviz"/>
+
+    <arg name="x_start" default="-1.6"/>
+    <arg name="y_start" default="1"/>
     <arg name="z_start" default="0.11"/>
 
-    <arg name="world_name" default=""/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
     <arg name="model" default="$(find simulation)/urdf/robot.urdf.xacro"/>
-    <arg name="rvizconfig" default="$(find simulation)/rviz/robot.rviz" />
-
-    <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
-    <include file="$(find simulation)/launch/empty_world.launch">
-        <arg name="world_name" value="$(arg world_name)" />
-    </include>
 
     <!-- convert urdf.xacro to just urdf-->
-    <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+    <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>
 
     <!-- push robot_description to factory and spawn robot in gazebo-->
     <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
-          args="-x $(arg x_start) -y $(arg y_start) -z $(arg z_start) -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />
+          args="-x $(arg x_start) -y $(arg y_start) -z $(arg z_start) -unpause -urdf -model robot -param robot_description"
+          respawn="false" output="screen"/>
 
-    <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
-        <param name="publish_frequency" type="double" value="30.0" />
-    </node>
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
+    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher"/>
 
     <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
-        <param name="~odometry_topic" value="ground_truth/state" />
-        <param name="~frame_id" value="world" />
-        <param name="~child_frame_id" value="GT/base_link" />
-    </node>
-
-    <node pkg="topic_tools" type="relay"  name="relay"
-          args="joint_states joint_states_copy">
-    </node>
-
-    <rosparam param="source_list">["joint_states_copy"]</rosparam>
-    <node pkg="joint_state_publisher" type="joint_state_publisher"  name="joint_state_publisher">
-        <remap from="/joint_states" to="/default/joint_states"/>
-    </node>
-
-    <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher2">
-        <param name="publish_frequency" type="double" value="100.0" />
-        <remap from="/joint_states" to="/default/joint_states"/>
-        <param name="tf_prefix" type="string" value="GT" />
-    </node>
-
-    <node pkg="tf" type="static_transform_publisher" name="odom_world_broadcaster" args="$(arg x_start) $(arg y_start) $(arg z_start) 0 -0.0 0 world odom 10" />
+        <param name="~odometry_topic" value="ground_truth/state"/>
+        <param name="~frame_id" value="world"/>
+        <param name="~child_frame_id" value="base_link"/></node>
 
     <rosparam command="load"
               file="$(find simulation)/config/joints.yaml"
-              ns="robot_joint_state_controller" />
+              ns="robot_joint_state_controller"/>
     <rosparam command="load"
               file="$(find simulation)/config/diffdrive.yaml"
-              ns="robot_diff_drive_controller" />
+              ns="robot_diff_drive_controller"/>
 
-    <node name="controller_spawner" pkg="controller_manager" type="spawner" args="robot_joint_state_controller robot_diff_drive_controller"></node>
+    <node pkg="tf" type="static_transform_publisher" name="odom_world_broadcaster"
+          args="$(arg x_start) $(arg y_start) $(arg z_start) 0 0 0 world odom 10"/>
 
     <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
-        <param name="scale_linear" value="1.5" />
-        <param name="scale_angular" value="3.0" />
-        <remap from="/cmd_vel" to="/robot_diff_drive_controller/cmd_vel"/>
+        <param name="scale_linear" value="1"/>
+        <param name="scale_angular" value="1"/>
+        <remap from="/cmd_vel" to="/cmd_vel"/></node>
+
+    <node pkg="joy" type="joy_node" name="joy_node"><param name="deadzone" value="0.01"/></node>
+
+    <!-- TODO: figure out pid later -->
+    <param name="Kp" value="1.0"/><param name="Ki" value="0.0"/><param name="Kd" value="0.1"/>
+    <param name="upper_limit" value="10"/><param name="lower_limit" value="-10"/>
+    <param name="windup_limit" value="10"/><param name="cutoff_frequency" value="20"/>
+    <param name="max_loop_frequency" value="105.0"/><param name="min_loop_frequency" value="95.0"/>
+    <param name="setpoint_timeout" value="-1.0"/>
+    <node name="pid_motor_right" pkg="pid" type="controller" output="screen">
+        <param name="Kp" value="${Kp}"/><param name="Ki" value="${Ki}"/><param name="Kd" value="${Kd}"/>
+        <param name="upper_limit" value="${upper_limit}"/><param name="lower_limit" value="${lower_limit}"/><param name="windup_limit" value="${windup_limit}"/>
+        <param name="cutoff_frequency" value="${cutoff_frequency}"/><param name="max_loop_frequency" value="${max_loop_frequency}"/><param name="min_loop_frequency" value="${min_loop_frequency}"/>
+        <param name="setpoint_timeout" value="-${setpoint_timeout}"/>
+        <remap from="setpoint" to="pid_motor_right/setpoint"/>
+        <remap from="control_effort" to="pid_motor_right/control_effort"/>
+        <remap from="state" to="pid_motor_right/state"/>
     </node>
-
-    <node pkg="joy" type="joy_node" name="joy_node">
-        <param name="deadzone" value="0.1" />
+    <node name="pid_motor_left" pkg="pid" type="controller" output="screen">
+        <param name="Kp" value="${Kp}"/><param name="Ki" value="${Ki}"/><param name="Kd" value="${Kd}"/>
+        <param name="upper_limit" value="${upper_limit}"/><param name="lower_limit" value="${lower_limit}"/><param name="windup_limit" value="${windup_limit}"/>
+        <param name="cutoff_frequency" value="${cutoff_frequency}"/><param name="max_loop_frequency" value="${max_loop_frequency}"/><param name="min_loop_frequency" value="${min_loop_frequency}"/>
+        <param name="setpoint_timeout" value="-${setpoint_timeout}"/>
+        <remap from="setpoint" to="pid_motor_left/setpoint"/>
+        <remap from="control_effort" to="pid_motor_left/control_effort"/>
+        <remap from="state" to="pid_motor_left/state"/>
     </node>
 
-    <!--
-    <node pkg="simulation" name="get_models" type="getModels.py" output="screen"/>
-    <node pkg="simulation" name="set_models" type="setModels.py" output="screen"/>
-    -->
+<!--    <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure"/>-->
 
-    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="false" />
+<!--    <node pkg="simulation" type="cmd_vel2motor.py" name="cmd_vel2motor"/>-->
+    <!--    <node pkg="simulation" type="quintic_polynomials_planner.py" name="quintic_polynomials_planner"/>-->
+    <!--    <node pkg="simulation" name="get_models" type="getModels.py" output="screen"/>-->
+    <!--    <node pkg="simulation" name="set_models" type="setModels.py" output="screen"/>-->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="false"/>
 
 </launch>