diff --git a/README.md b/README.md index d7b54ac..43ba79f 100644 --- a/README.md +++ b/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 diff --git a/build_system.md b/build_system.md index e2439f5..6fcafcb 100644 --- a/build_system.md +++ b/build_system.md @@ -59,7 +59,7 @@ ros2 run micro_ros_setup configure_firmware.sh -t udp -i [you using the value ``. 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 diff --git a/motor_calibration_driver.md b/motor_calibration_driver.md new file mode 100644 index 0000000..f9332df --- /dev/null +++ b/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) + diff --git a/raspi_robot_msgs/msg/MotorsTest.msg b/raspi_robot_msgs/msg/MotorsTest.msg new file mode 100644 index 0000000..76c6440 --- /dev/null +++ b/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 diff --git a/raspi_robot_msgs/srv/SonarPosition.srv b/raspi_robot_msgs/msg/SonarPosition.msg similarity index 58% rename from raspi_robot_msgs/srv/SonarPosition.srv rename to raspi_robot_msgs/msg/SonarPosition.msg index a94be03..b18b232 100644 --- a/raspi_robot_msgs/srv/SonarPosition.srv +++ b/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 diff --git a/rover/raspi_rover/app-colcon.meta b/rover/raspi_rover/app-colcon.meta index f6d9f63..830b665 100644 --- a/rover/raspi_rover/app-colcon.meta +++ b/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", diff --git a/rover/raspi_rover/app.c b/rover/raspi_rover/app.c index 632de6b..af160ec 100644 --- a/rover/raspi_rover/app.c +++ b/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,10 +46,9 @@ #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; @@ -55,7 +56,8 @@ 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); diff --git a/rover/raspi_rover/app_messages.c b/rover/raspi_rover/app_messages.c index 5fb8ed4..0aabb55 100644 --- a/rover/raspi_rover/app_messages.c +++ b/rover/raspi_rover/app_messages.c @@ -8,43 +8,16 @@ #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/motors_test.h" +#include "raspi_robot_msgs/msg/sonar_position.h" #include "rosidl_runtime_c/string.h" #include "sensor_msgs/msg/battery_state.h" #include "sensor_msgs/msg/range.h" #include "std_msgs/msg/header.h" -// Handy constants. -#define PI (3.1415f) -#define MS_PER_S (1000) -#define NS_PER_MS (1000 * 1000) -#define NS_PER_S (1000 * 1000 * 1000) - -// Information about the robot. -// Wheel diameter. -#define WHEEL_DIAMETER_MM (68) -#define WHEEL_CIRCUMFERENCE_M (PI * WHEEL_DIAMETER_MM / 1000.0f) -// Distance between centres of wheels. -#define WHEEL_BASE_MM (160) -#define WHEEL_BASE_M (WHEEL_BASE_MM / 1000) -// Encoder ticks per revolution. -#define ENCODER_TICKS_PER_REV (12) -#define METRES_PER_ENCODER_TICK (WHEEL_CIRCUMFERENCE_M / ENCODER_TICKS_PER_REV) -// Motor constants. -// FIXME(AJB) Validate these. Guesses for now. -#define MAXIMUM_SPEED_M_S (0.50) -#define MINIMUM_SPEED_M_S (0.10) -// The motors do not move the robot when less than this duty value. -// FIXME(AJB) Remove this when motor controller working. -#define MINIMUM_MOTOR_PERCENT (30) -// Ticks is preset to 1 second, 10 ticks. -#define MOTOR_TICKS (10) - -// FIXME(AJB) Hack based on knowledge of system at time of writing. -#define ODOMETRY_CALL_INTERVAL_MS (1000) -#define ODOMETRY_CALL_INTERVAL_S (ODOMETRY_CALL_INTERVAL_MS / MS_PER_S) - // Logging name. static const char *TAG = "app_messages"; @@ -133,8 +106,8 @@ static void calculate_odometry(const float delta_time_s, float *pose_x_m, *pose_x_m += (*velocity_x_m_s * cos(*velocity_theta)) * delta_time_s; *pose_y_m += (*velocity_x_m_s * sin(*velocity_theta)) * delta_time_s; *pose_theta += *velocity_theta * delta_time_s; - ESP_LOGI(TAG, "Odom pose: x %f, y %f, theta %f", *pose_x_m, - *pose_y_m, *pose_theta); + ESP_LOGI(TAG, "Odom pose: x %f, y %f, theta %f", *pose_x_m, *pose_y_m, + *pose_theta); } /**** API functions ****/ @@ -192,39 +165,29 @@ void messages_odometry(nav_msgs__msg__Odometry *msg) { msg->twist.twist.angular.z = velocity_theta; } -void messages_cmd_vel(const geometry_msgs__msg__Twist *msg) { +void messages_cmd_vel(const void *msg_in) { + const geometry_msgs__msg__Twist *msg = + (const geometry_msgs__msg__Twist *)msg_in; // Only use two variables in the message for differential drive. - float forward = msg->linear.x; - float rotate = msg->angular.z; - ESP_LOGI(TAG, "Received: forward %f, rotate %f", forward, rotate); - // Convert float to percent within motor limits. - int8_t left_percent = 0; - int8_t right_percent = 0; - // Do most calculations using absolute velocity. - if (forward < 0.0) { - forward = -forward; - } - // Clamp the maximum speed. - if (forward > MAXIMUM_SPEED_M_S) { - forward = MAXIMUM_SPEED_M_S; - } - // Convert the forward value to a motor percent. - if (forward > MINIMUM_SPEED_M_S) { - int8_t forward_percent = - (int8_t)((float)(forward / MAXIMUM_SPEED_M_S) * 100); - // Clamp minimum percent. - if (forward_percent < MINIMUM_MOTOR_PERCENT) { - forward_percent = MINIMUM_MOTOR_PERCENT; - } - // Add sign back in and convert to left and right values. - // To go forward, one motor is +ve and the other is -ve. - if (forward_percent > 0) { - left_percent = forward_percent; - right_percent = -forward_percent; - } else { - left_percent = -forward_percent; - right_percent = forward_percent; - } - } // Any value less than minimum speed is ignored. - raspi_robot_motors_drive(left_percent, right_percent, MOTOR_TICKS); + float linear_m_s = msg->linear.x; + float angular_r_s = msg->angular.z; + ESP_LOGI(TAG, "Received: linear_m_s %f, angular_r_s %f", linear_m_s, + angular_r_s); + raspi_robot_motors_set_velocities(linear_m_s, angular_r_s, MOTOR_TICKS); } + +void messages_motors_test(const void *msg_in) { + const raspi_robot_msgs__msg__MotorsTest *msg = + (const raspi_robot_msgs__msg__MotorsTest *)msg_in; + ESP_LOGI(TAG, "Received motors_test: %d", msg->test); + // TODO AJB +} + +void messages_sonar_position(const void *msg_in) { + raspi_robot_msgs__msg__SonarPosition *msg = + (raspi_robot_msgs__msg__SonarPosition *)msg_in; + ESP_LOGI(TAG, "Requested sonar position: x %d, y %d", msg->x, msg->y); + // TODO AJB +} + + diff --git a/rover/raspi_rover/app_messages.h b/rover/raspi_rover/app_messages.h index 3a69dc1..8613516 100644 --- a/rover/raspi_rover/app_messages.h +++ b/rover/raspi_rover/app_messages.h @@ -1,7 +1,6 @@ #ifndef APP_MESSAGES_H #define APP_MESSAGES_H -#include "geometry_msgs/msg/twist.h" #include "nav_msgs/msg/odometry.h" #include "sensor_msgs/msg/battery_state.h" #include "sensor_msgs/msg/range.h" @@ -9,6 +8,8 @@ void messages_battery_state(sensor_msgs__msg__BatteryState *msg); void messages_range(sensor_msgs__msg__Range *msg); void messages_odometry(nav_msgs__msg__Odometry *msg); -void messages_cmd_vel(const geometry_msgs__msg__Twist *msg); +void messages_cmd_vel(const void *msg_in); +void messages_motors_test(const void *msg_in); +void messages_sonar_position(const void *msg_in); #endif // APP_MESSAGES_H \ No newline at end of file diff --git a/rover/raspi_rover/raspi_robot_driver/include/raspi_robot_driver.h b/rover/raspi_rover/raspi_robot_driver/include/raspi_robot_driver.h index 2911cd1..47a1f27 100644 --- a/rover/raspi_rover/raspi_robot_driver/include/raspi_robot_driver.h +++ b/rover/raspi_rover/raspi_robot_driver/include/raspi_robot_driver.h @@ -4,6 +4,38 @@ #include #include +// Handy constants. +#define PI (3.1415f) +#define MS_PER_S (1000) +#define NS_PER_MS (1000 * 1000) +#define NS_PER_S (1000 * 1000 * 1000) + +// Information about the robot. +// Wheel diameter. +#define WHEEL_DIAMETER_MM (68) +#define WHEEL_CIRCUMFERENCE_M (PI * WHEEL_DIAMETER_MM / 1000.0f) +// Distance between centres of wheels. +#define WHEEL_BASE_MM (160) +#define WHEEL_BASE_M (WHEEL_BASE_MM / 1000) + +// Encoder ticks per revolution. +#define ENCODER_TICKS_PER_REV (12) +#define METRES_PER_ENCODER_TICK (WHEEL_CIRCUMFERENCE_M / ENCODER_TICKS_PER_REV) + +// Motor constants. +// FIXME(AJB) Validate these. Guesses for now. +#define MAXIMUM_SPEED_M_S (0.50) +#define MINIMUM_SPEED_M_S (0.10) +// The motors do not move the robot when less than this duty value. +// FIXME(AJB) Remove this when motor controller working. +#define MINIMUM_MOTOR_PERCENT (30) +// Ticks is preset to 1 second, 10 ticks. +#define MOTOR_TICKS (10) + +// FIXME(AJB) Hack based on knowledge of system at time of writing. +#define ODOMETRY_CALL_INTERVAL_MS (1000) +#define ODOMETRY_CALL_INTERVAL_S (ODOMETRY_CALL_INTERVAL_MS / MS_PER_S) + typedef enum { // The numbered values are used to access an internal array. RASPI_ROBOT_LED_ESP_BLUE = 0, @@ -35,7 +67,7 @@ typedef struct { bool switch1; bool switch2; /// The last value of the sonar in millimetres from the front of the unit. - /// Maximum range is 4 metres. + /// Maximum range is 4 metres but it is unreliable over 2 metres. uint16_t sonar_mm; } status_t; @@ -83,6 +115,24 @@ void raspi_robot_set_led(const raspi_robot_led_t led_in, void raspi_robot_motors_drive(int8_t percent_left, int8_t percent_right, uint16_t ticks); +/** + * @brief Drive the selected motors at the given speed for the given time. + * @param linear_m_s Desired speed in meters per second. +ve values are forward, + * -ve values are reverse. Desired velocities are limited to a range that the + * robot can deliver. + * @param angular_r_s Desired rotation in radians per second. +ve is clockwise, + * -ve is anti-clockwise. Desired velocities are limited to a range that the + * robot can deliver. + * @param ticks Duration to drive the motors in ticks. + */ +void raspi_robot_motors_set_velocities(float linear_m_s, float angular_r_s, + uint16_t ticks); + +/** + * @brief Calibrate the motors. + */ +void raspi_robot_motors_calibrate(); + /** * @brief Returns the current battery voltage. * diff --git a/rover/raspi_rover/raspi_robot_driver/src/raspi_robot_driver.c b/rover/raspi_rover/raspi_robot_driver/src/raspi_robot_driver.c index e0ab366..7ab3f02 100644 --- a/rover/raspi_rover/raspi_robot_driver/src/raspi_robot_driver.c +++ b/rover/raspi_rover/raspi_robot_driver/src/raspi_robot_driver.c @@ -133,7 +133,51 @@ void raspi_robot_motors_drive(int8_t percent_left, int8_t percent_right, update_encoder_counts(); } -uint32_t raspi_robot_get_battery_voltage() { return adc_battery_voltage(); } +void raspi_robot_motors_set_velocities(float linear_m_s, float angular_r_s, + uint16_t ticks) { + // Convert float to percent within motor limits. + int8_t left_percent = 0; + int8_t right_percent = 0; + // Do most calculations using absolute velocity. + float abs_velocity_m_s = 0; + if (linear_m_s < 0.0) { + abs_velocity_m_s = -linear_m_s; + } else { + abs_velocity_m_s = linear_m_s; + } + // Clamp the maximum speed. + if (abs_velocity_m_s > MAXIMUM_SPEED_M_S) { + abs_velocity_m_s = MAXIMUM_SPEED_M_S; + } + // Convert the forward value to a motor percent. + if (abs_velocity_m_s > MINIMUM_SPEED_M_S) { + int8_t forward_percent = + (int8_t)((float)(abs_velocity_m_s / MAXIMUM_SPEED_M_S) * 100); + // Clamp minimum percent. + if (forward_percent < MINIMUM_MOTOR_PERCENT) { + forward_percent = MINIMUM_MOTOR_PERCENT; + } + // Add sign back in and convert to left and right values. + // To go forward, one motor is +ve and the other is -ve. + if (forward_percent > 0) { + left_percent = forward_percent; + right_percent = -forward_percent; + } else { + left_percent = -forward_percent; + right_percent = forward_percent; + } + } // Any value less than minimum speed is ignored. + raspi_robot_motors_drive(left_percent, right_percent, MOTOR_TICKS); +} + +void raspi_robot_motors_calibrate() { + // Comment to keep formating sensible. +} + +uint32_t raspi_robot_get_battery_voltage() { + // Just return the result. + return adc_battery_voltage(); +} int16_t raspi_robot_get_hall_effect() { // Just return the result.