Get gyro board situation fixed.
- Finally got the dip switch thing working. (Not tested.)
- Fixed issues with gyro_sensor_receiver.
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index add717f..927e004 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -1,3 +1,5 @@
+#include <inttypes.h>
+
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/wrapping_counter.h"
@@ -22,55 +24,61 @@
// encoder or not.
inline double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (44.0 / 32.0 /*encoder gears*/) * // the encoders are on the wheels.
+ (32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
(3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
}
-// TODO (danielp): This needs to be modified eventually.
-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);
+// Translates values from the ADC into voltage.
+inline double adc_translate(uint16_t in) {
+ static const double kVRefN = 0;
+ static const double kVRefP = 3.3;
+ static const int kMaximumValue = 0x3FF;
+ return kVRefN +
+ (static_cast<double>(in) / static_cast<double>(kMaximumValue) *
+ (kVRefP - kVRefN));
+}
+
+inline double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+inline double battery_translate(uint16_t in) {
+ return adc_translate(in) * 80.8 / 17.8;
}
} // namespace
class GyroSensorReceiver : public USBReceiver {
- virtual void ProcessData() override {
- if (data()->robot_id != 0) {
- LOG(ERROR, "gyro board sent data for robot id %hhd!"
- " dip switches are %x\n",
- data()->robot_id, data()->base_status & 0xF);
- return;
- } else {
- LOG(DEBUG, "processing a packet dip switches %x\n",
- data()->base_status & 0xF);
- }
+ public:
+ GyroSensorReceiver() : USBReceiver(1) {}
+ virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
gyro.MakeWithBuilder()
- .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .angle(gyro_translate(data()->gyro_angle))
.Send();
-
- LOG(INFO, "Got encoder data: %lf, %lf\n",
- drivetrain_translate(data()->main.right_drive),
- drivetrain_translate(data()->main.left_drive));
+ LOG(DEBUG, "right drive: %f, left drive: %f\n",
+ drivetrain_translate(data()->main.shooter_angle),
+ drivetrain_translate(data()->main.indexer));
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data()->main.right_drive))
.left_encoder(-drivetrain_translate(data()->main.left_drive))
.Send();
+
+ LOG(DEBUG, "battery %f %f %" PRIu16 "\n",
+ battery_translate(data()->main.battery_voltage),
+ adc_translate(data()->main.battery_voltage),
+ data()->main.battery_voltage);
+ LOG(DEBUG, "halls l=%f r=%f\n",
+ adc_translate(data()->main.left_drive_hall),
+ adc_translate(data()->main.right_drive_hall));
}
-
- WrappingCounter top_rise_;
- WrappingCounter top_fall_;
- WrappingCounter bottom_rise_;
- WrappingCounter bottom_fall_delay_;
- WrappingCounter bottom_fall_;
};
} // namespace bot3
int main() {
- ::aos::Init();
+ ::aos::Init(frc971::USBReceiver::kRelativePriority);
::bot3::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();