diff --git a/include/TerriBull/Devices/device.hpp b/include/TerriBull/Devices/device.hpp index 5c80441..346d91f 100644 --- a/include/TerriBull/Devices/device.hpp +++ b/include/TerriBull/Devices/device.hpp @@ -2,8 +2,7 @@ #define __TERRIBULL_DEVICES__ #include "../TerriBull.hpp" - -class DeviceManager; +// namespace TerriBull { /** * E_DEVICE_NONE = 0, @@ -18,13 +17,13 @@ class DeviceManager; * E_DEVICE_GPS = 20, * E_DEVICE_SERIAL = 129, */ -typedef struct { +typedef struct { pros::c::v5_device_e_t pros_device_type; uint8_t port; } DeviceHeader; /** - * E_MOTOR_GEARSET_36 = 0, // 36:1, 100 RPM, Red gear set + * E_MOTOR_GEARSET_36 = 0, // 36:1, 100 RPM, Red 6gear set * E_MOTOR_GEAR_RED = E_MOTOR_GEARSET_36, * E_MOTOR_GEAR_100 = E_MOTOR_GEARSET_36, * E_MOTOR_GEARSET_18 = 1, // 18:1, 200 RPM, Green gear set @@ -37,10 +36,9 @@ typedef struct { */ typedef enum device_callback_return_code { - FUNC_NOT_FOUND = 0, - SUCCESS = 1, - DEVICE_TYPE_MISMATCH = 2, - DEVICE_NOT_EXIST = 3 + SUCCESS = 0, + DEVICE_TYPE_MISMATCH = 1, + DEVICE_NOT_EXIST = 2, }; typedef struct { @@ -52,12 +50,54 @@ typedef struct { // Other motor-specific members } MotorDevice; +typedef struct { + DeviceHeader header; // First member is the DeviceHeader + double position; + // Other rotation sensor-specific members +} RotationSensorDevice; +/** + * @brief Should Initialize device with Port and proto data + * + * @param port + * @param data contains {uint8_t port, int GEAR_SET, int BREAK_MODE} + * @return device_callback_return_code + */ typedef struct { uint8_t port; int GEAR_SET; int BREAK_MODE; } motor_initialize_callback_data; +typedef struct { + uint8_t port; +} rotation_sensor_initialize_callback_data; + + +device_callback_return_code motor_device_initalize(TerriBull::DeviceManager* _motherSys, motor_initialize_callback_data data) { + MotorDevice* device = (MotorDevice*) malloc(sizeof(MotorDevice)); + device->header.pros_device_type = pros::c::v5_device_e_t::E_DEVICE_MOTOR; + device->header.port = data.port; + device->gear_set = (pros::motor_gearset_e_t) data.GEAR_SET; + device->break_mode = (pros::motor_brake_mode_e_t) data.BREAK_MODE; + pros::c::motor_tare_position(device->header.port); + pros::c::motor_set_brake_mode(device->header.port, device->break_mode); + pros::c::motor_set_gearing(device->header.port, device->gear_set); + device->velocity = pros::c::motor_get_actual_velocity(device->header.port); + _motherSys->add_device(device); // pointer to device + return device_callback_return_code::SUCCESS; +} + + +device_callback_return_code rotation_sensor_initialize(TerriBull::DeviceManager* _motherSys, rotation_sensor_initialize_callback_data data) { + RotationSensorDevice* device = (RotationSensorDevice*) malloc(sizeof(RotationSensorDevice)); + device->header.pros_device_type = pros::c::v5_device_e_t::E_DEVICE_ROTATION; + device->header.port = data.port; + pros::c::rotation_reset(device->header.port); + device->position = pros::c::rotation_get_position(device->header.port); + _motherSys->add_device(device); // pointer to device + return device_callback_return_code::SUCCESS; +} + /** * @brief Should set motors velocity to the proto data * @@ -71,10 +111,47 @@ typedef struct { int16_t velocity; } motor_set_velocity_callback_data; +typedef struct { + uint8_t port; + double position; +} rotation_sensor_set_position_callback_data; + +typedef struct { + uint8_t port; +} rotation_sensor_reset_position_callback_data; + +device_callback_return_code motor_device_set_velocity(TerriBull::DeviceManager* _motherSys, motor_set_velocity_callback_data data) { + DeviceHeader* device = _motherSys->get_device(data.port); + if (device != nullptr) return device_callback_return_code::DEVICE_NOT_EXIST; + if( ! device->pros_device_type == pros::c::v5_device_e_t::E_DEVICE_MOTOR) return device_callback_return_code::DEVICE_TYPE_MISMATCH; + + MotorDevice* motor = static_cast((void*)device); + pros::c::motor_move_velocity(motor->header.port, data.velocity); + return device_callback_return_code::SUCCESS; +} + + +device_callback_return_code rotation_sensor_set_position(TerriBull::DeviceManager* _motherSys, rotation_sensor_set_position_callback_data data) { + DeviceHeader* device = _motherSys->get_device(data.port); + if (device != nullptr) return device_callback_return_code::DEVICE_NOT_EXIST; + if( ! device->pros_device_type == pros::c::v5_device_e_t::E_DEVICE_ROTATION) return device_callback_return_code::DEVICE_TYPE_MISMATCH; + RotationSensorDevice* rotation_sensor = static_cast((void*)device); + pros::c::rotation_set_position(rotation_sensor->header.port, data.position); + return device_callback_return_code::SUCCESS; +} + +device_callback_return_code rotation_sensor_reset_position(TerriBull::DeviceManager* _motherSys, rotation_sensor_reset_position_callback_data data) { + DeviceHeader* device = _motherSys->get_device(data.port); + if (device != nullptr) return device_callback_return_code::DEVICE_NOT_EXIST; + if( ! device->pros_device_type == pros::c::v5_device_e_t::E_DEVICE_ROTATION) return device_callback_return_code::DEVICE_TYPE_MISMATCH; + + RotationSensorDevice* rotation_sensor = static_cast((void*)device); + pros::c::rotation_reset(rotation_sensor->header.port); + return device_callback_return_code::SUCCESS; typedef struct { - DeviceHeader header; + DeviceHeader header; // First member is the DeviceHeader float roll; float yaw; float pitch; @@ -82,31 +159,8 @@ typedef struct { } IMUDevice; typedef struct { - DeviceHeader header; + DeviceHeader header; // First member is the DeviceHeader float value; } AnalogDevice; - -#include "../DeviceManager.hpp" - -// Callbacks and Function Definitions - -/** - * @brief Should Initialize motor device with Port and proto data - * - * @param data contains {uint8_t port, int GEAR_SET, int BREAK_MODE} - * @return device_callback_return_code - */ -device_callback_return_code motor_device_initalize(DeviceManager* _motherSys, motor_initialize_callback_data data); -/** - * @brief Should set motors velocity to the proto data - * - * @param port - * @param data contains {uint8_t port, int GEAR_SET, int BREAK_MODE} - * @return device_callback_return_code - * This velocity corresponds to different actual speeds depending on the gearset used for the motor. This results in a range of +-100 for E_MOTOR_GEARSET_36, +-200 for E_MOTOR_GEARSET_18, and +-600 for blue. The velocity is held with PID to ensure consistent speed, as opposed to setting the motor’s voltage. - */ -device_callback_return_code motor_device_set_velocity(DeviceManager* _motherSys, motor_set_velocity_callback_data data); - - #endif \ No newline at end of file