split analog.c out intelligently and added an id to the sensor packet
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index 10135e3..6f07a2a 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -5,70 +5,9 @@
namespace frc971 {
-// The struct that the gyro board sends out with all of the data in it.
-struct GyroBoardData {
- int64_t gyro_angle;
-
- int32_t left_drive;
- int32_t right_drive;
- int32_t shooter_angle;
- int32_t shooter;
- int32_t indexer;
- int32_t wrist;
-
- int32_t capture_top_rise;
- int32_t capture_top_fall;
- int32_t capture_bottom_fall_delay;
- int32_t capture_wrist_rise;
- int32_t capture_shooter_angle_rise;
-
- uint8_t top_rise_count;
-
- uint8_t top_fall_count;
-
- uint8_t bottom_rise_count;
-
- uint8_t bottom_fall_delay_count;
- uint8_t bottom_fall_count;
-
- uint8_t wrist_rise_count;
-
- uint8_t shooter_angle_rise_count;
-
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- };
- uint32_t digitals;
- };
-
- void NetworkToHost() {
- // Apparently it sends the information out in little endian.
-#if 0
- using ::aos::ntoh;
-
- gyro_angle = ntoh(gyro_angle);
-
- right_drive = ntoh(right_drive);
- left_drive = ntoh(left_drive);
- shooter_angle = ntoh(shooter_angle);
- shooter = ntoh(shooter);
- indexer = ntoh(indexer);
- wrist = ntoh(wrist);
-
- capture_top_rise = ntoh(capture_top_rise);
- capture_top_fall = ntoh(capture_top_fall);
- capture_bottom_fall_delay = ntoh(capture_bottom_fall_delay);
- capture_wrist_rise = ntoh(capture_wrist_rise);
- capture_shooter_angle_rise = ntoh(capture_shooter_angle_rise);
-
- digitals = ntoh(digitals);
-#endif
- }
-} __attribute__((__packed__));
+#define DATA_STRUCT_NAME GyroBoardData
+#include "../../gyro_board/src/usb/data_struct.h"
+#undef DATA_STRUCT_NAME
} // namespace frc971
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index 06e760c..e1e08d7 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -159,8 +159,14 @@
}
void ProcessData(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
+ if (data->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n", data->robot_id, data->dip_switches);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n", data->dip_switches);
+ }
+
static ::aos::time::Time last_time = ::aos::time::Time::Now();
if ((last_time - ::aos::time::Time::Now()) >
::aos::time::Time::InMS(0.00205)) {
@@ -171,58 +177,58 @@
.angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- UpdateWrappingCounter(data->top_rise_count,
+ UpdateWrappingCounter(data->main.top_rise_count,
&last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
+ UpdateWrappingCounter(data->main.top_fall_count,
&last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
+ UpdateWrappingCounter(data->main.bottom_rise_count,
&last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
+ UpdateWrappingCounter(data->main.bottom_fall_delay_count,
&last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
+ UpdateWrappingCounter(data->main.bottom_fall_count,
&last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
+ UpdateWrappingCounter(data->main.wrist_rise_count,
&last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
+ UpdateWrappingCounter(data->main.shooter_angle_rise_count,
&last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
.Send();
wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
+ .pos(wrist_translate(data->main.wrist))
+ .hall_effect(data->main.wrist_hall_effect)
+ .calibration(wrist_translate(data->main.capture_wrist_rise))
.Send();
angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(data->angle_adjust_bottom_hall_effect)
+ .angle(angle_adjust_translate(data->main.shooter_angle))
+ .bottom_hall_effect(data->main.angle_adjust_bottom_hall_effect)
.middle_hall_effect(false)
.bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
+ data->main.capture_shooter_angle_rise))
.middle_calibration(angle_adjust_translate(
0))
.Send();
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
+ .position(shooter_translate(data->main.shooter))
.Send();
index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(data->top_disc)
+ .index_position(index_translate(data->main.indexer))
+ .top_disc_detect(data->main.top_disc)
.top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_posedge_position(index_translate(data->main.capture_top_rise))
.top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(data->bottom_disc)
+ .top_disc_negedge_position(index_translate(data->main.capture_top_fall))
+ .bottom_disc_detect(data->main.bottom_disc)
.bottom_disc_posedge_count(bottom_rise_count_)
.bottom_disc_negedge_count(bottom_fall_count_)
.bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
+ data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
.Send();
}
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 4dec23e..1e5343e 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -82,8 +82,13 @@
}
void UnpackFrom(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
+ if (data->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n", data->robot_id, data->dip_switches);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n", data->dip_switches);
+ }
static ::aos::time::Time last_time = ::aos::time::Time::Now();
if ((last_time - ::aos::time::Time::Now()) >
@@ -95,58 +100,58 @@
.angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- UpdateWrappingCounter(data->top_rise_count,
+ UpdateWrappingCounter(data->main.top_rise_count,
&last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
+ UpdateWrappingCounter(data->main.top_fall_count,
&last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
+ UpdateWrappingCounter(data->main.bottom_rise_count,
&last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
+ UpdateWrappingCounter(data->main.bottom_fall_delay_count,
&last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
+ UpdateWrappingCounter(data->main.bottom_fall_count,
&last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
+ UpdateWrappingCounter(data->main.wrist_rise_count,
&last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
+ UpdateWrappingCounter(data->main.shooter_angle_rise_count,
&last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
.Send();
wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(!data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
+ .pos(wrist_translate(data->main.wrist))
+ .hall_effect(!data->main.wrist_hall_effect)
+ .calibration(wrist_translate(data->main.capture_wrist_rise))
.Send();
angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
+ .angle(angle_adjust_translate(data->main.shooter_angle))
+ .bottom_hall_effect(!data->main.angle_adjust_bottom_hall_effect)
.middle_hall_effect(false)
.bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
+ data->main.capture_shooter_angle_rise))
.middle_calibration(angle_adjust_translate(
0))
.Send();
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
+ .position(shooter_translate(data->main.shooter))
.Send();
index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(!data->top_disc)
+ .index_position(index_translate(data->main.indexer))
+ .top_disc_detect(!data->main.top_disc)
.top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_posedge_position(index_translate(data->main.capture_top_rise))
.top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(!data->bottom_disc)
+ .top_disc_negedge_position(index_translate(data->main.capture_top_fall))
+ .bottom_disc_detect(!data->main.bottom_disc)
.bottom_disc_posedge_count(bottom_rise_count_)
.bottom_disc_negedge_count(bottom_fall_count_)
.bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
+ data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
.Send();
}