WIP: Started on motor calibration driver

README.md

@@ -94,6 +94,11 @@ Total hours: 62
 
 * Implement `odom` publisher.  #6
   * Test odometry and debug.
+  4 hours.
+
+* Use encoders to provide feedback for motor control #12
+  Need to add calculations to convert desired m/s value into encoder ticks per second.
+  1hr
 
 ## To do
 

build_system.md

@@ -59,7 +59,7 @@ ros2 run micro_ros_setup configure_firmware.sh <application name> -t udp -i [you
 
 using the value `<application name>`.  The top level file then includes `firmware/toolchain/esp-idf/tools/cmake/project.cmake` that does "a lot of stuff" and eventually includes the file `firmware/freertos_apps/microros_esp32_extensions/main/CMakeLists.txt`.
 
-Finally figured out how to make it work.  For some reason, you can't just add a subdirectory and use CMakeLists.txt files as you would expect.  This is something to do with the way the ESP build system works (looks like it was coded by a Python programmer who does not know how make works).  So there is a work around where a file `app.cmake` is included from the app directory that contains the list of files to build.  There is also a change to the top of file `firmware/freertos_apps/microros_esp32_extensions/main/CMakeLists.txt` that looks like this:
+Finally figured out how to make it work.  For some reason, you can't just add a subdirectory and use CMakeLists.txt files as you would expect.  This is something to do with the way the ESP build system works.  So there is a work around where a file `app.cmake` is included from the app directory that contains the list of files to build.  There is also a change to the top of file `firmware/freertos_apps/microros_esp32_extensions/main/CMakeLists.txt` that looks like this:
 
 ```cmake
 # message("AJB: UROS_APP_FOLDER: " ${UROS_APP_FOLDER})
@@ -163,7 +163,25 @@ ros2 run micro_ros_setup build_agent.sh
 
 When adding publishers etc. you need to update the `app-colcon.meta` file to tell the build system that number of publishers etc.  This change takes effect when the command `ros2 run micro_ros_setup build_firmware.sh` is run.
 
-Not having the correct numbers in `app-colcon.meta` and the value of `EXECUTOR_HANDLE_COUNT` causes a failure during intialisation.  Check the numbers carefully.
+Not having the correct numbers in `app-colcon.meta` and the value of `EXECUTOR_HANDLE_COUNT` causes a failure during initialisation.  Check the numbers carefully.
+
+NOTE: Only one service can be run at a time, even if you change the values in the `app-colcon.meta` file.   The line of code in the file `src/uros/rmw_microxrcedds/rmw_microxrcedds_c/src/utils.c` shows the problem where a single value of status and the number 1 is used.
+
+```cpp
+bool run_xrce_session(rmw_context_impl_t* context, uint16_t requests)
+...
+        // This only handles one request at time
+        uint8_t status;
+        if (!uxr_run_session_until_all_status(&context->session,
+                RMW_UXRCE_ENTITY_CREATION_DESTROY_TIMEOUT,
+                &requests, &status, 1))
+```
+
+This problem causes error messages like this:
+
+```text
+[ERROR] [0000000001.351339000] [rclc]: [rclc_service_init_default] Error in rcl_service_init: Not available memory node, at /home/build/ws/firmware/mcu_ws/uros/rmw_microxrcedds/rmw_microxrcedds_c/src/rmw_service.c:79, at /home/build/ws/firmware/mcu_ws/uros/rcl/rcl/src/rcl/service.c:193
+```
 
 ## Adding a common interface message
 

motor_calibration_driver.md

@@ -0,0 +1,41 @@
+# Motor Calibration Driver
+
+In an ideal world, I would be able to use continuous servo motors to control the wheels of my robot.  Instead, I'm using DC motors and optical encoders, very much cheaper but much more difficult to control using software.
+
+Most motor controllers are designed around a PID controller that are designed to set the motor speed over a number of iterations to a constant speed.  This approach is only useful when a robot is travelling over long distances.  This robot has a wheel circumference of about 21cm, so when making small adjustments, say 5cm, the wheel needs to be turned about a quarter of a turn and that corresponds to 3 slots on the encoder wheel.
+
+The other problem is that many cheap DC motors do not start rotating with the same input power.  For instance, with the wheels suspended, the left motor of this robot starts turning forward at 16% duty cycle and the right motor starts turning forward at 23%.  So using bare duty cycle is going to cause problems.  This is where the idea of motor calibration comes in.
+
+We also need to consider the battery voltage being used to drive the motors.  In our case, the motors are rated at 6V but we are driving them from a 2S LiPo battery with an output voltage of between 6.4V and 8.4V.  To prevent overheating the motors, the amount of power being applied to the motors is the important value to consider.  Assuming the resistance of the motor is constant, the amount of power being applied to the motor is determined by the voltage being applied times the duty cycle (in the range of 0 to 1).  The battery voltage is something that is easy to measure so can be used as an input to the control algorithm.
+
+Basically, the idea is to implement a look up table for each motor that can be used to convert the desired rotational speed into a duty cycle value to drive the motor.  The use of optical encoders makes it possible to test different duty cycle values and record the effect on the output shaft.
+
+The motors will be calibrated by sending a command to the robot that then performs a series of maneuvers and records the results.  The results are then converted into a look up table and stored in NVM.  The saved look up table is then used to control the motors.  The calibration can be performed as often as desired.
+
+## Motor calibration
+
+The first question that comes to mind is what values should be stored in the look up table.  The simplest implementation is to go though the duty cycle of each motor from 1 to 100 forward and reverse and record the encoder counts per unit time.  This info is then stored in a look up table.
+
+Once this has been done, if a certain speed is requested, the speed can be converted into encoder ticks per second, the look up table searched for the nearest encoder ticks per second value and the duty cycle applied.
+
+https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/storage/nvs_flash.html
+
+
+
+
+## To do list
+
+1. Send message to start calibration.  The ROS self test message is suitable. However, when tested only one service at a time works with micro-ROS, so using a subscriber and publisher instead.
+1. Run tests several times, capturing results for analysis to improve what is done.
+1. Test various implementations.
+1.
+
+The motors being used are similar to this: https://www.aliexpress.com/item/32434622843.html
+
+Description:
+Operating voltage: 3V~12VDC (recommended operating voltage of about 6 to 8V)
+Maximum torque: 800gf cm min (3V)
+No-load speed: 1:48 (3V time)
+The load current: 70mA (250mA MAX) (3V)
+Size: 7x2.2x1.8cm(approx)
+

raspi_robot_msgs/msg/MotorsTest.msg

@@ -0,0 +1,12 @@
+# Starts a test of the motors.
+
+# The available tests.
+# Stop any test in progress.
+int8 STOP=0
+# Start a cycle of test values continously.
+int8 CONTINUOUS_CYCLE=1
+# Start the calibration routine.
+int8 CALIBRATE=2
+
+# The test to perform.
+int8 test

raspi_robot_msgs/srv/SonarPosition.srv → raspi_robot_msgs/msg/SonarPosition.msg

@@ -4,8 +4,3 @@
 # Servo positions - range +90 to -90 degrees, 0 is centred.
 int8 x
 int8 y
----
-# The actual servo positions set.
-# The hardware implementation may limit the range of movement.
-int8 x
-int8 y

rover/raspi_rover/app-colcon.meta

@@ -4,7 +4,7 @@
             "cmake-args": [
                 "-DRMW_UXRCE_MAX_NODES=1",
                 "-DRMW_UXRCE_MAX_PUBLISHERS=4",
-                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=3",
+                "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=5",
                 "-DRMW_UXRCE_MAX_SERVICES=1",
                 "-DRMW_UXRCE_MAX_CLIENTS=1",
                 "-DRMW_UXRCE_MAX_HISTORY=1",

rover/raspi_rover/app.c

@@ -13,11 +13,13 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "geometry_msgs/msg/twist.h"
+#include "micro_ros_diagnostic_msgs/srv/micro_ros_self_test.h"
 #include "nav_msgs/msg/odometry.h"
 #include "raspi_robot_driver.h"
 #include "raspi_robot_msgs/msg/leds.h"
 #include "raspi_robot_msgs/msg/motors.h"
-#include "raspi_robot_msgs/srv/sonar_position.h"
+#include "raspi_robot_msgs/msg/motors_test.h"
+#include "raspi_robot_msgs/msg/sonar_position.h"
 #include "sensor_msgs/msg/battery_state.h"
 #include "sensor_msgs/msg/range.h"
 
@@ -44,18 +46,18 @@
 #define MS_PER_TICK (1000 / TICK_RATE_HZ)
 #define US_PER_TICK (MS_PER_TICK * 1000)
 
-// Number of executor handles: one timer, three subscribers, one service.
+// Number of executor handles: 1 timer, 5 subscribers, 0 services.
 // Publishers don't count as they are driven by the timer.
 #define EXECUTOR_HANDLE_COUNT (6)
-// #define EXECUTOR_HANDLE_COUNT (4)
 
 rcl_publisher_t publisher_battery_state;
 rcl_publisher_t publisher_odometry;
 rcl_publisher_t publisher_range;
 rcl_subscription_t subscriber_cmd_vel;
 rcl_subscription_t subscriber_leds;
 rcl_subscription_t subscriber_motors;
-rcl_service_t service_sonar_position;
+rcl_subscription_t subscriber_motors_test;
+rcl_subscription_t subscriber_sonar_position;
 
 // Logging name.
 static const char *TAG = "raspi_rover";
@@ -67,6 +69,7 @@ static const char *k_range = "range";
 // Custom topic/service names.
 static const char *k_robot_leds = "raspi_robot/leds";
 static const char *k_robot_motors = "raspi_robot/motors";
+static const char *k_robot_motors_test = "raspi_robot/motors_test";
 static const char *k_robot_sonar_position = "raspi_robot/sonar_position";
 // Messages to publish.
 static nav_msgs__msg__Odometry *odometry_msg = NULL;
@@ -103,12 +106,6 @@ static void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
   }
 }
 
-static void subscription_callback_cmd_vel(const void *msg_in) {
-  const geometry_msgs__msg__Twist *msg =
-      (const geometry_msgs__msg__Twist *)msg_in;
-  messages_cmd_vel(msg);
-}
-
 static void subscription_callback_leds(const void *msg_in) {
   const raspi_robot_msgs__msg__Leds *msg =
       (const raspi_robot_msgs__msg__Leds *)msg_in;
@@ -126,19 +123,6 @@ static void subscription_callback_motors(const void *msg_in) {
   raspi_robot_motors_drive(msg->left_percent, msg->right_percent, ticks);
 }
 
-static void service_sonar_position_callback(const void *req, void *res) {
-  raspi_robot_msgs__srv__SonarPosition_Request *request =
-      (raspi_robot_msgs__srv__SonarPosition_Request *)req;
-  raspi_robot_msgs__srv__SonarPosition_Response *response =
-      (raspi_robot_msgs__srv__SonarPosition_Response *)res;
-  int16_t x = request->x;
-  int16_t y = request->y;
-  ESP_LOGI(TAG, "Requested sonar position: x %d, y %d", x, y);
-  raspi_robot_servo_set(&x, &y);
-  response->x = x;
-  response->y = y;
-}
-
 void appMain(void *arg) {
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rclc_support_t support;
@@ -186,13 +170,30 @@ void appMain(void *arg) {
       ROSIDL_GET_MSG_TYPE_SUPPORT(raspi_robot_msgs, msg, Motors),
       k_robot_motors));
 
+  RCCHECK(rclc_subscription_init_default(
+      &subscriber_motors_test, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(raspi_robot_msgs, msg, MotorsTest),
+      k_robot_motors_test));
+
+  RCCHECK(rclc_subscription_init_default(
+      &subscriber_sonar_position, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(raspi_robot_msgs, msg, SonarPosition),
+      k_robot_sonar_position));
+
   // Create services.
   ESP_LOGI(TAG, "Creating services");
   RCCHECK(rclc_service_init_default(
-      &service_sonar_position, &node,
+      &subscriber_sonar_position, &node,
       ROSIDL_GET_SRV_TYPE_SUPPORT(raspi_robot_msgs, srv, SonarPosition),
       k_robot_sonar_position));
 
+  const rosidl_service_type_support_t *type_support =
+      ROSIDL_GET_SRV_TYPE_SUPPORT(micro_ros_diagnostic_msgs, srv,
+                                  MicroROSSelfTest);
+  rcl_ret_t temp_rc = rclc_service_init_default(
+      &service_self_test_motors, &node, type_support, k_self_test_motors);
+  printf("AJB: Status on line %d: %d. \n", __LINE__, temp_rc);
+
   // Create timer.
   ESP_LOGI(TAG, "Creating timers");
   rcl_timer_t timer = rcl_get_zero_initialized_timer();
@@ -209,30 +210,35 @@ void appMain(void *arg) {
   RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
   RCCHECK(rclc_executor_add_timer(&executor, &timer));
 
-  ESP_LOGI(TAG, "Adding subs");
+  ESP_LOGI(TAG, "Adding cmd_vel sub");
   geometry_msgs__msg__Twist twist_msg;
-  RCCHECK(rclc_executor_add_subscription(
-      &executor, &subscriber_cmd_vel, &twist_msg,
-      &subscription_callback_cmd_vel, ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber_cmd_vel,
+                                         &twist_msg, &messages_cmd_vel,
+                                         ON_NEW_DATA));
 
-  ESP_LOGI(TAG, "Adding led subs");
+  ESP_LOGI(TAG, "Adding led sub");
   raspi_robot_msgs__msg__Leds leds_msg;
   RCCHECK(rclc_executor_add_subscription(&executor, &subscriber_leds, &leds_msg,
                                          &subscription_callback_leds,
                                          ON_NEW_DATA));
 
-  ESP_LOGI(TAG, "Adding motor subs");
+  ESP_LOGI(TAG, "Adding motor sub");
   raspi_robot_msgs__msg__Motors motors_msg;
   RCCHECK(rclc_executor_add_subscription(
       &executor, &subscriber_motors, &motors_msg, &subscription_callback_motors,
       ON_NEW_DATA));
 
-  ESP_LOGI(TAG, "Adding services");
-  raspi_robot_msgs__srv__SonarPosition_Request request;
-  raspi_robot_msgs__srv__SonarPosition_Response response;
-  RCCHECK(rclc_executor_add_service(&executor, &service_sonar_position,
-                                    &request, &response,
-                                    &service_sonar_position_callback));
+  ESP_LOGI(TAG, "Adding motor test sub");
+  raspi_robot_msgs__msg__MotorsTest motors_test_msg;
+  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber_motors_test,
+                                         &motors_test_msg,
+                                         &messages_motors_test, ON_NEW_DATA));
+
+  ESP_LOGI(TAG, "Adding sonar position sub");
+  raspi_robot_msgs__msg__SonarPosition sonar_position_msg;
+  RCCHECK(rclc_executor_add_subscription(
+      &executor, &subscriber_sonar_position, &sonar_position_msg,
+      &messages_sonar_position, ON_NEW_DATA));
 
   // Flash the blue LED when running.
   raspi_robot_init();
@@ -250,10 +256,11 @@ void appMain(void *arg) {
   RCCHECK(rcl_subscription_fini(&subscriber_cmd_vel, &node));
   RCCHECK(rcl_subscription_fini(&subscriber_leds, &node));
   RCCHECK(rcl_subscription_fini(&subscriber_motors, &node));
+  RCCHECK(rcl_subscription_fini(&subscriber_motors_test, &node));
+  RCCHECK(rcl_subscription_fini(&subscriber_sonar_position, &node));
   RCCHECK(rcl_publisher_fini(&publisher_battery_state, &node))
   RCCHECK(rcl_publisher_fini(&publisher_odometry, &node))
   RCCHECK(rcl_publisher_fini(&publisher_range, &node))
-  RCCHECK(rcl_service_fini(&service_sonar_position, &node));
   RCCHECK(rcl_node_fini(&node))
   nav_msgs__msg__Odometry__destroy(odometry_msg);
   sensor_msgs__msg__BatteryState__destroy(battery_state_msg);