-
Notifications
You must be signed in to change notification settings - Fork 0
/
pilsbot_driver.h
131 lines (103 loc) · 3.69 KB
/
pilsbot_driver.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
#pragma once
#include "pid.h"
#include <chrono>
#include "hardware_interface/base_interface.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_status_values.hpp"
#include <rclcpp/rclcpp.hpp>
#include <pilsbot_driver/linear_interpolation.hpp>
// #include <diagnostic_msgs/DiagnosticStatus.h>
// #include <diagnostic_msgs/DiagnosticArray.h>
// #include <diagnostic_msgs/KeyValue.h>
constexpr int max_length = 1024;
int hoverboard_fd = -1;
class HoverboardAPI;
using linear_interpolator::CalibrationListSerialized;
namespace pilsbot_driver
{
class PilsbotDriver : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
{
typedef LinearInterpolator<unsigned, double> PotInterpolator;
public:
~PilsbotDriver();
RCLCPP_SHARED_PTR_DEFINITIONS(PilsbotDriver)
hardware_interface::return_type configure(const hardware_interface::HardwareInfo &info) override;
hardware_interface::return_type start() override;
hardware_interface::return_type stop() override;
hardware_interface::return_type read() override;
hardware_interface::return_type write() override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
void tick();
private:
enum class WhichSerial
{
hoverboard,
head_mcu
};
void read_from_head_mcu_continuously();
bool set_head_mcu_pin(const bool pin);
bool set_update_period_of_head_mcu(head_mcu::UpdatePeriodMs period_ms);
bool try_setup_serial(const WhichSerial which);
void try_toggle_hoverboard_power();
struct WheelStatus {
double commanded_turning_rate = 0;
double curr_position = 0;
double curr_speed = 0;
};
struct HoverboardSensors {
double voltage = 0;
double avg_amperage_motor0 = 0;
double avg_amperage_motor1 = 0;
double txBufferLevel = 0;
};
struct SteeringAxleSensors {
double steering_angle_raw;
double endstop_l; // stupid state interface only allows double
double endstop_r; // stupid state interface only allows double
double steering_angle_normalized;
};
struct Params {
double wheel_radius = 0.125;
struct {
std::string tty_device = "/dev/ttyHoverboard";
PID::Settings pid = PID::Settings {
.Kp = .28, .Ki = 1, .Kd = .001,
.dt = 1, .max = 600, .min = NAN, //min/max is PWM (0-2000)
.max_dv = 200, .overshoot_integral_adaptation = .5
};
} hoverboard;
struct {
std::string tty_device = "/dev/ttyHeadMCU";
unsigned baudrate = 115200;
unsigned update_period_ms = 5;
CalibrationListSerialized calibration_val = {
// TODO: proper config loading
50601, -1.56601,
32432, 0.0,
14142, 1.56601
};
} head_mcu;
unsigned serial_connect_retries = 3;
// Minimum calculated speed to have the wheels moving.
} params_;
//int hoverboard_fd = -1; // declared globally because of ugly api
int head_mcu_fd = -1;
std::vector<WheelStatus> wheels_;
PID wheel_controller_l;
PID wheel_controller_r;
HoverboardSensors hoverboard_sensors_;
// head_mcu threading
std::atomic<bool> stop_;
PotInterpolator interpolator_;
SteeringAxleSensors axle_sensors_;
std::thread reading_function_;
rclcpp::Clock clock;
rclcpp::Time last_serial_read; // last time read from serial
rclcpp::Time last_write_tick; // last time a 'write' tick happened
HoverboardAPI *api;
};
}