hooked up the battery voltage and analog hall effect shifter stuff
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 1d3546d..c554173 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -11,6 +11,7 @@
#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/input/usb_receiver.h"
+#include "frc971/constants.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -27,50 +28,57 @@
namespace frc971 {
namespace {
-inline double drivetrain_translate(int32_t in) {
+double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
-inline double wrist_translate(int32_t in) {
+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) {
+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);
}
-inline double shooter_translate(int32_t in) {
+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) {
+double index_translate(int32_t in) {
return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
(1.0) /*gears*/ * (2 * M_PI);
}
// Translates values from the ADC into voltage.
-inline double adc_translate(uint16_t in) {
+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 const int kMaximumValue = 0xFFF;
+ static const double kDividerGnd = 31.6, kDividerOther = 28;
+ return (kVRefN +
(static_cast<double>(in) / static_cast<double>(kMaximumValue) *
- (kVRefP - kVRefN));
+ (kVRefP - kVRefN))) * (kDividerGnd + kDividerOther) / kDividerGnd;
}
-inline double gyro_translate(int64_t in) {
+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;
+double battery_translate(uint16_t in) {
+ static const double kDividerBig = 98.9, kDividerSmall = 17.8;
+ return adc_translate(in) * kDividerBig / kDividerSmall;
+}
+
+double hall_translate(const constants::ShifterHallEffect &k, uint16_t in) {
+ const double voltage = adc_translate(in);
+ return (voltage - k.low) / (k.high - k.low);
}
} // namespace
@@ -150,9 +158,16 @@
battery_translate(data()->main.battery_voltage),
adc_translate(data()->main.battery_voltage),
data()->main.battery_voltage);
- LOG(DEBUG, "halls l=%f r=%f\n",
+ LOG(DEBUG, "halls l=%f r=%f %" PRIx16 " %" PRIx16 "\n",
adc_translate(data()->main.left_drive_hall),
- adc_translate(data()->main.right_drive_hall));
+ adc_translate(data()->main.right_drive_hall),
+ data()->main.left_drive_hall,
+ data()->main.right_drive_hall);
+ LOG(DEBUG, "real l=%f r=%f\n",
+ hall_translate(constants::GetValues().left_drive,
+ data()->main.left_drive_hall),
+ hall_translate(constants::GetValues().right_drive,
+ data()->main.right_drive_hall));
}
bool bad_gyro_;