Calibrated superstructure sensors.
Change-Id: I423537e51e13b5836d31cfe6f859c9d98afe2d91
diff --git a/y2016/BUILD b/y2016/BUILD
index 65502fd..7dc41a2 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -48,8 +48,9 @@
':joystick_reader',
'//aos:prime_start_binaries',
'//y2016/control_loops/drivetrain:drivetrain',
+ '//y2016/control_loops/superstructure:superstructure',
+ '//y2016/control_loops/shooter:shooter',
'//y2016/autonomous:auto',
- '//y2016/actors:binaries',
'//y2016/wpilib:wpilib_interface',
],
srcs = [
diff --git a/y2016/constants.cc b/y2016/constants.cc
index 0785646..a5d87ca 100644
--- a/y2016/constants.cc
+++ b/y2016/constants.cc
@@ -79,23 +79,27 @@
// Intake
{
- 0.0,
+ // Value to add to the pot reading for the intake.
+ -4.550531 + 150.40906362 * M_PI / 180.0,
{Values::kZeroingSampleSize, Values::kIntakeEncoderIndexDifference,
- 0.9, 0.3},
+ // Location of an index pulse.
+ -0.087413 + 150.40906362 * M_PI / 180.0, 0.3},
},
// Shoulder
{
- 0.0,
+ // Value to add to the pot reading for the shoulder.
+ -1.0 - 0.0822 + 0.06138835 * M_PI / 180.0,
{Values::kZeroingSampleSize, Values::kShoulderEncoderIndexDifference,
- 0.9, 0.3},
+ 0.06138835 * M_PI / 180.0 + 0.353794, 0.3},
},
// Wrist
{
- 0.0,
+ // Value to add to the pot reading for the wrist.
+ 3.2390714288298668 + -0.06138835 * M_PI / 180.0,
{Values::kZeroingSampleSize, Values::kWristEncoderIndexDifference,
- 0.9, 0.3},
+ -0.06138835 * M_PI / 180.0 - 0.260542, 0.3},
},
};
break;
diff --git a/y2016/constants.h b/y2016/constants.h
index 6e7c1a9..615b607 100644
--- a/y2016/constants.h
+++ b/y2016/constants.h
@@ -28,7 +28,7 @@
// The ratio from the encoder shaft to the drivetrain wheels.
static constexpr double kDrivetrainEncoderRatio =
- (18.0 / 36.0) /*output reduction*/ * (30.0 / 44.0) /*encoder gears*/;
+ (18.0 / 36.0) /*output reduction*/ * (44.0 / 30.0) /*encoder gears*/;
// Ratios for our subsystems.
static constexpr double kShooterEncoderRatio = 1.0;
@@ -39,7 +39,7 @@
static constexpr double kWristEncoderRatio =
16.0 / 48.0 * 18.0 / 64.0 * 14.0 / 54.0;
- static constexpr double kIntakePotRatio = 16.0 / 48.0;
+ static constexpr double kIntakePotRatio = 16.0 / 48.0 * 18.0 / 72.0;
static constexpr double kShoulderPotRatio = 16.0 / 58.0;
static constexpr double kWristPotRatio = 16.0 / 48.0;
diff --git a/y2016/control_loops/python/drivetrain.py b/y2016/control_loops/python/drivetrain.py
index bb86625..c5782c9 100755
--- a/y2016/control_loops/python/drivetrain.py
+++ b/y2016/control_loops/python/drivetrain.py
@@ -79,7 +79,7 @@
# Radius of the robot, in meters (requires tuning by hand)
self.rb = 0.601 / 2.0
# Radius of the wheels, in meters.
- self.r = 0.097155
+ self.r = 0.097155 * 0.9811158901447808
# Resistance of the motor, divided by the number of motors.
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 9eb328d..4eb2e0d 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -83,13 +83,13 @@
double drivetrain_translate(int32_t in) {
return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
constants::Values::kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
+ control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
}
double drivetrain_velocity_translate(double in) {
return (1.0 / in) / 256.0 /*cpr*/ *
constants::Values::kDrivetrainEncoderRatio *
- control_loops::drivetrain::kWheelRadius * 2.0 / 2.0;
+ control_loops::drivetrain::kWheelRadius * 2.0 * M_PI;
}
double shooter_translate(int32_t in) {
@@ -98,7 +98,7 @@
}
double intake_translate(int32_t in) {
- return -static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
+ return static_cast<double>(in) / (512.0 /*cpr*/ * 4.0 /*4x*/) *
constants::Values::kIntakeEncoderRatio * (2 * M_PI /*radians*/);
}
@@ -286,7 +286,7 @@
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
- drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
-drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
@@ -307,7 +307,7 @@
{
auto shooter_message = shooter_queue.position.MakeMessage();
shooter_message->theta_left =
- shooter_translate(shooter_left_encoder_->GetRaw());
+ shooter_translate(-shooter_left_encoder_->GetRaw());
shooter_message->theta_right =
shooter_translate(shooter_right_encoder_->GetRaw());
shooter_message.Send();
@@ -323,7 +323,7 @@
shoulder_translate, shoulder_pot_translate, false,
values.shoulder.pot_offset);
CopyPotAndIndexPosition(wrist_encoder_, &superstructure_message->wrist,
- wrist_translate, wrist_pot_translate, false,
+ wrist_translate, wrist_pot_translate, true,
values.wrist.pot_offset);
superstructure_message.Send();
@@ -589,25 +589,25 @@
SensorReader reader;
// TODO(constants): Update these input numbers.
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_drivetrain_left_hall(make_unique<AnalogInput>(1));
- reader.set_drivetrain_right_hall(make_unique<AnalogInput>(2));
+ reader.set_drivetrain_left_encoder(make_encoder(5));
+ reader.set_drivetrain_right_encoder(make_encoder(6));
+ reader.set_drivetrain_left_hall(make_unique<AnalogInput>(5));
+ reader.set_drivetrain_right_hall(make_unique<AnalogInput>(6));
- reader.set_shooter_left_encoder(make_encoder(0));
- reader.set_shooter_right_encoder(make_encoder(0));
+ reader.set_shooter_left_encoder(make_encoder(3));
+ reader.set_shooter_right_encoder(make_encoder(4));
reader.set_intake_encoder(make_encoder(0));
reader.set_intake_index(make_unique<DigitalInput>(0));
reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
- reader.set_shoulder_encoder(make_encoder(0));
- reader.set_shoulder_index(make_unique<DigitalInput>(0));
- reader.set_shoulder_potentiometer(make_unique<AnalogInput>(0));
+ reader.set_shoulder_encoder(make_encoder(2));
+ reader.set_shoulder_index(make_unique<DigitalInput>(2));
+ reader.set_shoulder_potentiometer(make_unique<AnalogInput>(2));
- reader.set_wrist_encoder(make_encoder(0));
- reader.set_wrist_index(make_unique<DigitalInput>(0));
- reader.set_wrist_potentiometer(make_unique<AnalogInput>(0));
+ reader.set_wrist_encoder(make_encoder(1));
+ reader.set_wrist_index(make_unique<DigitalInput>(1));
+ reader.set_wrist_potentiometer(make_unique<AnalogInput>(1));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));