Set sonar angle to 5° degree
authorErik Andresen <erik@vontaene.de>
Mon, 19 Jun 2017 17:30:37 +0000 (19:30 +0200)
committerErik Andresen <erik@vontaene.de>
Mon, 19 Jun 2017 17:30:37 +0000 (19:30 +0200)
avr/motor_ctrl/main.c
avr/motor_ctrl/main.hex
config/wt_node.cfg
launch/robot_localization.launch
launch/wild_thumper.launch
scripts/sensor_board.py
scripts/wt_node.py
urdf/wild_thumper.urdf.xacro

index 16c8b0c..5d65529 100644 (file)
@@ -97,8 +97,8 @@
  */
 
 
  */
 
 
-#define KP 0.062
-#define KI 0.12
+#define KP 0.09
+#define KI 0.07
 #define KD 0.0
 #define PID_T 0.01
 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:47
 #define KD 0.0
 #define PID_T 0.01
 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:47
 #define STEP_PER_M_RIGHT (STEP_PER_M)
 #define WHEEL_DIST 0.39912 // Measured: 0.252
 #define PWM_BREAK INT16_MIN
 #define STEP_PER_M_RIGHT (STEP_PER_M)
 #define WHEEL_DIST 0.39912 // Measured: 0.252
 #define PWM_BREAK INT16_MIN
+#define STALL_LIMIT 140000
 
 #define TWI_ACK   TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
 #define TWI_RESET TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
 
 #define TWI_ACK   TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
 #define TWI_RESET TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
@@ -899,25 +900,25 @@ static void update_pid(void) {
        static int32_t esum4=0;
 
        // protect motors from damage if stalling
        static int32_t esum4=0;
 
        // protect motors from damage if stalling
-       if (labs(esum1) > 140000 && speed1 == 0) {
+       if (labs(esum1) > STALL_LIMIT && speed1 == 0) {
                motor1 = 0;
                motor1_mode = MOTOR_MANUAL;
                error_state |= (1<<4);
                esum1 = 0;
        }       
                motor1 = 0;
                motor1_mode = MOTOR_MANUAL;
                error_state |= (1<<4);
                esum1 = 0;
        }       
-       if (labs(esum2) > 140000 && speed2 == 0) {
+       if (labs(esum2) > STALL_LIMIT && speed2 == 0) {
                motor2 = 0;
                motor2_mode = MOTOR_MANUAL;
                error_state |= (1<<5);
                esum2 = 0;
        }       
                motor2 = 0;
                motor2_mode = MOTOR_MANUAL;
                error_state |= (1<<5);
                esum2 = 0;
        }       
-       if (labs(esum3) > 140000 && speed3 == 0) {
+       if (labs(esum3) > STALL_LIMIT && speed3 == 0) {
                motor3 = 0;
                motor3_mode = MOTOR_MANUAL;
                error_state |= (1<<6);
                esum3 = 0;
        }       
                motor3 = 0;
                motor3_mode = MOTOR_MANUAL;
                error_state |= (1<<6);
                esum3 = 0;
        }       
-       if (labs(esum4) > 140000 && speed4 == 0) {
+       if (labs(esum4) > STALL_LIMIT && speed4 == 0) {
                motor4 = 0;
                motor4_mode = MOTOR_MANUAL;
                error_state |= (1<<7);
                motor4 = 0;
                motor4_mode = MOTOR_MANUAL;
                error_state |= (1<<7);
index ec2e8cc..aef12fe 100644 (file)
 :10140000B90AB501882777FD8095982FC0908E0086\r
 :10141000D0908F00E0909000F0909100C60ED71E03\r
 :10142000E81EF91EC0928E00D0928F00E0929000CC\r
 :10140000B90AB501882777FD8095982FC0908E0086\r
 :10141000D0908F00E0909000F0909100C60ED71E03\r
 :10142000E81EF91EC0928E00D0928F00E0929000CC\r
-:10143000F09291000E94FC1026EB33EF4DE75DE344\r
+:10143000F09291000E94FC102CEE31E548EB5DE348\r
 :101440000E94E9112B013C01C701B6010E94FC106A\r
 :101440000E94E9112B013C01C701B6010E94FC106A\r
-:1014500021E539E44DE95AE30E94E9119B01AC0111\r
+:1014500024E330E847E35AE30E94E9119B01AC0121\r
 :10146000C301B2010E94F60F6B017C0180918000E4\r
 :1014700090918100B501681B790B882777FD8095D5\r
 :10148000982F0E94FC1020E030E0A9010E94E91191\r
 :10146000C301B2010E94F60F6B017C0180918000E4\r
 :1014700090918100B501681B790B882777FD8095D5\r
 :10148000982F0E94FC1020E030E0A9010E94E91191\r
 :1015C000B90AB501882777FD8095982FC0908A00C9\r
 :1015D000D0908B00E0908C00F0908D00C60ED71E4E\r
 :1015E000E81EF91EC0928A00D0928B00E0928C0017\r
 :1015C000B90AB501882777FD8095982FC0908A00C9\r
 :1015D000D0908B00E0908C00F0908D00C60ED71E4E\r
 :1015E000E81EF91EC0928A00D0928B00E0928C0017\r
-:1015F000F0928D000E94FC1026EB33EF4DE75DE387\r
+:1015F000F0928D000E94FC102CEE31E548EB5DE38B\r
 :101600000E94E9112B013C01C701B6010E94FC10A8\r
 :101600000E94E9112B013C01C701B6010E94FC10A8\r
-:1016100021E539E44DE95AE30E94E9119B01AC014F\r
+:1016100024E330E847E35AE30E94E9119B01AC015F\r
 :10162000C301B2010E94F60F6B017C0180917E0024\r
 :1016300090917F00B501681B790B882777FD809515\r
 :10164000982F0E94FC1020E030E0A9010E94E911CF\r
 :10162000C301B2010E94F60F6B017C0180917E0024\r
 :1016300090917F00B501681B790B882777FD809515\r
 :10164000982F0E94FC1020E030E0A9010E94E911CF\r
 :10178000B90AB501882777FD8095982FC09086000B\r
 :10179000D0908700E0908800F0908900C60ED71E98\r
 :1017A000E81EF91EC0928600D0928700E092880061\r
 :10178000B90AB501882777FD8095982FC09086000B\r
 :10179000D0908700E0908800F0908900C60ED71E98\r
 :1017A000E81EF91EC0928600D0928700E092880061\r
-:1017B000F09289000E94FC1026EB33EF4DE75DE3C9\r
+:1017B000F09289000E94FC102CEE31E548EB5DE3CD\r
 :1017C0000E94E9112B013C01C701B6010E94FC10E7\r
 :1017C0000E94E9112B013C01C701B6010E94FC10E7\r
-:1017D00021E539E44DE95AE30E94E9119B01AC018E\r
+:1017D00024E330E847E35AE30E94E9119B01AC019E\r
 :1017E000C301B2010E94F60F6B017C0180917C0065\r
 :1017F00090917D00B501681B790B882777FD809556\r
 :10180000982F0E94FC1020E030E0A9010E94E9110D\r
 :1017E000C301B2010E94F60F6B017C0180917C0065\r
 :1017F00090917D00B501681B790B882777FD809556\r
 :10180000982F0E94FC1020E030E0A9010E94E9110D\r
 :10194000B90AB501882777FD8095982FC09082004D\r
 :10195000D0908300E0908400F0908500C60ED71EE2\r
 :10196000E81EF91EC0928200D0928300E0928400AB\r
 :10194000B90AB501882777FD8095982FC09082004D\r
 :10195000D0908300E0908400F0908500C60ED71EE2\r
 :10196000E81EF91EC0928200D0928300E0928400AB\r
-:10197000F09285000E94FC1026EB33EF4DE75DE30B\r
+:10197000F09285000E94FC102CEE31E548EB5DE30F\r
 :101980000E94E9112B013C01C701B6010E94FC1025\r
 :101980000E94E9112B013C01C701B6010E94FC1025\r
-:1019900021E539E44DE95AE30E94E9119B01AC01CC\r
+:1019900024E330E847E35AE30E94E9119B01AC01DC\r
 :1019A000C301B2010E94F60F6B017C0180917A00A5\r
 :1019B00090917B00B501681B790B882777FD809596\r
 :1019C000982F0E94FC1020E030E0A9010E94E9114C\r
 :1019A000C301B2010E94F60F6B017C0180917A00A5\r
 :1019B00090917B00B501681B790B882777FD809596\r
 :1019C000982F0E94FC1020E030E0A9010E94E9114C\r
index 725c565..aad0d54 100755 (executable)
@@ -6,7 +6,7 @@ gen = ParameterGenerator()
 
 gen.add("range_sensor_clip",   bool_t,         0, "Clip range sensor values to max range", True)
 gen.add("range_sensor_max",    double_t,       0, "Range sensor max range", 0.5, 0.04, 4)
 
 gen.add("range_sensor_clip",   bool_t,         0, "Clip range sensor values to max range", True)
 gen.add("range_sensor_max",    double_t,       0, "Range sensor max range", 0.5, 0.04, 4)
-gen.add("odom_covar_xy",       double_t,       0, "Odometry covariance: translation", 1e-2, 1e-6, 1)
+gen.add("odom_covar_xy",       double_t,       0, "Odometry covariance: translation", 1e-3, 1e-6, 1)
 gen.add("odom_covar_angle",    double_t,       0, "Odometry covariance: rotation", 1.00, 1e-6, 6.2832)
 gen.add("rollover_protect",    bool_t,         0, "Enable motor rollover protection on pitch", True)
 gen.add("rollover_protect_limit",double_t,     0, "Pitch rollover protection limit (degree)", 45, 0, 90)
 gen.add("odom_covar_angle",    double_t,       0, "Odometry covariance: rotation", 1.00, 1e-6, 6.2832)
 gen.add("rollover_protect",    bool_t,         0, "Enable motor rollover protection on pitch", True)
 gen.add("rollover_protect_limit",double_t,     0, "Pitch rollover protection limit (degree)", 45, 0, 90)
index d3da317..0a7aa6d 100644 (file)
@@ -21,7 +21,7 @@
                   carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
                   as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
                   if not specified. -->
                   carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
                   as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
                   if not specified. -->
-               <param name="sensor_timeout" value="2.0"/>
+               <!-- <param name="sensor_timeout" value="2.0"/> -->
 
                <!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
                   are operating in a planar environment and want to ignore the effect of small variations in the
 
                <!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
                   are operating in a planar environment and want to ignore the effect of small variations in the
@@ -59,7 +59,7 @@
                <!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
                   can be used for future dating the transform, which is required for interaction with some other
                   packages. Defaults to 0.0 if unspecified. -->
                <!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
                   can be used for future dating the transform, which is required for interaction with some other
                   packages. Defaults to 0.0 if unspecified. -->
-               <param name="transform_time_offset" value="0.0"/>
+               <!-- <param name="transform_time_offset" value="0.0"/> -->
 
                <!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
                   TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
 
                <!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
                   TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
@@ -67,6 +67,7 @@
                   parameters obviously have no default values, and must be specified. -->
                <param name="odom0" value="odom"/>
                <param name="imu0" value="imu"/>
                   parameters obviously have no default values, and must be specified. -->
                <param name="odom0" value="odom"/>
                <param name="imu0" value="imu"/>
+               <rosparam param="imu0_queue_size">10</rosparam>
 
                <!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
                   which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
 
                <!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
                   which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
@@ -77,8 +78,8 @@
                   making this parameter required for each sensor. -->
                <!-- x/y not included because of redundancy with velocities -->
                <!-- vyaw not included in odom because too inaccurate -->
                   making this parameter required for each sensor. -->
                <!-- x/y not included because of redundancy with velocities -->
                <!-- vyaw not included in odom because too inaccurate -->
-               <rosparam param="odom0_config">[false, false, false, false, false, false, true, true, false, false, false, false, false, false, false]</rosparam>
-               <rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, false, false, false]</rosparam>
+               <rosparam param="odom0_config">[false, false, false, false, false, false, true, true, true, false, false, true, false, false, false]</rosparam>
+               <rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
 
                <!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
                   measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation
 
                <!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
                   measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation
                   if the node is unhappy with any settings or data. -->
                <param name="print_diagnostics" value="true"/>
 
                   if the node is unhappy with any settings or data. -->
                <param name="print_diagnostics" value="true"/>
 
+               <!-- If true, will dynamically scale the process_noise_covariance based on the robot?s velocity. This is useful, e.g., when you want your
+                    robot?s estimate error covariance to stop growing when the robot is stationary. Defaults to false. -->
+               <param name="dynamic_process_noise_covariance" value="true"/>
+
                <remap from="odometry/filtered" to="odom_combined"/>
        </node>
 </launch>
                <remap from="odometry/filtered" to="odom_combined"/>
        </node>
 </launch>
index 9f2c287..a3f39f9 100644 (file)
@@ -15,7 +15,7 @@
                <rosparam command="load" file="$(find wild_thumper)/config/analyzers.yaml"/>
        </node>
 
                <rosparam command="load" file="$(find wild_thumper)/config/analyzers.yaml"/>
        </node>
 
-       <node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false" required="false">
+       <node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false" required="true">
                <param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>
                <param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>
@@ -33,6 +33,7 @@
                <arg name="nodelet_manager_name" value="nodelet_manager" />
        </include>
 
                <arg name="nodelet_manager_name" value="nodelet_manager" />
        </include>
 
+       <!--
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
                <remap from="imu_data" to="imu"/>
                <remap from="vo" to="gps"/>
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
                <remap from="imu_data" to="imu"/>
                <remap from="vo" to="gps"/>
@@ -49,8 +50,8 @@
                <param name="frame_id" value="odom"/>
                <param name="child_frame_id" value="base_footprint"/>
        </node>
                <param name="frame_id" value="odom"/>
                <param name="child_frame_id" value="base_footprint"/>
        </node>
+       -->
 
 
-       <!--
        <include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/>
        <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
                <param name="magnetic_declination_radians" value="0.48"/>
        <include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/>
        <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
                <param name="magnetic_declination_radians" value="0.48"/>
@@ -63,5 +64,4 @@
                <remap from="/odometry/filtered" to="odom_combined"/>
                <remap from="/odometry/gps" to="gps"/>
        </node>
                <remap from="/odometry/filtered" to="odom_combined"/>
                <remap from="/odometry/gps" to="gps"/>
        </node>
-       -->
 </launch>
 </launch>
index 8efeb16..e431b57 100755 (executable)
@@ -11,8 +11,8 @@ from pyshared.humidity import *
 from wild_thumper.msg import Sensor
 
 # Board warming offset
 from wild_thumper.msg import Sensor
 
 # Board warming offset
-TEMP_ERROR = -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = 0 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
 
 """
 LDR:
 
 """
 LDR:
index 1549ea6..1cf9d8d 100755 (executable)
@@ -237,9 +237,9 @@ class MoveBase:
                odom.pose.pose.orientation.w = odom_quat[3]
                odom.pose.covariance[0] = self.odom_covar_xy # x
                odom.pose.covariance[7] = self.odom_covar_xy # y
                odom.pose.pose.orientation.w = odom_quat[3]
                odom.pose.covariance[0] = self.odom_covar_xy # x
                odom.pose.covariance[7] = self.odom_covar_xy # y
-               odom.pose.covariance[14] = 99999 # z
-               odom.pose.covariance[21] = 99999 # rotation about X axis
-               odom.pose.covariance[28] = 99999 # rotation about Y axis
+               odom.pose.covariance[14] = self.odom_covar_xy # z
+               odom.pose.covariance[21] = self.odom_covar_angle # rotation about X axis
+               odom.pose.covariance[28] = self.odom_covar_angle # rotation about Y axis
                odom.pose.covariance[35] = self.odom_covar_angle # rotation about Z axis
 
                # set the velocity
                odom.pose.covariance[35] = self.odom_covar_angle # rotation about Z axis
 
                # set the velocity
index 47039b6..8be843c 100644 (file)
        <joint name="sonar_forward_left_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_forward_left"/>
        <joint name="sonar_forward_left_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_forward_left"/>
-               <origin xyz="0.115 0.05 -0.012" rpy="0 0 0"/>
+               <origin xyz="0.115 0.05 -0.012" rpy="0 ${-5*PI/180} 0"/>
        </joint>
 
        <joint name="sonar_forward_right_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_forward_right"/>
        </joint>
 
        <joint name="sonar_forward_right_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_forward_right"/>
-               <origin xyz="0.115 -0.05 -0.012" rpy="0 0 0"/>
+               <origin xyz="0.115 -0.05 -0.012" rpy="0 ${-5*PI/180} 0"/>
        </joint>
 
        <joint name="sonar_backward_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_backward"/>
        </joint>
 
        <joint name="sonar_backward_joint" type="fixed">
                <parent link="mounting_plate"/>
                <child link="sonar_backward"/>
-               <origin xyz="-0.115 0.0 -0.012" rpy="0 ${PI} 0"/>
+               <origin xyz="-0.115 0.0 -0.012" rpy="0 ${-175*PI/180} 0"/>
        </joint>
 
        <joint name="ir_left_joint" type="fixed">
        </joint>
 
        <joint name="ir_left_joint" type="fixed">