Thunderbots Project
Loading...
Searching...
No Matches
power_frame_msg.hpp
1#pragma once
2
3#include <pb_decode.h>
4#include <pb_encode.h>
5#include <proto/power_frame_msg.nanopb.h>
6
7#include <cmath>
8#include <cstdint>
9#include <memory>
10#include <unordered_map>
11#include <vector>
12
13#ifdef PLATFORMIO_BUILD
14#include <proto/power_frame_msg.nanopb.h>
15#else // PLATFORMIO_BUILD
16#include "proto/power_frame_msg.pb.h"
17#include "proto/primitive.pb.h"
18#include "proto/primitive/primitive_types.h"
19#include "shared/constants.h"
20
21extern "C"
22{
23#include "proto/power_frame_msg.nanopb.h"
24}
25#endif // PLATFORMIO_BUILD
26
27// The nanopb generated size isn't c++ compatible so we redefine it here
28// TODO(#2592): Remove with upgrade to nanopb
29#undef TbotsProto_PowerFrame_size
30#define TbotsProto_PowerFrame_size \
31 std::max(TbotsProto_PowerPulseControl_size, TbotsProto_PowerStatus_size) + \
32 2 * sizeof(uint32_t) + sizeof(uint16_t)
33
34
41template <typename T>
42std::vector<uint8_t> serializeToVector(const T& data)
43{
44 const pb_field_t* fields;
45 int size;
46 if (std::is_same<T, TbotsProto_PowerFrame>::value)
47 {
48 fields = TbotsProto_PowerFrame_fields;
49 size = TbotsProto_PowerFrame_size;
50 }
51 else if (std::is_same<T, TbotsProto_PowerPulseControl>::value)
52 {
53 fields = TbotsProto_PowerPulseControl_fields;
54 size = TbotsProto_PowerPulseControl_size;
55 }
56 else if (std::is_same<T, TbotsProto_PowerStatus>::value)
57 {
58 fields = TbotsProto_PowerStatus_fields;
59 size = TbotsProto_PowerStatus_size;
60 }
61 else
62 {
63 throw std::runtime_error("Unexpected type as argument");
64 }
65
66 std::vector<uint8_t> buffer(size);
67 pb_ostream_t stream =
68 pb_ostream_from_buffer(static_cast<uint8_t*>(buffer.data()), buffer.size());
69 if (!pb_encode(&stream, fields, &data))
70 {
71 throw std::runtime_error(
72 "Failed to encode PowerFrame msg to vector when converting nanopb to vector");
73 }
74 return buffer;
75}
76
83void inline setPowerMsg(TbotsProto_PowerFrame& frame,
84 const TbotsProto_PowerPulseControl& control)
85{
86 frame.which_power_msg = TbotsProto_PowerFrame_power_control_tag;
87 frame.power_msg.power_control = control;
88}
89
90void inline setPowerMsg(TbotsProto_PowerFrame& frame,
91 const TbotsProto_PowerStatus& status)
92{
93 frame.which_power_msg = TbotsProto_PowerFrame_power_status_tag;
94 frame.power_msg.power_status = status;
95}
96
102TbotsProto_PowerStatus inline createNanoPbPowerStatus(
103 float battery_voltage, float capacitor_voltage, float current_draw,
104 TbotsProto_Geneva_Slot geneva_slot, uint32_t sequence_num, bool breakbeam_tripped)
105{
106 TbotsProto_PowerStatus status = {.battery_voltage = battery_voltage,
107 .capacitor_voltage = capacitor_voltage,
108 .current_draw = current_draw,
109 .geneva_slot = geneva_slot,
110 .sequence_num = sequence_num,
111 .breakbeam_tripped = breakbeam_tripped};
112 return status;
113}
114
123inline uint32_t calculateChickerPulseWidth(int kick_constant, double kick_coefficient,
124 float speed)
125{
126 return static_cast<uint32_t>(kick_constant * std::exp(kick_coefficient * speed));
127}
128
129// These functions are only used in power service so they are excluded from platformio
130// Protobuf needs additional support to run on an esp32
131#ifndef PLATFORMIO_BUILD
132
143TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl(
144 const TbotsProto::PowerControl& power_control, double kick_coeff, int kick_constant,
145 int chip_pulse_width)
146{
147 TbotsProto_PowerPulseControl nanopb_control =
148 TbotsProto_PowerPulseControl_init_default;
149
150 // Safety bounds
151 kick_constant = std::min(kick_constant, MAX_KICK_CONSTANT);
152 kick_coeff = std::min(kick_coeff, MAX_KICK_COEFFICIENT);
153
154 switch (power_control.chicker().chicker_command_case())
155 {
156 case TbotsProto::PowerControl::ChickerControl::kKickSpeedMPerS:
157 nanopb_control.chicker.which_chicker_command =
158 TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag;
159 // TODO (#3193): refactor kick pulse width calculation out into seperate
160 // function
161 nanopb_control.chicker.chicker_command.kick_pulse_width =
162 calculateChickerPulseWidth(kick_constant, kick_coeff,
163 power_control.chicker().kick_speed_m_per_s());
164 break;
165 case TbotsProto::PowerControl::ChickerControl::kChipDistanceMeters:
166 nanopb_control.chicker.which_chicker_command =
167 TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag;
168 nanopb_control.chicker.chicker_command.chip_pulse_width = chip_pulse_width;
169 break;
170 case TbotsProto::PowerControl::ChickerControl::kAutoChipOrKick:
171 nanopb_control.chicker.which_chicker_command =
172 TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag;
173 switch (power_control.chicker().auto_chip_or_kick().auto_chip_or_kick_case())
174 {
175 case TbotsProto::AutoChipOrKick::kAutokickSpeedMPerS:
176 nanopb_control.chicker.chicker_command.auto_chip_or_kick
177 .which_auto_chip_or_kick =
178 TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag;
179 nanopb_control.chicker.chicker_command.auto_chip_or_kick
180 .auto_chip_or_kick.autokick_pulse_width =
181 calculateChickerPulseWidth(kick_constant, kick_coeff,
182 power_control.chicker()
183 .auto_chip_or_kick()
184 .autokick_speed_m_per_s());
185 break;
186 case TbotsProto::AutoChipOrKick::kAutochipDistanceMeters:
187 nanopb_control.chicker.chicker_command.auto_chip_or_kick
188 .which_auto_chip_or_kick =
189 TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag;
190 nanopb_control.chicker.chicker_command.auto_chip_or_kick
191 .auto_chip_or_kick.autochip_pulse_width = chip_pulse_width;
192 break;
193
194 default:
195 break;
196 }
197 break;
198 default:
199 break;
200 }
201 switch (power_control.geneva_slot())
202 {
203 case TbotsProto::Geneva::LEFT:
204 nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_LEFT;
205 break;
206 case TbotsProto::Geneva::CENTRE_LEFT:
207 nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_CENTRE_LEFT;
208 break;
209 case TbotsProto::Geneva::CENTRE:
210 nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_CENTRE;
211 break;
212 case TbotsProto::Geneva::CENTRE_RIGHT:
213 nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_CENTRE_RIGHT;
214 break;
215 case TbotsProto::Geneva::RIGHT:
216 nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_RIGHT;
217 break;
218 default:
219 break;
220 }
221 return nanopb_control;
222}
229TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl(
230 ChickerCommandMode chicker_command, uint32_t kick_pulse_width,
231 uint32_t chip_pulse_width, AutoChipOrKickMode auto_chip_or_kick,
232 uint32_t autochip_pulse_width, uint32_t autokick_pulse_width,
233 TbotsProto_Geneva_Slot geneva_slot)
234{
235 TbotsProto_PowerPulseControl control = TbotsProto_PowerPulseControl_init_default;
236 TbotsProto_PowerPulseControl_ChickerControl chicker =
237 TbotsProto_PowerPulseControl_ChickerControl_init_default;
238 switch (chicker_command)
239 {
240 case ChickerCommandMode::CHIP:
241 control.chicker.which_chicker_command =
242 TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag;
243 chicker.chicker_command.chip_pulse_width = chip_pulse_width;
244 break;
245 case ChickerCommandMode::KICK:
246 control.chicker.which_chicker_command =
247 TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag;
248 chicker.chicker_command.kick_pulse_width = kick_pulse_width;
249 break;
250 case ChickerCommandMode::AUTOCHIPORKICK:
251 control.chicker.which_chicker_command =
252 TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag;
253 switch (auto_chip_or_kick)
254 {
255 case AutoChipOrKickMode::AUTOCHIP:
256 chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick =
257 TbotsProto_PowerPulseControl_AutoChipOrKick_autochip_pulse_width_tag;
258 chicker.chicker_command.auto_chip_or_kick.auto_chip_or_kick
259 .autochip_pulse_width = autochip_pulse_width;
260 break;
261 case AutoChipOrKickMode::AUTOKICK:
262 chicker.chicker_command.auto_chip_or_kick.which_auto_chip_or_kick =
263 TbotsProto_PowerPulseControl_AutoChipOrKick_autokick_pulse_width_tag;
264 chicker.chicker_command.auto_chip_or_kick.auto_chip_or_kick
265 .autokick_pulse_width = autokick_pulse_width;
266 break;
267 default:
268 break;
269 }
270 break;
271 default:
272 break;
273 }
274
275 control.chicker = chicker;
276 control.geneva_slot = geneva_slot;
277
278 return control;
279}
286std::unique_ptr<TbotsProto::PowerStatus> inline createTbotsPowerStatus(
287 const TbotsProto_PowerStatus& status)
288{
289 auto buffer = serializeToVector(status);
290 auto proto_status = std::make_unique<TbotsProto::PowerStatus>();
291 proto_status->ParseFromString(std::string(buffer.begin(), buffer.end()));
292 return proto_status;
293}
294#endif