added support for different drivetrain gear ratios
We went down to 2 CIMs on the practice bot and reduce the pinion size,
so the code has to know how to deal with that.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 8f9c165..8fbb875 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -70,6 +70,9 @@
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
+const int kCompDrivetrainGearboxPinion = 19;
+const int kPracticeDrivetrainGearboxPinion = 17;
+
struct Values {
// Wrist hall effect positive and negative edges.
double wrist_hall_effect_start_angle;
@@ -107,6 +110,8 @@
// Deadband voltage.
double angle_adjust_deadband;
+ int drivetrain_gearbox_pinion;
+
// what camera_center returns
int camera_center;
};
@@ -139,6 +144,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kCompAngleAdjustDeadband,
+ kCompDrivetrainGearboxPinion,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -159,6 +165,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kPracticeAngleAdjustDeadband,
+ kPracticeDrivetrainGearboxPinion,
kPracticeCameraCenter};
break;
default:
@@ -289,6 +296,13 @@
return true;
}
+bool drivetrain_gearbox_pinion(int *pinion) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *pinion = values->drivetrain_gearbox_pinion;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 6a14e87..a1fc749 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -43,6 +43,10 @@
// Returns the deadband voltage to use.
bool angle_adjust_deadband(double *voltage);
+// Sets *pinion to the number of teeth on the pinion that drives the drivetrain
+// wheels.
+bool drivetrain_gearbox_pinion(int *pinion);
+
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
bool camera_center(int *center);
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index de60cb1..817efa9 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -15,6 +15,8 @@
#include "frc971/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/constants.h"
+#include "aos/common/messages/RobotState.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -31,8 +33,17 @@
namespace {
inline double drivetrain_translate(int32_t in) {
+ int pinion_size;
+ if (::aos::robot_state.get() == NULL) {
+ if (!::aos::robot_state.FetchNext()) {
+ LOG(WARNING, "couldn't fetch robot state\n");
+ return 0;
+ }
+ }
+ if (!constants::drivetrain_gearbox_pinion(&pinion_size)) return 0;
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
+ (pinion_size / 50.0) /*output reduction*/ *
+ (64.0 / 24.0) /*encoder gears*/ *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 5e08a20..1758e95 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -61,6 +61,8 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
],
},
{