Zeroed robot.
Change-Id: I3c01fefb55ef63849208c558a8d7a1be8af76666
diff --git a/frc971/constants.cc b/frc971/constants.cc
index b64f52b..8d4afed 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -241,36 +241,36 @@
// Claw values, in radians.
// 0 is level with the ground.
// Positive moves in the direction of positive encoder values.
- {0.0000000000, 1.5700000000,
- 0.1000000000, 1.2000000000},
+ {-0.05, M_PI / 2.0 + 0.1,
+ 0.0, M_PI / 2.0},
// Zeroing constants for wrist.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kClawEncoderIndexDifference, 0.0},
- 0.0,
+ {kZeroingSampleSize, kClawEncoderIndexDifference, 0.977913},
+ 6.1663463999999992,
},
{
// Elevator values, in meters.
// 0 is at the top of the elevator frame.
// Positive is down towards the drivebase.
- {0.0000000000, 0.6790000000,
- 0.2000000000, 0.6000000000},
+ {-0.00500, 0.679000,
+ 0.010000, 0.650000},
// Arm values, in radians.
// 0 is sticking straight out horizontally over the intake/front.
// Positive is rotating up and into the robot (towards the back).
- {-1.570000000, 1.5700000000,
- -1.200000000, 1.2000000000},
+ {-M_PI/2 - 0.05, M_PI/2 + 0.05,
+ -M_PI/2, M_PI/2},
// Elevator zeroing constants: left, right.
// TODO(sensors): Get actual offsets for these.
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.0},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.016041},
+ {kZeroingSampleSize, kElevatorEncoderIndexDifference, 0.011367},
// Arm zeroing constants: left, right.
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
- {kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
- 0.0, 0.0, 0.0, 0.0,
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.312677},
+ {kZeroingSampleSize, kArmEncoderIndexDifference, -0.396278},
+ 0.72069366666666679, -0.078959636363636357, -3.4952331578947375, 3.5263507647058816,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 0cebf53..12b34ae 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -62,7 +62,7 @@
}
double arm_translate(int32_t in) {
- return static_cast<double>(in) /
+ return -static_cast<double>(in) /
(512.0 /*cpr*/ * 4.0 /*4x*/) *
constants::GetValues().arm_encoder_ratio *
(2 * M_PI /*radians*/);
@@ -84,7 +84,7 @@
}
double elevator_potentiometer_translate(double voltage) {
- return voltage *
+ return -voltage *
constants::GetValues().elev_pot_ratio *
(2 * M_PI /*radians*/) *
constants::GetValues().elev_distance_per_radian *
@@ -99,7 +99,7 @@
}
double claw_potentiometer_translate(double voltage) {
- return voltage *
+ return -voltage *
constants::GetValues().claw_pot_ratio *
(5.0 /*turns*/ / 5.0 /*volts*/) *
(2 * M_PI /*radians*/);
@@ -267,19 +267,19 @@
auto fridge_message = fridge_queue.position.MakeMessage();
CopyPotAndIndexPosition(arm_left_encoder_, &fridge_message->arm.left,
arm_translate, arm_potentiometer_translate, false,
- values.fridge.left_elevator_potentiometer_offset);
+ values.fridge.left_arm_potentiometer_offset);
CopyPotAndIndexPosition(
arm_right_encoder_, &fridge_message->arm.right, arm_translate,
arm_potentiometer_translate, true,
- values.fridge.right_elevator_potentiometer_offset);
+ values.fridge.right_arm_potentiometer_offset);
CopyPotAndIndexPosition(
elevator_left_encoder_, &fridge_message->elevator.left,
elevator_translate, elevator_potentiometer_translate, false,
- values.fridge.left_arm_potentiometer_offset);
+ values.fridge.left_elevator_potentiometer_offset);
CopyPotAndIndexPosition(
elevator_right_encoder_, &fridge_message->elevator.right,
elevator_translate, elevator_potentiometer_translate, true,
- values.fridge.right_arm_potentiometer_offset);
+ values.fridge.right_elevator_potentiometer_offset);
fridge_message.Send();
}