Modified gyro_board_reader and gyro_board_data.
Removed code for sensors that are nonexistent on the third robot.
Please note that gyro_board_reader still has to have the gear ratios
in the translate functions updated for this specific robot.
diff --git a/bot3/input/gyro_board_data.h b/bot3/input/gyro_board_data.h
index 10135e3..a3b60e5 100644
--- a/bot3/input/gyro_board_data.h
+++ b/bot3/input/gyro_board_data.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_INPUT_GYRO_BOARD_DATA_H_
-#define FRC971_INPUT_GYRO_BOARD_DATA_H_
+#ifndef BOT3_INPUT_GYRO_BOARD_DATA_H_
+#define BOT3_INPUT_GYRO_BOARD_DATA_H_
#include "aos/common/byteorder.h"
-namespace frc971 {
+namespace bot3 {
// The struct that the gyro board sends out with all of the data in it.
struct GyroBoardData {
@@ -13,14 +13,10 @@
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;
@@ -31,14 +27,8 @@
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;
};
@@ -70,6 +60,6 @@
}
} __attribute__((__packed__));
-} // namespace frc971
+} // namespace bot3
-#endif // FRC971_INPUT_GYRO_BOARD_DATA_H_
+#endif // BOT3_INPUT_GYRO_BOARD_DATA_H_
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
index 06e760c..62c3657 100644
--- a/bot3/input/gyro_board_reader.cc
+++ b/bot3/input/gyro_board_reader.cc
@@ -12,7 +12,7 @@
#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
#include "frc971/control_loops/index/index_motor.q.h"
#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/input/gyro_board_data.h"
+#include "bot3/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -21,13 +21,10 @@
#endif
using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
using ::frc971::sensors::gyro;
-namespace frc971 {
+namespace bot3 {
namespace {
inline double drivetrain_translate(int32_t in) {
@@ -36,28 +33,12 @@
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-inline double wrist_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
-}
-
-inline double angle_adjust_translate(int32_t in) {
- static const double kCableDiameter = 0.060;
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
- (2 * M_PI);
-}
-
+// TODO(daniel): This might have to change if I find out that the gear ratios are different.
inline double shooter_translate(int32_t in) {
return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
(15.0 / 34.0) /*gears*/ * (2 * M_PI);
}
-inline double index_translate(int32_t in) {
- return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
- (1.0) /*gears*/ * (2 * M_PI);
-}
-
} // namespace
class GyroBoardReader {
@@ -72,11 +53,7 @@
bottom_fall_delay_count_(0),
last_bottom_fall_delay_count_(0),
bottom_fall_count_(0),
- last_bottom_fall_count_(0),
- wrist_rise_count_(0),
- last_wrist_rise_count_(0),
- shooter_angle_rise_count_(0),
- last_shooter_angle_rise_count_(0) {
+ last_bottom_fall_count_(0) {
}
void Run() {
@@ -181,50 +158,15 @@
&last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
UpdateWrappingCounter(data->bottom_fall_count,
&last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
- &last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->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))
.Send();
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(data->angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
shooter.position.MakeWithBuilder()
.position(shooter_translate(data->shooter))
.Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(data->top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->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)
- .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))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
- .Send();
}
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
@@ -239,17 +181,13 @@
uint8_t last_bottom_fall_delay_count_;
int32_t bottom_fall_count_;
uint8_t last_bottom_fall_count_;
- int32_t wrist_rise_count_;
- uint8_t last_wrist_rise_count_;
- int32_t shooter_angle_rise_count_;
- uint8_t last_shooter_angle_rise_count_;
};
-} // namespace frc971
+} // namespace bot3
int main() {
::aos::Init();
- ::frc971::GyroBoardReader reader;
+ ::bot3::GyroBoardReader reader;
reader.Run();
::aos::Cleanup();
return 0;