add support for offsetting potentiometer values to zero
Change-Id: Ic526dd27c4608814e12e7bd0fd33801b69779ca3
diff --git a/frc971/constants.cc b/frc971/constants.cc
index b0cbc6b..b64f52b 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -116,6 +116,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
+ 0.0,
},
{
@@ -139,6 +140,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// End "sensor" values.
@@ -180,7 +182,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
-
+ 0.0,
},
{
@@ -203,6 +205,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// End "sensor" values.
@@ -244,6 +247,7 @@
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
{kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
+ 0.0,
},
{
@@ -266,6 +270,7 @@
// Arm zeroing constants: left, right.
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
+ 0.0, 0.0, 0.0, 0.0,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index 883e1b1..5012817 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -11,6 +11,10 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
+// Everything is in SI units (volts, radians, meters, seconds, etc).
+// Some of these values are related to the conversion between raw values
+// (encoder counts, voltage, etc) to scaled units (radians, meters, etc).
+
// This structure contains current values for all of the things that change.
struct Values {
// Drivetrain Values /////
@@ -53,13 +57,14 @@
struct ZeroingConstants {
// The number of samples in the moving average filter.
int average_filter_size;
- // The difference in SI units between two index pulses.
+ // The difference in scaled units between two index pulses.
double index_difference;
- // The absolute position in SI units of one of the index pulses.
+ // The absolute position in scaled units of one of the index pulses.
double measured_index_position;
};
// Defines a range of motion for a subsystem.
+ // These are all absolute positions in scaled units.
struct Range {
double lower_hard_limit;
double upper_hard_limit;
@@ -70,6 +75,10 @@
struct Claw {
Range wrist;
ZeroingConstants zeroing;
+ // The value to add to potentiometer readings after they have been converted
+ // to radians so that the resulting value is 0 when the claw is at absolute
+ // 0 (horizontal straight out the front).
+ double potentiometer_offset;
};
Claw claw;
@@ -81,6 +90,13 @@
ZeroingConstants right_elev_zeroing;
ZeroingConstants left_arm_zeroing;
ZeroingConstants right_arm_zeroing;
+
+ // Values to add to scaled potentiometer readings so 0 lines up with the
+ // physical absolute 0.
+ double left_elevator_potentiometer_offset;
+ double right_elevator_potentiometer_offset;
+ double left_arm_potentiometer_offset;
+ double right_arm_potentiometer_offset;
};
Fridge fridge;
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index b6a08de..1941144 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -68,7 +68,7 @@
(2 * M_PI /*radians*/);
}
-double arm_pot_translate(double voltage) {
+double arm_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().arm_pot_ratio *
(5.0 /*volts*/ / 5.0 /*turns*/) *
@@ -83,7 +83,7 @@
constants::GetValues().elev_distance_per_radian;
}
-double elevator_pot_translate(double voltage) {
+double elevator_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().elev_pot_ratio *
(2 * M_PI /*radians*/) *
@@ -98,7 +98,7 @@
(2 * M_PI /*radians*/);
}
-double claw_pot_translate(double voltage) {
+double claw_potentiometer_translate(double voltage) {
return voltage /
constants::GetValues().claw_pot_ratio *
(5.0 /*volts*/ / 5.0 /*turns*/) *
@@ -254,25 +254,33 @@
dma_synchronizer_->RunIteration();
+ const auto values = constants::GetValues();
+
{
auto fridge_message = fridge_queue.position.MakeMessage();
CopyPotAndIndexPosition(arm_left_encoder_, &fridge_message->arm.left,
- arm_translate, arm_pot_translate, false);
- CopyPotAndIndexPosition(arm_right_encoder_, &fridge_message->arm.right,
- arm_translate, arm_pot_translate, true);
+ arm_translate, arm_potentiometer_translate, false,
+ values.fridge.left_elevator_potentiometer_offset);
+ CopyPotAndIndexPosition(
+ arm_right_encoder_, &fridge_message->arm.right, arm_translate,
+ arm_potentiometer_translate, true,
+ values.fridge.right_elevator_potentiometer_offset);
CopyPotAndIndexPosition(
elevator_left_encoder_, &fridge_message->elevator.left,
- elevator_translate, elevator_pot_translate, false);
- CopyPotAndIndexPosition(elevator_right_encoder_,
- &fridge_message->elevator.right,
- elevator_translate, elevator_pot_translate, true);
+ elevator_translate, elevator_potentiometer_translate, false,
+ values.fridge.left_arm_potentiometer_offset);
+ CopyPotAndIndexPosition(
+ elevator_right_encoder_, &fridge_message->elevator.right,
+ elevator_translate, elevator_potentiometer_translate, true,
+ values.fridge.right_arm_potentiometer_offset);
fridge_message.Send();
}
{
auto claw_message = claw_queue.position.MakeMessage();
CopyPotAndIndexPosition(wrist_encoder_, &claw_message->joint,
- claw_translate, claw_pot_translate, false);
+ claw_translate, claw_potentiometer_translate,
+ false, values.claw.potentiometer_offset);
claw_message.Send();
}
}
@@ -289,16 +297,20 @@
void CopyPotAndIndexPosition(
const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> pot_translate, bool reverse) {
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double potentiometer_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.polled_encoder_value());
- position->pot =
- multiplier * pot_translate(encoder.polled_potentiometer_voltage());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.polled_potentiometer_voltage()) +
+ potentiometer_offset;
position->latched_encoder =
multiplier * encoder_translate(encoder.last_encoder_value());
position->latched_pot =
- multiplier * pot_translate(encoder.last_potentiometer_voltage());
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ potentiometer_offset;
position->index_pulses = encoder.index_posedge_count();
}
@@ -306,20 +318,23 @@
const InterruptEncoderAndPotentiometer &encoder,
PotAndIndexPosition *position,
::std::function<double(int32_t)> encoder_translate,
- ::std::function<double(double)> pot_translate, bool reverse) {
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double potentiometer_offset) {
const double multiplier = reverse ? -1.0 : 1.0;
position->encoder =
multiplier * encoder_translate(encoder.encoder()->GetRaw());
- position->pot =
- multiplier * pot_translate(encoder.potentiometer()->GetVoltage());
+ position->pot = multiplier * potentiometer_translate(
+ encoder.potentiometer()->GetVoltage()) +
+ potentiometer_offset;
position->latched_encoder =
multiplier * encoder_translate(encoder.last_encoder_value());
position->latched_pot =
- multiplier * pot_translate(encoder.last_potentiometer_voltage());
+ multiplier *
+ potentiometer_translate(encoder.last_potentiometer_voltage()) +
+ potentiometer_offset;
position->index_pulses = encoder.index_posedge_count();
}
-
::std::unique_ptr<DMASynchronizer> dma_synchronizer_;
DMAEncoderAndPotentiometer arm_left_encoder_, arm_right_encoder_,