diff --git a/.vscode/settings.json b/.vscode/settings.json index 66059b9..fb5cfc1 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -31,7 +31,10 @@ "hw_encoder.h": "c", "stdbool.h": "c", "stdint.h": "c", - "type_traits": "c" + "type_traits": "c", + "math.h": "c", + "odometry_manager.h": "c", + "cmath": "c" }, "C_Cpp.errorSquiggles": "disabled", "cmake.sourceDirectory": "D:/Development/kinisi-motor-controller-firmware/.pio/libdeps/native/Unity" diff --git a/commands.json b/commands.json index 6905fda..82a6feb 100644 --- a/commands.json +++ b/commands.json @@ -1,5 +1,5 @@ { - "version": "1.0.3", + "version": "1.0.4", "commands": [ { "command": "INITIALIZE_MOTOR", @@ -268,6 +268,79 @@ }, "category": "Encoder" }, + { + "command": "START_ENCODER_ODOMETRY", + "code": "0x13", + "description": "This command starts the odometry calculation for the specified encoder.", + "properties": [ + { + "name": "encoder_index", + "type": "uint8_t", + "range": [ + 0, + 3 + ], + "description": "The index of the encoder to start the odometry calculation for." + } + ], + "category": "Encoder" + }, + { + "command": "RESET_ENCODER_ODOMETRY", + "code": "0x14", + "description": "This command resets the odometry calculation for the specified encoder.", + "properties": [ + { + "name": "encoder_index", + "type": "uint8_t", + "range": [ + 0, + 3 + ], + "description": "The index of the encoder to reset the odometry calculation for." + } + ], + "category": "Encoder" + }, + { + "command": "STOP_ENCODER_ODOMETRY", + "code": "0x15", + "description": "This command stops the odometry calculation for the specified encoder.", + "properties": [ + { + "name": "encoder_index", + "type": "uint8_t", + "range": [ + 0, + 3 + ], + "description": "The index of the encoder to stop the odometry calculation for." + } + ], + "category": "Encoder" + }, + { + "command": "GET_ENCODER_ODOMETRY", + "code": "0x16", + "description": "This command retrieves the odometry of the specified encoder.", + "properties": [ + { + "name": "encoder_index", + "type": "uint8_t", + "range": [ + 0, + 3 + ], + "description": "The index of the encoder to retrieve the odometry for." + } + ], + "response": { + "name": "odometry", + "type": "double", + "description": "The odometry of the encoder in radians." + }, + "category": "Encoder" + }, { "command": "INITIALIZE_GPIO_PIN", "code": "0x20", @@ -401,6 +474,12 @@ "type": "double", "default": 1, "description": "Diameter of the robot wheels in meters." + }, + { + "name": "encoder_resolution", + "type": "double", + "default": 0, + "description": "Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero." } ], "category": "Platform" @@ -439,6 +518,12 @@ "type": "double", "default": 1, "description": "Distance berween the center of the robot and the center of the wheels in millimeters." + }, + { + "name": "encoder_resolution", + "type": "double", + "default": 0, + "description": "Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero." } ], "category": "Platform" @@ -479,7 +564,7 @@ "category": "Platform" }, { - "command": "SET_PLATFORM_CONTROLLER", + "command": "START_PLATFORM_CONTROLLER", "code": "0x41", "description": "This command sets the controller for the platform.", "properties": [ @@ -498,11 +583,6 @@ "type": "double", "description": "Derivative constant of PID" }, - { - "name": "encoder_resolution", - "type": "double", - "description": "Encoder resolution in ticks per revolution. The value can not be negative or zero." - }, { "name": "integral_limit", "type": "double", @@ -542,7 +622,43 @@ "name": "platform_velocity", "type": "object", "description": "The current velocity of the platform in meters per second." - } + }, + "category": "Platform" + }, + { + "command": "STOP_PLATFORM_CONTROLLER", + "code": "0x44", + "description": "This command stops the controller for the platform.", + "category": "Platform" + }, + { + "command": "START_PLATFORM_ODOMETRY", + "code": "0x45", + "description": "This command starts the odometry calculation for the platform.", + "category": "Platform" + }, + { + "command": "RESET_PLATFORM_ODOMETRY", + "code": "0x46", + "description": "This command resets the odometry calculation for the platform.", + "category": "Platform" + }, + { + "command": "STOP_PLATFORM_ODOMETRY", + "code": "0x47", + "description": "This command stops the odometry calculation for the platform.", + "category": "Platform" + }, + { + "command": "GET_PLATFORM_ODOMETRY", + "code": "0x48", + "description": "This command retrieves the odometry of the platform in meters and radians.", + "response":{ + "name": "platform_odometry", + "type": "object", + "description": "The odometry of the platform in meters and radians." + }, + "category": "Platform" } ], "objects": [ @@ -612,6 +728,27 @@ "description": "Theta component of platform velocity in radians per second" } ] + }, + { + "name": "platform_odometry", + "description": "The odometry of the platform in meters and radians.", + "properties": [ + { + "name": "x", + "type": "double", + "description": "X component of platform odometry in meters" + }, + { + "name": "y", + "type": "double", + "description": "Y component of platform odometry in meters" + }, + { + "name": "t", + "type": "double", + "description": "Theta component of platform odometry in radians" + } + ] } ] } \ No newline at end of file diff --git a/commands.md b/commands.md index 61ff087..2ec6556 100644 --- a/commands.md +++ b/commands.md @@ -1,18 +1,18 @@ # Kinisi motor controller commands -Version: 1.0.3 +Version: 1.0.4 --- ## Commands ### INITIALIZE_MOTOR (0x01) -Description: This command initializes a motor and prepares it for use. +Description: This command initializes a motor and prepares it for use.\ Properties: - motor_index (uint8_t): The index of the motor to initialize. - Range: 0 to 3 - is_reversed (bool): Whether or not the motor is reversed. ### SET_MOTOR_SPEED (0x02) -Description: This command sets the speed of the specified motor in PWM. +Description: This command sets the speed of the specified motor in PWM.\ Properties: - motor_index (uint8_t): The index of the motor to set the speed for. - Range: 0 to 3 @@ -20,19 +20,19 @@ Properties: - Range: -100.0 to 100.0 ### STOP_MOTOR (0x03) -Description: This command stops motor by setting its speed to 0. +Description: This command stops motor by setting its speed to 0.\ Properties: - motor_index (uint8_t): The index of the motor to set the speed for. - Range: 0 to 3 ### BRAKE_MOTOR (0x04) -Description: This command brakes motor. +Description: This command brakes motor.\ Properties: - motor_index (uint8_t): The index of the motor to set the speed for. - Range: 0 to 3 ### INITIALIZE_MOTOR_CONTROLLER (0x05) -Description: This command sets the controller for the specified motor. +Description: This command sets the controller for the specified motor.\ Properties: - motor_index (uint8_t): The index of the motor to set the controller for. - Range: 0 to 3 @@ -46,20 +46,20 @@ Properties: - integral_limit (double): Integral limit of PID controller. The value can not be negative or zero. If the value is zero or negative, the integral limit is disabled. ### SET_MOTOR_TARGET_SPEED (0x06) -Description: This command sets the target speed for the specified motor in radians. +Description: This command sets the target speed for the specified motor in radians.\ Properties: - motor_index (uint8_t): The index of the motor to set the target velocity for. - Range: 0 to 3 - speed (double): The speed of the motor. ### RESET_MOTOR_CONTROLLER (0x07) -Description: This command resets the controller for the specified motor. +Description: This command resets the controller for the specified motor.\ Properties: - motor_index (uint8_t): The index of the motor to reset the controller for. - Range: 0 to 3 ### GET_MOTOR_CONTROLLER_STATE (0x08) -Description: This command gets the state of the controller for the specified motor. +Description: This command gets the state of the controller for the specified motor.\ Properties: - motor_index (uint8_t): The index of the motor to get the state for. - Range: 0 to 3 @@ -67,13 +67,13 @@ Response: - motor_controller_state (object): The state of the controller for the specified motor. ### DELETE_MOTOR_CONTROLLER (0x09) -Description: This command deletes the controller for the specified motor. +Description: This command deletes the controller for the specified motor.\ Properties: - motor_index (uint8_t): The index of the motor to delete the controller for. - Range: 0 to 3 ### INITIALIZE_ENCODER (0x11) -Description: This command initializes an encoder and prepares it for use. +Description: This command initializes an encoder and prepares it for use.\ Properties: - encoder_index (uint8_t): The index of the encoder to initialize. - Range: 0 to 3 @@ -81,49 +81,75 @@ Properties: - is_reversed (bool): Whether or not the encoder is reversed. ### GET_ENCODER_VALUE (0x12) -Description: This command retrieves the current value of the encoder. +Description: This command retrieves the current value of the encoder.\ Properties: - encoder_index (uint8_t): The index of the encoder to retrieve the value for. - Range: 0 to 3 Response: - encoderValue (uint16_t): The current value of the encoder. +### START_ENCODER_ODOMETRY (0x13) +Description: This command starts the odometry calculation for the specified encoder.\ +Properties: +- encoder_index (uint8_t): The index of the encoder to start the odometry calculation for. + - Range: 0 to 3 + +### RESET_ENCODER_ODOMETRY (0x14) +Description: This command resets the odometry calculation for the specified encoder.\ +Properties: +- encoder_index (uint8_t): The index of the encoder to reset the odometry calculation for. + - Range: 0 to 3 + +### STOP_ENCODER_ODOMETRY (0x15) +Description: This command stops the odometry calculation for the specified encoder.\ +Properties: +- encoder_index (uint8_t): The index of the encoder to stop the odometry calculation for. + - Range: 0 to 3 + +### GET_ENCODER_ODOMETRY (0x16) +Description: This command retrieves the odometry of the specified encoder.\ +Properties: +- encoder_index (uint8_t): The index of the encoder to retrieve the odometry for. + - Range: 0 to 3 +Response: + - odometry (double): The odometry of the encoder in radians. + ### INITIALIZE_GPIO_PIN (0x20) -Description: This command initializes a digital pin and prepares it for use. +Description: This command initializes a digital pin and prepares it for use.\ Properties: - pin_number (uint8_t): The number of the pin to initialize. - mode (uint8_t): Set digital pin as input or output. Modes: 0 = INPUT_PULLDOWN, 1 = INPUT_PULLUP, 2 = INPUT_NOPULL, 3 = OUTPUT. ### SET_GPIO_PIN_STATE (0x21) -Description: This command sets the specified pin to a state. +Description: This command sets the specified pin to a state.\ Properties: - pin_number (uint8_t): The number of the pin to set to a state. - state (uint8_t): The state of the pin. 0 = LOW, 1 = HIGH. ### GET_GPIO_PIN_STATE (0x22) -Description: This command gets the state of the specified pin. +Description: This command gets the state of the specified pin.\ Properties: - pin_number (uint8_t): The number of the pin to get the state for. Response: - state (uint8_t): The state of the pin. 0 = LOW, 1 = HIGH. ### TOGGLE_GPIO_PIN_STATE (0x23) -Description: This command toggles the specified pin. +Description: This command toggles the specified pin.\ Properties: - pin_number (uint8_t): The number of the pin to toggle. ### SET_STATUS_LED_STATE (0x25) -Description: This command sets the status LED to a state. +Description: This command sets the status LED to a state.\ Properties: - state (uint8_t): The state of the status LED. 0 = OFF, 1 = ON. ### TOGGLE_STATUS_LED_STATE (0x26) -Description: This command toggles the status LED. +Description: This command toggles the status LED.\ Properties: - None ### INITIALIZE_MECANUM_PLATFORM (0x30) -Description: This command initializes a mecanum platform and prepares it for use. +Description: This command initializes a mecanum platform and prepares it for use.\ Properties: - is_reversed_0 (bool): Determins if motor 0 is reversed. - is_reversed_1 (bool): Determins if motor 1 is reversed. @@ -132,18 +158,20 @@ Properties: - length (double): Length of the platform in meters. - width (double): Width of the platform in meters. - wheels_diameter (double): Diameter of the robot wheels in meters. +- encoder_resolution (double): Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero. ### INITIALIZE_OMNI_PLATFORM (0x31) -Description: This command initializes a omni platform and prepares it for use. +Description: This command initializes a omni platform and prepares it for use.\ Properties: - is_reversed_0 (bool): Determins if motor 0 is reversed. - is_reversed_1 (bool): Determins if motor 1 is reversed. - is_reversed_2 (bool): Determins if motor 2 is reversed. - wheels_diameter (double): Diameter of the robot wheels in millimeters. - robot_radius (double): Distance berween the center of the robot and the center of the wheels in millimeters. +- encoder_resolution (double): Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero. ### SET_PLATFORM_VELOCITY (0x40) -Description: This command sets the velocity for the platform in PWM. +Description: This command sets the velocity for the platform in PWM.\ Properties: - x (double): X component of platform velocity in PWM - Range: -100.0 to 100.0 @@ -152,26 +180,52 @@ Properties: - t (double): Theta component of platform velocity in PWM - Range: -100.0 to 100.0 -### SET_PLATFORM_CONTROLLER (0x41) -Description: This command sets the controller for the platform. +### START_PLATFORM_CONTROLLER (0x41) +Description: This command sets the controller for the platform.\ Properties: - kp (double): Proportional constant of PID - ki (double): Integral constant of PID - kd (double): Derivative constant of PID -- encoder_resolution (double): Encoder resolution in ticks per revolution. The value can not be negative or zero. - integral_limit (double): Integral limit of PID controller. The value can not be negative or zero. If the value is zero or negative, the integral limit is disabled. ### SET_PLATFORM_TARGET_VELOCITY (0x42) -Description: This command set the target velocity for the platform in meters per second. +Description: This command set the target velocity for the platform in meters per second.\ Properties: - x (double): X component of platform velocity in meters per second - y (double): Y component of platform velocity in meters per second - t (double): Theta component of platform velocity in radians per second ### GET_PLATFORM_CURRENT_VELOCITY (0x43) -Description: This command gets the current velocity of the platform in meters per second. +Description: This command gets the current velocity of the platform in meters per second.\ Properties: - None Response: - platform_velocity (object): The current velocity of the platform in meters per second. +### STOP_PLATFORM_CONTROLLER (0x44) +Description: This command stops the controller for the platform.\ +Properties: +- None + +### START_PLATFORM_ODOMETRY (0x45) +Description: This command starts the odometry calculation for the platform.\ +Properties: +- None + +### RESET_PLATFORM_ODOMETRY (0x46) +Description: This command resets the odometry calculation for the platform.\ +Properties: +- None + +### STOP_PLATFORM_ODOMETRY (0x47) +Description: This command stops the odometry calculation for the platform.\ +Properties: +- None + +### GET_PLATFORM_ODOMETRY (0x48) +Description: This command retrieves the odometry of the platform in meters and radians.\ +Properties: +- None +Response: + - platform_odometry (object): The odometry of the platform in meters and radians. + diff --git a/include/commands.h b/include/commands.h index 05a18a1..8abcfa0 100644 --- a/include/commands.h +++ b/include/commands.h @@ -2,8 +2,8 @@ // Kinisi motr controller commands. // This file is auto generated by the commands generator from JSON file. // Do not edit this file manually. -// Timestamp: 2024-02-11 18:22:00 -// Version: 1.0.3 +// Timestamp: 2024-02-20 22:32:00 +// Version: 1.0.4 // ---------------------------------------------------------------------------- #pragma once @@ -43,6 +43,18 @@ // This command retrieves the current value of the encoder. #define GET_ENCODER_VALUE 0x12 +// This command starts the odometry calculation for the specified encoder. +#define START_ENCODER_ODOMETRY 0x13 + +// This command resets the odometry calculation for the specified encoder. +#define RESET_ENCODER_ODOMETRY 0x14 + +// This command stops the odometry calculation for the specified encoder. +#define STOP_ENCODER_ODOMETRY 0x15 + +// This command retrieves the odometry of the specified encoder. +#define GET_ENCODER_ODOMETRY 0x16 + // This command initializes a digital pin and prepares it for use. #define INITIALIZE_GPIO_PIN 0x20 @@ -71,7 +83,7 @@ #define SET_PLATFORM_VELOCITY 0x40 // This command sets the controller for the platform. -#define SET_PLATFORM_CONTROLLER 0x41 +#define START_PLATFORM_CONTROLLER 0x41 // This command set the target velocity for the platform in meters per second. #define SET_PLATFORM_TARGET_VELOCITY 0x42 @@ -79,6 +91,21 @@ // This command gets the current velocity of the platform in meters per second. #define GET_PLATFORM_CURRENT_VELOCITY 0x43 +// This command stops the controller for the platform. +#define STOP_PLATFORM_CONTROLLER 0x44 + +// This command starts the odometry calculation for the platform. +#define START_PLATFORM_ODOMETRY 0x45 + +// This command resets the odometry calculation for the platform. +#define RESET_PLATFORM_ODOMETRY 0x46 + +// This command stops the odometry calculation for the platform. +#define STOP_PLATFORM_ODOMETRY 0x47 + +// This command retrieves the odometry of the platform in meters and radians. +#define GET_PLATFORM_ODOMETRY 0x48 + // motor_controller_state: The state of the controller for the specified motor. #pragma pack(push, 1) @@ -105,6 +132,16 @@ typedef struct } platform_velocity; #pragma pack(pop) +// platform_odometry: The odometry of the platform in meters and radians. +#pragma pack(push, 1) +typedef struct +{ + double x; // X component of platform odometry in meters + double y; // Y component of platform odometry in meters + double t; // Theta component of platform odometry in radians +} platform_odometry; +#pragma pack(pop) + #pragma pack(push, 1) typedef struct @@ -178,6 +215,26 @@ typedef struct uint8_t encoder_index; // The index of the encoder to retrieve the value for. } get_encoder_value; + // START_ENCODER_ODOMETRY: This command starts the odometry calculation for the specified encoder. + struct { + uint8_t encoder_index; // The index of the encoder to start the odometry calculation for. + } start_encoder_odometry; + + // RESET_ENCODER_ODOMETRY: This command resets the odometry calculation for the specified encoder. + struct { + uint8_t encoder_index; // The index of the encoder to reset the odometry calculation for. + } reset_encoder_odometry; + + // STOP_ENCODER_ODOMETRY: This command stops the odometry calculation for the specified encoder. + struct { + uint8_t encoder_index; // The index of the encoder to stop the odometry calculation for. + } stop_encoder_odometry; + + // GET_ENCODER_ODOMETRY: This command retrieves the odometry of the specified encoder. + struct { + uint8_t encoder_index; // The index of the encoder to retrieve the odometry for. + } get_encoder_odometry; + // INITIALIZE_GPIO_PIN: This command initializes a digital pin and prepares it for use. struct { uint8_t pin_number; // The number of the pin to initialize. @@ -218,6 +275,7 @@ typedef struct double length; // Length of the platform in meters. double width; // Width of the platform in meters. double wheels_diameter; // Diameter of the robot wheels in meters. + double encoder_resolution; // Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero. } initialize_mecanum_platform; // INITIALIZE_OMNI_PLATFORM: This command initializes a omni platform and prepares it for use. @@ -227,6 +285,7 @@ typedef struct uint8_t is_reversed_2; // Determins if motor 2 is reversed. double wheels_diameter; // Diameter of the robot wheels in millimeters. double robot_radius; // Distance berween the center of the robot and the center of the wheels in millimeters. + double encoder_resolution; // Encoder resolution in ticks per revolution. The value can not be negative. If platform does not have encoders, the value should be set to zero. } initialize_omni_platform; // SET_PLATFORM_VELOCITY: This command sets the velocity for the platform in PWM. @@ -236,14 +295,13 @@ typedef struct double t; // Theta component of platform velocity in PWM } set_platform_velocity; - // SET_PLATFORM_CONTROLLER: This command sets the controller for the platform. + // START_PLATFORM_CONTROLLER: This command sets the controller for the platform. struct { double kp; // Proportional constant of PID double ki; // Integral constant of PID double kd; // Derivative constant of PID - double encoder_resolution; // Encoder resolution in ticks per revolution. The value can not be negative or zero. double integral_limit; // Integral limit of PID controller. The value can not be negative or zero. If the value is zero or negative, the integral limit is disabled. - } set_platform_controller; + } start_platform_controller; // SET_PLATFORM_TARGET_VELOCITY: This command set the target velocity for the platform in meters per second. struct { @@ -256,6 +314,26 @@ typedef struct struct { } get_platform_current_velocity; + // STOP_PLATFORM_CONTROLLER: This command stops the controller for the platform. + struct { + } stop_platform_controller; + + // START_PLATFORM_ODOMETRY: This command starts the odometry calculation for the platform. + struct { + } start_platform_odometry; + + // RESET_PLATFORM_ODOMETRY: This command resets the odometry calculation for the platform. + struct { + } reset_platform_odometry; + + // STOP_PLATFORM_ODOMETRY: This command stops the odometry calculation for the platform. + struct { + } stop_platform_odometry; + + // GET_PLATFORM_ODOMETRY: This command retrieves the odometry of the platform in meters and radians. + struct { + } get_platform_odometry; + } properties; } controller_command_t; #pragma pack(pop) diff --git a/include/controllers_manager.h b/include/controllers_manager.h index ead3dd9..0ccd1e7 100644 --- a/include/controllers_manager.h +++ b/include/controllers_manager.h @@ -7,7 +7,6 @@ #include "hw_encoder.h" #include #include -#include "semphr.h" #include "commands.h" // Define motor identifiers as bit masks @@ -36,10 +35,16 @@ void controllers_manager_initialize_controller(uint8_t motor_index, uint8_t enco kp: Proportional gain ki: Integral gain kd: Derivative gain - encoder_resolution: Encoder resolution in pulses per revolution integral_limit: Integral limit must be positive. If negative or zero, integral limit is disabled */ -void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, double kp, double ki, double kd, double encoder_resolution, double integral_limit); +void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, double kp, double ki, double kd, double integral_limit); + +/* +Stop controller for selected motor +Parameters: + motor_index: Motor index +*/ +void controllers_manager_stop_controller_multiple(uint8_t motor_selection); /* Delete controller for single motor diff --git a/include/hw_config.h b/include/hw_config.h index 821ff88..ee14c57 100644 --- a/include/hw_config.h +++ b/include/hw_config.h @@ -15,6 +15,7 @@ #ifdef MP_V3 // Motor configurations #define NUMBER_MOTORS 4 + #define NUMBER_ENCODERS 4 static const motor_info_t motor_info[NUMBER_MOTORS] = { { @@ -130,6 +131,7 @@ #ifdef MP_V2 // Motor configurations #define NUMBER_MOTORS 4 + #define NUMBER_ENCODERS 4 static const motor_info_t motor_info[NUMBER_MOTORS] = { { @@ -241,6 +243,7 @@ // Motor configurations #define NUMBER_MOTORS 4 + #define NUMBER_ENCODERS 4 static const motor_info_t motor_info[NUMBER_MOTORS] = { { diff --git a/include/odometry_manager.h b/include/odometry_manager.h new file mode 100644 index 0000000..f7bc8e8 --- /dev/null +++ b/include/odometry_manager.h @@ -0,0 +1,63 @@ +//------------------------------------------------------------ +// File name: odometry_manager.h +//------------------------------------------------------------ + +#pragma once + +#include +#include + +/* +Initialize odometry manager +*/ +void odometry_manager_initialize(); + +/* +Start encoder odometry +Parameters: + encoder_index: Index of the encoder +*/ +void encoder_start_odometry(uint8_t encoder_index); + +/* +Reset encoder odometry +Parameters: + encoder_index: Index of the encoder +*/ +void encoder_reset_odometry(uint8_t encoder_index); + +/* +Get encoder odometry +Parameters: + encoder_index: Index of the encoder +Returns: + Odometry of the encoder +*/ +double encoder_get_odometry(uint8_t encoder_index); + +/* +Stop encoder odometry +Parameters: + encoder_index: Index of the encoder +*/ +void encoder_stop_odometry(uint8_t encoder_index); + +/* +Is odometry manager initialized +*/ +uint8_t odometry_manager_is_not_initialized(); + +/* +Initialize odometry manager +*/ +void odometry_manager_initialize(); + +/* +Get platform odometry +*/ +platform_odometry_t odometry_manager_get_platform_odometry(); + +/* +Reset platform odometry +*/ +void odometry_manager_reset_platform_odometry(); \ No newline at end of file diff --git a/include/platform.h b/include/platform.h index d7c0fb9..aa990d8 100644 --- a/include/platform.h +++ b/include/platform.h @@ -23,10 +23,16 @@ typedef struct{ double kp; double ki; double kd; - double encoder_resolution; // Encoder resolution in pulses per revolution double integral_limit; // Integral limit must be positive. If negative or zero, integral limit is disabled }plaform_controller_settings_t; +// Platform odometry +typedef struct{ + double x; + double y; + double t; +} platform_odometry_t; + /* Initialize mecanum platform Parameters: @@ -37,8 +43,9 @@ Initialize mecanum platform length: Length of the platform in meters width: Width of the platform in meters wheel_diameter: Wheel diameter in meters + encoder_resolution: Encoder resolution in pulses per revolution */ -void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, uint8_t isReversed3, double length, double width, double wheel_diameter); +void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, uint8_t isReversed3, double length, double width, double wheel_diameter, double encoder_resolution); /* Initialize omni platform @@ -48,33 +55,74 @@ Initialize omni platform isReversed2: If true, the motor 2 is reversed wheel_diameter: Wheel diameter in meters robot_radius: Robot radius in meters + encoder_resolution: Encoder resolution in pulses per revolution */ -void initialize_omni_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, double wheel_diameter, double robot_radius); +void initialize_omni_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, double wheel_diameter, double robot_radius, double encoder_resolution); -/* Set platform velocity +/* +Set platform velocity Parameters: platform_velocity: Platform velocity. x, y and t are in PWM units [-100, 100] */ void set_platform_velocity(platform_velocity_t platform_velocity); // Controller functions -/* Initialize controller for current platform +/* +Start velocity controller for current platform Parameters: plaform_controller_settings: Platform controller settings */ -void platform_initialize_controller(plaform_controller_settings_t plaform_controller_settings); +void platform_start_velocity_controller(plaform_controller_settings_t plaform_controller_settings); -/* Set target velocity for current platform +/* +Set target velocity for current platform Parameters: platform_target_velocity: Platform target velocity. x, y and t are in meters per second */ void platform_set_target_velocity(platform_velocity_t platform_target_velocity); +/* +Stop velocity controller for current platform +*/ +void platform_stop_velocity_controller(); -void encoder_update_state(encoder_index_t index, unsigned int value); +// Odometry functions +/* +Start calculating platform odometry +*/ +void platform_start_odometry(); -unsigned int encoder_get_value(encoder_index_t index); -int encoder_get_velocity(encoder_index_t index); -int encoder_get_acceleration(encoder_index_t index); -// void init_motor_rps(motorIndex index); -// int get_motor_rps(motorIndex index); \ No newline at end of file +/* +Check if platform odometry calculation is enabled +Returns: + 1 if platform odometry calculation is enabled, 0 if not +*/ +uint8_t platform_is_odometry_enabled(); + +/* +Reset platform odometry +*/ +void platform_reset_odometry(); + +/* +Stop calculating platform odometry +*/ +void platform_stop_odometry(); + +/* +Get platform odometry +Returns: + Platform odometry +*/ +platform_odometry_t platform_get_odometry(); + +/* +platform update odometry +Parameters: + motor_indexes: Motor indexes + velocities: Velocities + motor_count: Motor count +Returns: + Platform odometry +*/ +platform_odometry_t platform_update_odometry(uint8_t* motor_indexes, double* velocities, uint8_t motor_count); \ No newline at end of file diff --git a/lib/hardware/hw_encoder.c b/lib/hardware/hw_encoder.c index 616c97c..77dce28 100644 --- a/lib/hardware/hw_encoder.c +++ b/lib/hardware/hw_encoder.c @@ -69,7 +69,7 @@ static void initialize_encoder_timer(TIM_HandleTypeDef *htim, TIM_TypeDef *typeD htim->Init.Period = 65535; // This value can't be 4294967295 since some of the timeers are 16 bit. htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - sConfig.EncoderMode = TIM_ENCODERMODE_TI1; + sConfig.EncoderMode = TIM_ENCODERMODE_TI12; sConfig.IC1Polarity = TIM_ICPOLARITY_RISING; sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI; sConfig.IC1Prescaler = TIM_ICPSC_DIV1; diff --git a/lib/hardware/hw_gpio.h b/lib/hardware/hw_gpio.h index abda697..5b58adf 100644 --- a/lib/hardware/hw_gpio.h +++ b/lib/hardware/hw_gpio.h @@ -37,4 +37,4 @@ void toggle_gpio_pin_state(uint8_t pin); // Status LED void initialize_status_led(); void set_status_led_state(uint8_t state); -void toggle_status_led(); +void toggle_status_led_state(); diff --git a/src/command_handler.c b/src/command_handler.c index d1923a2..1be4456 100644 --- a/src/command_handler.c +++ b/src/command_handler.c @@ -3,6 +3,7 @@ //------------------------------------------------------------ #include "commands.h" #include "controllers_manager.h" +#include "odometry_manager.h" #include #include #include @@ -102,6 +103,35 @@ void command_handler(controller_command_t* cmd, void (*command_callback)(uint8_t } break; + case START_ENCODER_ODOMETRY: + { + encoder_start_odometry( + cmd->properties.start_encoder_odometry.encoder_index); + } + break; + + case RESET_ENCODER_ODOMETRY: + { + encoder_reset_odometry( + cmd->properties.reset_encoder_odometry.encoder_index); + } + break; + + case GET_ENCODER_ODOMETRY: + { + double odometry = encoder_get_odometry( + cmd->properties.get_encoder_odometry.encoder_index); + command_callback((uint8_t*)&odometry, sizeof(double)); + } + break; + + case STOP_ENCODER_ODOMETRY: + { + encoder_stop_odometry( + cmd->properties.stop_encoder_odometry.encoder_index); + } + break; + // GPIO commands case INITIALIZE_GPIO_PIN: initialize_gpio_pin( @@ -144,7 +174,8 @@ void command_handler(controller_command_t* cmd, void (*command_callback)(uint8_t cmd->properties.initialize_mecanum_platform.is_reversed_3, cmd->properties.initialize_mecanum_platform.length, cmd->properties.initialize_mecanum_platform.width, - cmd->properties.initialize_mecanum_platform.wheels_diameter + cmd->properties.initialize_mecanum_platform.wheels_diameter, + cmd->properties.initialize_mecanum_platform.encoder_resolution ); break; @@ -154,7 +185,8 @@ void command_handler(controller_command_t* cmd, void (*command_callback)(uint8_t cmd->properties.initialize_omni_platform.is_reversed_1, cmd->properties.initialize_omni_platform.is_reversed_2, cmd->properties.initialize_omni_platform.wheels_diameter, - cmd->properties.initialize_omni_platform.robot_radius + cmd->properties.initialize_omni_platform.robot_radius, + cmd->properties.initialize_omni_platform.encoder_resolution ); break; @@ -169,17 +201,22 @@ void command_handler(controller_command_t* cmd, void (*command_callback)(uint8_t } break; - case SET_PLATFORM_CONTROLLER: + case START_PLATFORM_CONTROLLER: { plaform_controller_settings_t plaform_controller_settings = { - .kp = cmd->properties.set_platform_controller.kp, - .ki = cmd->properties.set_platform_controller.ki, - .kd = cmd->properties.set_platform_controller.kd, - .encoder_resolution = cmd->properties.set_platform_controller.encoder_resolution, - .integral_limit = cmd->properties.set_platform_controller.integral_limit + .kp = cmd->properties.start_platform_controller.kp, + .ki = cmd->properties.start_platform_controller.ki, + .kd = cmd->properties.start_platform_controller.kd, + .integral_limit = cmd->properties.start_platform_controller.integral_limit }; - platform_initialize_controller(plaform_controller_settings); + platform_start_velocity_controller(plaform_controller_settings); + } + break; + + case STOP_PLATFORM_CONTROLLER: + { + platform_stop_velocity_controller(); } break; @@ -193,5 +230,30 @@ void command_handler(controller_command_t* cmd, void (*command_callback)(uint8_t platform_set_target_velocity(platform_target_velocity); } break; + + case START_PLATFORM_ODOMETRY: + { + platform_start_odometry(); } + break; + + case RESET_PLATFORM_ODOMETRY: + { + platform_reset_odometry(); + } + break; + + case GET_PLATFORM_ODOMETRY: + { + platform_odometry_t platform_odometry = platform_get_odometry(); + command_callback((uint8_t*)&platform_odometry, sizeof(platform_odometry_t)); + } + break; + + case STOP_PLATFORM_ODOMETRY: + { + platform_stop_odometry(); + } + break; + } } \ No newline at end of file diff --git a/src/controllers_manager.c b/src/controllers_manager.c index 755b0d1..df3a393 100644 --- a/src/controllers_manager.c +++ b/src/controllers_manager.c @@ -4,6 +4,7 @@ #include "controllers_manager.h" #include "commands.h" #include +#include #include #include #include @@ -31,6 +32,7 @@ typedef struct controllers_manager_state double target_motor_speed[4]; // In radians per second controller_info_t Controller_info[4]; uint32_t update_interval_ms; + uint16_t previousEncoderValue[4]; SemaphoreHandle_t controller_state_mutex; } controllers_manager_state_t; @@ -46,6 +48,7 @@ static controllers_manager_t controllers_manager = { .target_motor_speed = {0}, .Controller_info = {{.state = STOP}, {.state = STOP},{.state = STOP},{.state = STOP}}, .update_interval_ms = 100, + .previousEncoderValue = {0}, .controller_state_mutex = NULL } }; @@ -64,7 +67,6 @@ void StartControllerTask(void *argument) const TickType_t xFrequency = pdMS_TO_TICKS(controllers_manager_state->update_interval_ms); TickType_t xLastWakeTime = xTaskGetTickCount(); unsigned int seq = 0; - uint16_t previousEncoderValue[4] = { 0 }; for(;;) { @@ -87,7 +89,7 @@ void StartControllerTask(void *argument) // Get current velocity from encoder const uint16_t current_encoder_value = get_encoder_value(index); // Calculate change in encoder value - uint16_t raw_change = current_encoder_value - previousEncoderValue[index]; + uint16_t raw_change = current_encoder_value - controllers_manager_state->previousEncoderValue[index]; // Check for overflow and adjust int last_encoder_change; @@ -97,7 +99,7 @@ void StartControllerTask(void *argument) last_encoder_change = raw_change; // No overflow or underflow } - previousEncoderValue[index] = current_encoder_value; + controllers_manager_state->previousEncoderValue[index] = current_encoder_value; // Caltulate current motor speed in radians per second from encoder ticks double current_motor_speed = 2.0 * M_PI * ((double)last_encoder_change / encoder_get_resolution(index)) * (1.0 / controller->T); @@ -184,27 +186,14 @@ void controllers_manager_initialize_controller(uint8_t motor_index, uint8_t enco if (xSemaphoreTake(controllers_manager.state.controller_state_mutex, portMAX_DELAY)) { + controllers_manager.state.previousEncoderValue[motor_index] = get_encoder_value(encoder_index); controllers_manager.state.Controller_info[motor_index] = controller_info; xSemaphoreGive(controllers_manager.state.controller_state_mutex); } } -void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, double kp, double ki, double kd, double encoder_resolution, double integral_limit) +void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, double kp, double ki, double kd, double integral_limit) { - // Check if all selected motors and encoders are initialized - for (uint8_t index = 0; index < NUMBER_MOTORS; index++) - { - if (motor_selection & (1 << index)) - { - // TODO Add assert here to check if motor initialized - // At this point motor should be initialized because platform already initialized - // assert(motor_is_initialized(motor_index) == 1); - - // Initialize encoder if not initialized - initialize_encoder(index, encoder_resolution, motor_is_reversed(index)); - } - } - // Initialize controller manager which starts task for all controllers if (controllers_manager_is_not_init()) { @@ -235,6 +224,7 @@ void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, controller_info.mIndex = motor_index; controller_info.eIndex = motor_index; + controllers_manager.state.previousEncoderValue[motor_index] = get_encoder_value(motor_index); controllers_manager.state.Controller_info[motor_index] = controller_info; } } @@ -243,6 +233,28 @@ void controllers_manager_initialize_controller_multiple(uint8_t motor_selection, } } +void controllers_manager_stop_controller_multiple(uint8_t motor_selection) +{ + if (xSemaphoreTake(controllers_manager.state.controller_state_mutex, portMAX_DELAY)) + { + for (uint8_t motor_index = 0; motor_index < NUMBER_MOTORS; motor_index++) + { + if (motor_selection & (1 << motor_index)) + { + controllers_manager.state.Controller_info[motor_index].state = STOP; + controllers_manager.state.Controller_info[motor_index].controller = (pid_controller_t){0}; + + // Stop motor + stop_motor(controllers_manager.state.Controller_info[motor_index].mIndex); + + // Set target speed to zero + controllers_manager.state.target_motor_speed[motor_index] = 0; + } + } + xSemaphoreGive(controllers_manager.state.controller_state_mutex); + } +} + void controllers_manager_delete_controller(uint8_t motor_index) { if (xSemaphoreTake(controllers_manager.state.controller_state_mutex, portMAX_DELAY)) { diff --git a/src/odometry_manager.c b/src/odometry_manager.c new file mode 100644 index 0000000..425c24a --- /dev/null +++ b/src/odometry_manager.c @@ -0,0 +1,231 @@ +//------------------------------------------------------------ +// File name: odometry_manager.c +//------------------------------------------------------------ +#include +#include +#include +#include +#include +#include +#include +#include + +#define ODOMETRY_UPDATE_INTERVAL 50 + +// Odometry manager state +typedef struct +{ + uint8_t is_initialized[NUMBER_ENCODERS]; + uint16_t encoder_previous_value[NUMBER_ENCODERS]; + double odometry[NUMBER_ENCODERS]; // in Radians + double odometry_change[NUMBER_ENCODERS]; // in Radians + osThreadId_t thread_handler; + uint32_t update_interval; // in ms + SemaphoreHandle_t odometry_mutex; + + // Platform odometry + platform_odometry_t platform_odometry; +} odometry_manager_state_t; + +static odometry_manager_state_t odometry_manager_state = { + .is_initialized = {0}, + .encoder_previous_value = {0}, + .odometry = {0}, + .thread_handler = NULL, + .update_interval = ODOMETRY_UPDATE_INTERVAL, + .odometry_mutex = NULL +}; + +//------------------------------------------------------------ +void odometry_manager_task(void *argument) +{ + odometry_manager_state_t *state = (odometry_manager_state_t *)argument; + + const TickType_t xFrequency = pdMS_TO_TICKS(state->update_interval); + TickType_t xLastWakeTime = xTaskGetTickCount(); + + while (1) + { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + // Obtain odometry mutex + if (xSemaphoreTake(state->odometry_mutex, portMAX_DELAY)) + { + // Update odometry for each encoder + for(int i = 0; i < NUMBER_ENCODERS; i++) + { + if(state->is_initialized[i]) + { + uint16_t encoder_value = get_encoder_value(i); + double resolution = encoder_get_resolution(i)/ (2 * M_PI); // Ticks per radians + uint16_t encoder_values_change_raw = encoder_value - state->encoder_previous_value[i]; + + // Check for overflow and adjust + double encoder_values_change = encoder_values_change_raw; + if (encoder_values_change_raw > 32768) { // Half of UINT16_MAX, detecting large backward movement (underflow) + encoder_values_change = -(65536 - encoder_values_change_raw); // Adjust for underflow + } + + state->odometry_change[i] = encoder_values_change / resolution; + state->odometry[i] = state->odometry[i] + state->odometry_change[i]; + + state->encoder_previous_value[i] = encoder_value; + } + } + + // Update odometry for the platform if it is initialized + if (platform_is_odometry_enabled()) + { + uint8_t motor_indexes[NUMBER_MOTORS]; + double velocities[NUMBER_MOTORS]; + for (int i = 0; i < NUMBER_MOTORS; i++) + { + motor_indexes[i] = i; + velocities[i] = state->odometry_change[i]; + } + + platform_odometry_t odometry_change = platform_update_odometry(motor_indexes, velocities, NUMBER_MOTORS); + state->platform_odometry.x += odometry_change.x; + state->platform_odometry.y += odometry_change.y; + state->platform_odometry.t += odometry_change.t; + } + osDelay(1); + xSemaphoreGive(state->odometry_mutex); + } + } +} + +//------------------------------------------------------------ +uint8_t odometry_manager_is_not_initialized() +{ + osThreadState_t status = osThreadGetState(odometry_manager_state.thread_handler); + return status == osThreadError; +} + +//------------------------------------------------------------ +void odometry_manager_initialize() +{ + if(odometry_manager_is_not_initialized()) + { + odometry_manager_state.odometry_mutex = xSemaphoreCreateMutex(); + + const osThreadAttr_t CommandsTask_attributes = { + .name = "CommandsTask", + .stack_size = 128 * 8, + .priority = (osPriority_t) osPriorityNormal, + }; + odometry_manager_state.thread_handler = osThreadNew(odometry_manager_task, &odometry_manager_state, &CommandsTask_attributes); + } +} + +//------------------------------------------------------------ +void encoder_start_odometry(uint8_t encoder_index) +{ + // Initialize odometry manager if it is not initialized + odometry_manager_initialize(); + + if(encoder_is_initialized(encoder_index) == 0) + { + return; + } + + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + odometry_manager_state.is_initialized[encoder_index] = 1; + odometry_manager_state.odometry[encoder_index] = 0; + // TODO: Should set previous value on first run of the update task + odometry_manager_state.encoder_previous_value[encoder_index] = get_encoder_value(encoder_index); + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } +} + +//------------------------------------------------------------ +void encoder_reset_odometry(uint8_t encoder_index) +{ + // Return if odometry manager is not initialized + if (odometry_manager_is_not_initialized()) + { + return; + } + + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + odometry_manager_state.odometry[encoder_index] = 0; + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } +} + +//------------------------------------------------------------ +double encoder_get_odometry(uint8_t encoder_index) +{ + double odometry = 0; + + // Check if odometry manager is initialized + if (!odometry_manager_is_not_initialized()) + { + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + odometry = odometry_manager_state.odometry[encoder_index]; + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } + } + + return odometry; +} + +//------------------------------------------------------------ +void encoder_stop_odometry(uint8_t encoder_index) +{ + // Return if odometry manager is not initialized + if (odometry_manager_is_not_initialized()) + { + return; + } + + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + odometry_manager_state.is_initialized[encoder_index] = 0; + odometry_manager_state.odometry[encoder_index] = 0; + odometry_manager_state.encoder_previous_value[encoder_index] = 0; + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } +} + +//------------------------------------------------------------ +platform_odometry_t odometry_manager_get_platform_odometry(){ + platform_odometry_t platform_odometry = {0}; + + // Check if odometry manager is initialized + if (!odometry_manager_is_not_initialized()) + { + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + platform_odometry = odometry_manager_state.platform_odometry; + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } + } + + return platform_odometry; +} + +//------------------------------------------------------------ +void odometry_manager_reset_platform_odometry(){ + // Return if odometry manager is not initialized + if (odometry_manager_is_not_initialized()) + { + return; + } + + // Obtain odometry mutex + if (xSemaphoreTake(odometry_manager_state.odometry_mutex, portMAX_DELAY)) + { + odometry_manager_state.platform_odometry.x = 0; + odometry_manager_state.platform_odometry.y = 0; + odometry_manager_state.platform_odometry.t = 0; + xSemaphoreGive(odometry_manager_state.odometry_mutex); + } +} \ No newline at end of file diff --git a/src/platform.c b/src/platform.c index 2cf122a..324d8ce 100644 --- a/src/platform.c +++ b/src/platform.c @@ -8,13 +8,17 @@ #include #include #include +#include #define SPEED_RESOLUTION 840 // Function types definitions typedef void (*set_platform_velocity_t)(platform_velocity_t); typedef void (*set_platform_target_velocity_t)(platform_velocity_t); -typedef void (*initialize_platform_controller_t)(plaform_controller_settings_t); +typedef void (*start_platform_velocity_controller_t)(plaform_controller_settings_t); +typedef void (*stop_platform_velocity_controller_t)(); +typedef void (*initialize_platform_odometry_t)(); +typedef platform_odometry_t (*update_platform_odometry_t)(uint8_t* motor_indexes, double* velocities, uint8_t motor_count); // Mecanum platform settings typedef struct @@ -43,23 +47,22 @@ typedef struct // Platform functions set_platform_velocity_t set_platform_velocity; set_platform_target_velocity_t set_platform_target_velocity; - initialize_platform_controller_t initialize_platform_controller; + start_platform_velocity_controller_t start_platform_velocity_controller; + stop_platform_velocity_controller_t stop_platform_velocity_controller; // Platform properties union { mecanum_platform_settings_t mecanum; omni_platform_settings_t omni; } properties; + + // Platform odometry + uint8_t is_odometry_enabled; + initialize_platform_odometry_t initialize_platform_odometry; + update_platform_odometry_t update_platform_odometry; + } platform_t; -typedef struct -{ - unsigned int value_n0; - unsigned int value_n1; - int velocity_n0; - int velocity_n1; - int acceleration_n0; -} encoder_state_t; typedef struct mecanum_velocity_t { @@ -74,8 +77,6 @@ static platform_t platform = { .is_initialized = 0, .is_controller_initialized = 0}; -static encoder_state_t encoder_state[4]; - // Function definitions double verify_range(double c); double sing(double c); @@ -105,25 +106,49 @@ void platform_set_target_velocity(platform_velocity_t platform_target_velocity) return; } - // TODO: Is controller initialized? - platform.set_platform_target_velocity(platform_target_velocity); } -void platform_initialize_controller(plaform_controller_settings_t plaform_controller_settings) +void platform_start_velocity_controller(plaform_controller_settings_t plaform_controller_settings) { if (!platform.is_initialized) { return; } - platform.initialize_platform_controller(plaform_controller_settings); + platform.start_platform_velocity_controller(plaform_controller_settings); platform.is_controller_initialized = 1; } +void platform_stop_velocity_controller() +{ + if (!platform.is_initialized || !platform.is_controller_initialized) + { + return; + } + + platform.is_controller_initialized = 0; + platform.stop_platform_velocity_controller(); +} + +platform_odometry_t platform_update_odometry(uint8_t* motor_indexes, double* velocities, uint8_t motor_count) +{ + if (!platform.is_initialized) + { + platform_odometry_t odometry = { + .x = 0, + .y = 0, + .t = 0 + }; + return odometry; + } + + return platform.update_platform_odometry(motor_indexes, velocities, motor_count); +} + // ------------------------------------------------------------------------ // Mecanum platform functions -void set_mecaunm_platform_velocity(platform_velocity_t platform_velocity){ +void mecaunm_platform_set_velocity(platform_velocity_t platform_velocity){ // Normalize velocity int l = abs(platform_velocity.x) + abs(platform_velocity.y) + abs(platform_velocity.t); platform_velocity.x = sing(platform_velocity.x) * platform_velocity.x * platform_velocity.x / l; @@ -143,14 +168,18 @@ void set_mecaunm_platform_velocity(platform_velocity_t platform_velocity){ set_motor_speed(MOTOR3, mecanum_velocity.motor3); } -void mecanum_platform_initialize_controller(plaform_controller_settings_t plaform_controller_settings) +void mecanum_platform_stop_velocity_controller() +{ + controllers_manager_stop_controller_multiple(BMOTOR0 | BMOTOR1 | BMOTOR2 | BMOTOR3); +} + +void mecanum_platform_start_velocity_controller(plaform_controller_settings_t plaform_controller_settings) { controllers_manager_initialize_controller_multiple( BMOTOR0 | BMOTOR1 | BMOTOR2 | BMOTOR3, plaform_controller_settings.kp, plaform_controller_settings.ki, plaform_controller_settings.kd, - plaform_controller_settings.encoder_resolution, plaform_controller_settings.integral_limit); } @@ -171,7 +200,47 @@ void mecanum_platform_set_target_velocity(platform_velocity_t platform_target_ve } -void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, uint8_t isReversed3, double length, double width, double wheel_diameter) +platform_odometry_t mecanum_platform_update_odometry(uint8_t* motor_indexes, double* velocities, uint8_t motor_count) +{ + platform_odometry_t odometry = { + .x = 0, + .y = 0, + .t = 0 + }; + + // Motor count must be 4 + if(motor_count < 4) + { + return odometry; + } + + // Calculate odometry (forwards kinematics) + // TODO: Verify direction of y axis + double R = platform.properties.mecanum.wheel_diameter / 2.0; // Radius of the wheel in meters + double L = platform.properties.mecanum.length; // Distance from front to back wheels in meters + double W = platform.properties.mecanum.width; // Distance between left and right wheels in meters + double v1 = velocities[0]; + double v2 = velocities[1]; + double v3 = velocities[2]; + double v4 = velocities[3]; + + odometry.x = R/4.0 * (v1 + v2 + v3 + v4); + odometry.y = R/4.0 * (v1 - v2 + v3 - v4); + odometry.t = R/(2.0 * (L + W)) * (v1 + v2 - v3 - v4); + + return odometry; +} + +void mecanum_platform_start_odometry() +{ + // Start odometry for each motor encoder + encoder_start_odometry(MOTOR0); + encoder_start_odometry(MOTOR1); + encoder_start_odometry(MOTOR2); + encoder_start_odometry(MOTOR3); +} + +void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, uint8_t isReversed3, double length, double width, double wheel_diameter, double encoder_resolution) { if (platform.is_initialized) { @@ -180,14 +249,27 @@ void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8 platform.properties.mecanum.length = length; platform.properties.mecanum.width = width; + initialize_motor(MOTOR0, isReversed0); initialize_motor(MOTOR1, isReversed1); initialize_motor(MOTOR2, isReversed2); initialize_motor(MOTOR3, isReversed3); + + if (encoder_resolution > 0) + { + initialize_encoder(MOTOR0, encoder_resolution, isReversed0); + initialize_encoder(MOTOR1, encoder_resolution, isReversed1); + initialize_encoder(MOTOR2, encoder_resolution, isReversed2); + initialize_encoder(MOTOR3, encoder_resolution, isReversed3); + } + platform.is_initialized = true; - platform.set_platform_velocity = set_mecaunm_platform_velocity; - platform.initialize_platform_controller = mecanum_platform_initialize_controller; + platform.set_platform_velocity = mecaunm_platform_set_velocity; + platform.start_platform_velocity_controller = mecanum_platform_start_velocity_controller; + platform.stop_platform_velocity_controller = mecanum_platform_stop_velocity_controller; platform.set_platform_target_velocity = mecanum_platform_set_target_velocity; + platform.initialize_platform_odometry = mecanum_platform_start_odometry; + platform.update_platform_odometry = mecanum_platform_update_odometry; platform.properties.mecanum.wheel_diameter = wheel_diameter; platform.properties.mecanum.length = length; @@ -199,9 +281,9 @@ void initialize_mecanum_platform(uint8_t isReversed0, uint8_t isReversed1, uint8 void set_omni_platform_velocity(platform_velocity_t platform_velocity) { - double V1 = sqrt(3.0)/2 * platform_velocity.x - 1/2 * platform_velocity.y + platform_velocity.t; - double V2 = -sqrt(3.0)/2 * platform_velocity.x - 1/2 * platform_velocity.y + platform_velocity.t; - double V3 = -platform_velocity.y + platform_velocity.t; + double V1 = sqrt(3.0)/2.0 * platform_velocity.x - 0.5 * platform_velocity.y + platform_velocity.t; + double V2 = -sqrt(3.0)/2.0 * platform_velocity.x - 0.5 * platform_velocity.y + platform_velocity.t; + double V3 = platform_velocity.y + platform_velocity.t; double maxv = fmax(fabs(V1), fmax(fabs(V2), fabs(V3))); if (maxv > 100.0) @@ -216,31 +298,71 @@ void set_omni_platform_velocity(platform_velocity_t platform_velocity) set_motor_speed(MOTOR2, V3); } -void omni_platform_initialize_controller(plaform_controller_settings_t plaform_controller_settings) +void omni_platform_start_velocity_controller(plaform_controller_settings_t plaform_controller_settings) { controllers_manager_initialize_controller_multiple( BMOTOR0 | BMOTOR1 | BMOTOR2, plaform_controller_settings.kp, plaform_controller_settings.ki, plaform_controller_settings.kd, - plaform_controller_settings.encoder_resolution, plaform_controller_settings.integral_limit); } +void omni_platform_stop_velocity_controller() +{ + controllers_manager_stop_controller_multiple(BMOTOR0 | BMOTOR1 | BMOTOR2); +} + void omni_platform_set_target_velocity(platform_velocity_t platform_target_velocity) { double R = platform.properties.omni.wheel_diameter / 2.0; // Radius of the wheel in meters double L = platform.properties.omni.robot_radius; // Distance from the center of the platform to the wheel in meters - double V1 = 1.0 / R * (sqrt(3.0)/2.0 * platform_target_velocity.x - 1.0/2.0 * platform_target_velocity.y + L * platform_target_velocity.t); - double V2 = 1.0 / R * (-sqrt(3.0)/2.0 * platform_target_velocity.x - 1.0/2.0 * platform_target_velocity.y + L * platform_target_velocity.t); - double V3 = 1.0 / R * (-platform_target_velocity.y + L * platform_target_velocity.t); + double V1 = 1.0 / R * (sqrt(3.0)/2.0 * platform_target_velocity.x - 0.5 * platform_target_velocity.y + L * platform_target_velocity.t); + double V2 = 1.0 / R * (-sqrt(3.0)/2.0 * platform_target_velocity.x - 0.5 * platform_target_velocity.y + L * platform_target_velocity.t); + double V3 = 1.0 / R * (platform_target_velocity.y + L * platform_target_velocity.t); uint8_t motor_indexes[] = {MOTOR0, MOTOR1, MOTOR2}; double target_speeds[] = {V1, V2, V3}; controllers_manager_set_target_speed_multiple(motor_indexes, target_speeds, 3); } -void initialize_omni_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, double wheel_diameter, double robot_radius) +platform_odometry_t omni_platform_update_odometry(uint8_t* motor_indexes, double* velocities, uint8_t motor_count) +{ + platform_odometry_t odometry = { + .x = 0, + .y = 0, + .t = 0 + }; + + // Motor count must be more than or equal 3 + if(motor_count < 3) + { + return odometry; + } + + // Calculate odometry (forwards kinematics) + double R = platform.properties.omni.wheel_diameter / 2.0; // Radius of the wheel in meters + double L = platform.properties.omni.robot_radius; // Distance from the center of the platform to the wheel in meters + double v1 = velocities[0]; + double v2 = velocities[1]; + double v3 = velocities[2]; + + odometry.x = R * (1/sqrt(3.0) * v1 - 1/sqrt(3.0) * v2); + odometry.y = R/3.0 * (-v1 - v2 + 2.0 * v3); + odometry.t = R/(3.0 * L) * (v1 + v2 + v3); + + return odometry; +} + +void initialize_omni_platform_odometry() +{ + // Start odometry for each motor encoder + encoder_start_odometry(MOTOR0); + encoder_start_odometry(MOTOR1); + encoder_start_odometry(MOTOR2); +} + +void initialize_omni_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t isReversed2, double wheel_diameter, double robot_radius, double encoder_resolution) { if (platform.is_initialized) { @@ -250,57 +372,62 @@ void initialize_omni_platform(uint8_t isReversed0, uint8_t isReversed1, uint8_t initialize_motor(MOTOR0, isReversed0); initialize_motor(MOTOR1, isReversed1); initialize_motor(MOTOR2, isReversed2); + + if (encoder_resolution > 0) + { + initialize_encoder(MOTOR0, encoder_resolution, isReversed0); + initialize_encoder(MOTOR1, encoder_resolution, isReversed1); + initialize_encoder(MOTOR2, encoder_resolution, isReversed2); + } + platform.is_initialized = true; platform.set_platform_velocity = set_omni_platform_velocity; - platform.initialize_platform_controller = omni_platform_initialize_controller; + platform.start_platform_velocity_controller = omni_platform_start_velocity_controller; + platform.stop_platform_velocity_controller = omni_platform_stop_velocity_controller; platform.set_platform_target_velocity = omni_platform_set_target_velocity; + platform.initialize_platform_odometry = initialize_omni_platform_odometry; + platform.update_platform_odometry = omni_platform_update_odometry; platform.properties.omni.wheel_diameter = wheel_diameter; platform.properties.omni.robot_radius = robot_radius; } // ------------------------------------------------------------------------ -// Encoder functions +// Odometry functions -void encoder_update_state(encoder_index_t index, unsigned int value) +void platform_start_odometry() { - encoder_state[index].value_n1 = encoder_state[index].value_n0; - encoder_state[index].value_n0 = value; - encoder_state[index].velocity_n1 = encoder_state[index].velocity_n0; - encoder_state[index].velocity_n0 = encoder_state[index].value_n0 - encoder_state[index].value_n1; - encoder_state[index].acceleration_n0 = encoder_state[index].velocity_n0 - encoder_state[index].velocity_n1; -} + // Initialize platform hardware for odometry if it is not initialized + platform.initialize_platform_odometry(); -unsigned int encoder_get_value(encoder_index_t index) -{ - return encoder_state[index].value_n0; + platform.is_odometry_enabled = 1; + + // Initialize odometry manager if it is not initialized + odometry_manager_initialize(); } -int encoder_get_velocity(encoder_index_t index) +uint8_t platform_is_odometry_enabled() { - return encoder_state[index].velocity_n0; + return platform.is_odometry_enabled; } -int get_encoder_acceleration(encoder_index_t index) +void platform_reset_odometry() { - return encoder_state[index].acceleration_n0; + // Reset odometry in odometry manager + odometry_manager_reset_platform_odometry(); } -/* -void init_motor_with_encoder(motorIndex mindex, encoder_index eindex) +void platform_stop_odometry() { - initialize_motor(index); - initialize_encoder(index); - encoder_values[index] = get_encoder_value(index); + platform.is_odometry_enabled = 0; } -int get_motor_rps(motorIndex index) +platform_odometry_t platform_get_odometry() { - + // Get current platform odometry from odometry manager + return odometry_manager_get_platform_odometry(); } -*/ - -// ------------------------------------------------------------------------ +// --------------------------------------------------4---------------------- // Utils functions double verify_range(double c) diff --git a/test/test_commands/test_commands.c b/test/test_commands/test_commands.c index c272b7b..7e86d9b 100644 --- a/test/test_commands/test_commands.c +++ b/test/test_commands/test_commands.c @@ -14,20 +14,19 @@ void test_set_motor_speed_command(void) char buffer[10] = {0X02, 0x02, 0x01, 0x11, 0xAA, 0x00, 0x00, 0x00, 0x00, 0x00}; controller_command_t* command = (controller_command_t*)(&buffer[0]); TEST_ASSERT_EQUAL_INT(command->commandType, SET_MOTOR_SPEED); - TEST_ASSERT_EQUAL_INT(command->properties.setMotorSpeed.motorIndex, 2); - TEST_ASSERT_EQUAL_INT(command->properties.setMotorSpeed.direction, 1); - TEST_ASSERT_EQUAL_INT(command->properties.setMotorSpeed.speed, 43537); + TEST_ASSERT_EQUAL_INT(command->properties.set_motor_speed.motor_index, 2); + TEST_ASSERT_EQUAL_DOUBLE(command->properties.set_motor_speed.pwm, 1); } void test_set_motor_controller_command(void) { char buffer[] = {0x03,0x00,0x33,0x33,0x33,0x33,0x33,0x33,0xd3,0x3f,0x9a,0x99,0x99,0x99,0x99,0x99,0xc9,0x3f,0x9a,0x99,0x99,0x99,0x99,0x99,0xb9,0x3f}; controller_command_t* command = (controller_command_t*)(&buffer[0]); - TEST_ASSERT_EQUAL_INT(command->commandType, MOTOR_SET_CONTROLLER); - TEST_ASSERT_EQUAL_INT(command->properties.setMotorSpeed.motorIndex, 0); - TEST_ASSERT_EQUAL_DOUBLE(command->properties.setMotorController.kp, 0.3); - TEST_ASSERT_EQUAL_DOUBLE(command->properties.setMotorController.ki, 0.2); - TEST_ASSERT_EQUAL_DOUBLE(command->properties.setMotorController.kd, 0.1); + TEST_ASSERT_EQUAL_INT(command->commandType, INITIALIZE_MOTOR_CONTROLLER); + TEST_ASSERT_EQUAL_INT(command->properties.initialize_motor_controller.motor_index, 0); + TEST_ASSERT_EQUAL_DOUBLE(command->properties.initialize_motor_controller.kp, 0.3); + TEST_ASSERT_EQUAL_DOUBLE(command->properties.initialize_motor_controller.ki, 0.2); + TEST_ASSERT_EQUAL_DOUBLE(command->properties.initialize_motor_controller.kd, 0.1); } int main(void) { diff --git a/tools/commands_generator/README.md b/tools/commands_generator/README.md index c53def2..29af103 100644 --- a/tools/commands_generator/README.md +++ b/tools/commands_generator/README.md @@ -21,14 +21,6 @@ python generator.py --language y + +# TODO: Verify y axis direction + +# Inverse kinematics of mecanum platform +def inverse_kinematics_mecanum(platform_target_velocity, R, L, W): + x = platform_target_velocity['x'] + y = platform_target_velocity['y'] + t = platform_target_velocity['t'] + + V1 = 1.0 / R * (x + y + (L + W) / 2 * t) + V2 = 1.0 / R * (x - y + (L + W) / 2 * t) + V3 = 1.0 / R * (x + y - (L + W) / 2 * t) + V4 = 1.0 / R * (x - y - (L + W) / 2 * t) + return {'v1': V1, 'v2': V2, 'v3': V3, 'v4': V4} + +# Forward kinematics of mecanum platform +def forward_kinematics_mecanum(velocities, R, L, W): + v1 = velocities['v1'] + v2 = velocities['v2'] + v3 = velocities['v3'] + v4 = velocities['v4'] + + x = R/4.0 * (v1 + v2 + v3 + v4) + y = R/4.0 * (v1 - v2 + v3 - v4) + t = R / (2.0 * (L + W)) * (v1 + v2 - v3 - v4) + + return {'x': x, 'y': y, 't': t} + +R = 0.1 +L = 0.4 +W = 0.3 +platform_velocity = {'x': 15.0, 'y': 30.0, 't': 20.0} +print("Input velocities:" ) +print(platform_velocity) + +print("Inverse kinematics") +velocities = inverse_kinematics_mecanum(platform_velocity, R, L, W) +print(velocities) + +print("Forward kinematics") +odometry = forward_kinematics_mecanum(velocities, R, L, W) +print(odometry) \ No newline at end of file diff --git a/tools/omni.py b/tools/omni.py new file mode 100644 index 0000000..fa79695 --- /dev/null +++ b/tools/omni.py @@ -0,0 +1,34 @@ +import math + +def set_omni_platform_velocity(platform_velocity, R, L): + V1 = 1/R * (math.sqrt(3.0)/2 * platform_velocity['x'] - 0.5 * platform_velocity['y'] + L * platform_velocity['t']) + V2 = 1/R * (-math.sqrt(3.0)/2 * platform_velocity['x'] - 0.5 * platform_velocity['y'] + L * platform_velocity['t']) + V3 = 1/R * (platform_velocity['y'] + L*platform_velocity['t']) + + return {'v1': V1, 'v2': V2, 'v3':V3} + + +def omni_platform_forward_kinematics(velocities, R, L): + v1 = velocities['v1'] + v2 = velocities['v2'] + v3 = velocities['v3'] + + x = R * (1/math.sqrt(3)*v1 - 1/math.sqrt(3)*v2) + y = R * (-1/3*v1 - 1/3*v2 + 2/3 * v3) + t = R * (1/(3*L)*v1 + 1/(3*L)*v2 + 1/(3*L)*v3) + + return {'x': x, 'y': y, 't': t} + +R = 2 +L = 3 +platform_velocity = {'x': 100.0, 'y': 0.0, 't': 0.0} +print("Input velocities:" ) +print(platform_velocity) + +print("Inverse kinematics") +velocities = set_omni_platform_velocity(platform_velocity, R, L) +print(velocities) + +print("Forward kinematics") +odometry = omni_platform_forward_kinematics(velocities, R, L) +print(odometry) \ No newline at end of file