got battery voltage measurement working + used
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 943e8c2..d60a0e9 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -93,7 +93,7 @@
uint16_t low_right_drive_hall;
uint16_t high_right_drive_hall;
- uint16_t battery_voltage;
+ uint16_t battery_voltage_high, battery_voltage_low;
HallEffectEdges pusher_distal, pusher_proximal;
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 70b19e3..cab3e28 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -122,6 +122,9 @@
packet->main.high_left_drive_hall = analog_get(6);
packet->main.high_right_drive_hall = analog_get(1);
+ packet->main.battery_voltage_high = analog_get(5);
+ packet->main.battery_voltage_low = analog_get(3);
+
packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
fill_top_claw_values(packet);
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 946d7e3..520255b 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -544,8 +544,13 @@
const double wiggle =
(static_cast<double>((counter_ % 10) / 5) - 0.5) * 5.0;
- loop_->U(0, 0) = ::aos::Clip((R_left / Kv)(0, 0) + wiggle, -12.0, 12.0);
- loop_->U(1, 0) = ::aos::Clip((R_right / Kv)(0, 0) + wiggle, -12.0, 12.0);
+ loop_->U(0, 0) = ::aos::Clip(
+ (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->U(1, 0) = ::aos::Clip(
+ (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
+ -12.0, 12.0);
+ loop_->U *= 12.0 / position_.battery_voltage;
}
}
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index b44bb8d..7648db8 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -48,21 +48,26 @@
}
// Translates values from the ADC into voltage.
-// TODO(brian): Fix this.
+// TODO(brian): Tune this to the actual hardware.
double adc_translate(uint16_t in) {
static const double kVcc = 5;
- //static const double kR1 = 5, kR2 = 6.65;
+ static const double kR1 = 5, kR2 = 6.65;
static const uint16_t kMaximumValue = 0x3FF;
- return (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ const double raw =
+ (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
+ return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
}
double gyro_translate(int64_t in) {
return in / 16.0 / 1000.0 / (180.0 / M_PI);
}
-double battery_translate(uint16_t in) {
- static const double kDividerBig = 98.9, kDividerSmall = 17.8;
- return adc_translate(in) * kDividerBig / kDividerSmall;
+double battery_translate(uint16_t in_high, uint16_t in_low) {
+ const double high = adc_translate(in_high), low = adc_translate(in_low);
+ static const double kDividerBig = 5.55, kDividerSmall = 2.66;
+ static const double kSensorVcc = 5.0;
+ return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
+ kDividerBig / kDividerSmall * kSensorVcc;
}
double sonar_translate(uint32_t in) {
@@ -179,7 +184,8 @@
.right_shifter_position(hall_translate(constants::GetValues().right_drive,
data->main.low_right_drive_hall,
data->main.high_right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage))
+ .battery_voltage(battery_translate(data->main.battery_voltage_high,
+ data->main.battery_voltage_low))
.Send();
{