Skip to content

Added Rotation Sensor to devices #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Johnnykoch02 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Added Rotation Sensor to devices #2

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
120 changes: 87 additions & 33 deletions include/TerriBull/Devices/device.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,7 @@
#define __TERRIBULL_DEVICES__

#include "../TerriBull.hpp"

class DeviceManager;
// namespace TerriBull {

/**
* E_DEVICE_NONE = 0,
Expand All @@ -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
Expand All @@ -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 {
Expand All @@ -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
*
Expand All @@ -71,42 +111,56 @@ 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<MotorDevice*>((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<RotationSensorDevice*>((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<RotationSensorDevice*>((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;
pros::c::imu_accel_s_t accel;
} 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