Drive code works on Tantrum.
Need to write the spring code. Drive now supports doubles... What a
pain.
Change-Id: Id589acdc443dcd81242a21e3b0c26f81d6974dc8
diff --git a/motors/BUILD b/motors/BUILD
index 3b6433d..292a40f 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -174,6 +174,7 @@
"//motors/peripheral:can",
"//motors/usb",
"//motors/usb:cdc",
+ "//motors/seems_reasonable:drivetrain_lib",
],
)
diff --git a/motors/seems_reasonable/BUILD b/motors/seems_reasonable/BUILD
new file mode 100644
index 0000000..47ca065
--- /dev/null
+++ b/motors/seems_reasonable/BUILD
@@ -0,0 +1,76 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:drivetrain",
+ ],
+)
+
+py_binary(
+ name = "polydrivetrain",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_drivetrain",
+ outs = [
+ "drivetrain_dog_motor_plant.h",
+ "drivetrain_dog_motor_plant.cc",
+ "kalman_drivetrain_motor_plant.h",
+ "kalman_drivetrain_motor_plant.cc",
+ ],
+ cmd = "$(location :drivetrain) $(OUTS)",
+ restricted_to = mcu_cpus,
+ tools = [
+ ":drivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_polydrivetrain",
+ outs = [
+ "polydrivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.cc",
+ "polydrivetrain_cim_plant.h",
+ "polydrivetrain_cim_plant.cc",
+ ],
+ cmd = "$(location :polydrivetrain) $(OUTS)",
+ restricted_to = mcu_cpus,
+ tools = [
+ ":polydrivetrain",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_lib",
+ srcs = [
+ "drivetrain_dog_motor_plant.cc",
+ "polydrivetrain_dog_motor_plant.cc",
+ ],
+ hdrs = [
+ "drivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop_uc",
+ "//frc971/control_loops/drivetrain:polydrivetrain_uc",
+ ],
+)
diff --git a/motors/seems_reasonable/drivetrain.py b/motors/seems_reasonable/drivetrain.py
new file mode 100644
index 0000000..dfa2f16
--- /dev/null
+++ b/motors/seems_reasonable/drivetrain.py
@@ -0,0 +1,38 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import drivetrain
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+kDrivetrain = drivetrain.DrivetrainParams(
+ J=8.0,
+ mass=100.0,
+ robot_radius=0.616 / 2.0,
+ wheel_radius=0.127 / 2.0 * 120.0 / 118.0,
+ G_low=46.0 / 60.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ G_high=62.0 / 44.0 * 20.0 / 48.0 * 14.0 / 62.0,
+ controller_poles=[0.82, 0.82],
+)
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ if len(argv) != 5:
+ print "Expected .h file name and .cc file name"
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(
+ argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ kDrivetrain,
+ scalar_type='float')
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/motors/seems_reasonable/polydrivetrain.py b/motors/seems_reasonable/polydrivetrain.py
new file mode 100644
index 0000000..4d3720e
--- /dev/null
+++ b/motors/seems_reasonable/polydrivetrain.py
@@ -0,0 +1,35 @@
+#!/usr/bin/python
+
+import sys
+from motors.seems_reasonable import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+def main(argv):
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(
+ argv[1:3],
+ argv[3:5], ['motors', 'seems_reasonable'],
+ drivetrain.kDrivetrain,
+ scalar_type='float')
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 0467599..c6d70b8 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -13,25 +13,76 @@
#include "motors/usb/cdc.h"
#include "motors/usb/usb.h"
#include "motors/util.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "motors/seems_reasonable/drivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h"
namespace frc971 {
namespace motors {
namespace {
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
+using ::frc971::constants::ShifterHallEffect;
+using ::frc971::control_loops::DrivetrainQueue_Goal;
+using ::frc971::control_loops::DrivetrainQueue_Output;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<float> &GetDrivetrainConfig() {
+ static DrivetrainConfig<float> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+
+ ::motors::seems_reasonable::MakeDrivetrainLoop,
+ ::motors::seems_reasonable::MakeVelocityDrivetrainLoop,
+ ::std::function<StateFeedbackLoop<7, 2, 4, float>()>(),
+
+ ::motors::seems_reasonable::kDt, ::motors::seems_reasonable::kRobotRadius,
+ ::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV,
+
+ ::motors::seems_reasonable::kHighGearRatio,
+ ::motors::seems_reasonable::kLowGearRatio, kThreeStateDriveShifter,
+ kThreeStateDriveShifter, true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */, 0.8 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */, 1.5 /* wheel_multiplier */,
+ };
+
+ return kDrivetrainConfig;
+};
+
+
::std::atomic<teensy::AcmTty *> global_stdout{nullptr};
+::std::atomic<PolyDrivetrain<float> *> global_polydrivetrain{nullptr};
+
// Last width we received on each channel.
uint16_t pwm_input_widths[5];
// When we received a pulse on each channel in milliseconds.
uint32_t pwm_input_times[5];
+constexpr int kChannelTimeout = 100;
+
+bool lost_channel(int channel) {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return true;
+ }
+ return false;
+}
+
// Returns the most recently captured value for the specified input channel
// scaled from -1 to 1, or 0 if it was captured over 100ms ago.
float convert_input_width(int channel) {
uint16_t width;
{
DisableInterrupts disable_interrupts;
- if (time_after(millis(), time_add(pwm_input_times[channel], 100))) {
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
return 0;
}
@@ -73,7 +124,9 @@
// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
// bandwidth.
void vesc_set_current(int vesc_id, float current) {
- const int32_t current_int = current * 1000.0f;
+ constexpr float kMaxCurrent = 80.0f;
+ const int32_t current_int =
+ ::std::max(-kMaxCurrent, ::std::min(kMaxCurrent, current)) * 1000.0f;
uint32_t id = CAN_EFF_FLAG;
id |= vesc_id;
id |= (0x01 /* SET_CURRENT */) << 8;
@@ -91,7 +144,9 @@
// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
// bandwidth.
void vesc_set_duty(int vesc_id, float duty) {
- const int32_t duty_int = duty * 100000.0f;
+ constexpr int32_t kMaxDuty = 99999;
+ const int32_t duty_int = ::std::max(
+ -kMaxDuty, ::std::min(kMaxDuty, static_cast<int32_t>(duty * 100000.0f)));
uint32_t id = CAN_EFF_FLAG;
id |= vesc_id;
id |= (0x00 /* SET_DUTY */) << 8;
@@ -316,7 +371,7 @@
const float sin = ConvertEncoderChannel(adc_readings.sin);
const float cos = ConvertEncoderChannel(adc_readings.cos);
- const float magnitude = ::std::sqrt(sin * sin + cos * cos);
+ const float magnitude = hypot(sin, cos);
const float magnitude_error = ::std::abs(magnitude - 1.0f);
valid = magnitude_error < 0.15f;
@@ -331,6 +386,8 @@
extern "C" void pit3_isr() {
PIT_TFLG3 = 1;
+ PolyDrivetrain<float> *polydrivetrain =
+ global_polydrivetrain.load(::std::memory_order_acquire);
salsa::SimpleAdcReadings adc_readings;
{
@@ -340,20 +397,69 @@
EncoderReading encoder(adc_readings);
- printf("TODO(Austin): 1kHz loop %d %d %d %d %d ADC %" PRIu16 " %" PRIu16
- " enc %d/1000 %s from %d\n",
- (int)(convert_input_width(0) * 1000),
- (int)(convert_input_width(1) * 1000),
- (int)(convert_input_width(2) * 1000),
- (int)(convert_input_width(3) * 1000),
- (int)(convert_input_width(4) * 1000), adc_readings.sin,
- adc_readings.cos, (int)(encoder.angle * 1000),
- encoder.valid ? "T" : "f",
- (int)(::std::sqrt(ConvertEncoderChannel(adc_readings.sin) *
- ConvertEncoderChannel(adc_readings.sin) +
- ConvertEncoderChannel(adc_readings.cos) *
- ConvertEncoderChannel(adc_readings.cos)) *
- 1000));
+ const bool lost_any_channel = lost_channel(0) || lost_channel(1) ||
+ lost_channel(2) || lost_channel(3) ||
+ lost_channel(4) ||
+ (::std::abs(convert_input_width(4)) < 0.05f);
+
+ if (polydrivetrain != nullptr) {
+ DrivetrainQueue_Goal goal;
+ goal.control_loop_driving = false;
+ if (lost_any_channel) {
+ goal.throttle = 0.0f;
+ goal.wheel = 0.0f;
+ } else {
+ goal.throttle = convert_input_width(1);
+ goal.wheel = convert_input_width(0);
+ }
+ goal.throttle = convert_input_width(1);
+ goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+
+ DrivetrainQueue_Output output;
+
+ polydrivetrain->SetGoal(goal);
+ polydrivetrain->Update();
+ polydrivetrain->SetOutput(&output);
+
+ vesc_set_duty(0, -output.left_voltage / 12.0f);
+ vesc_set_duty(1, -output.left_voltage / 12.0f);
+
+ vesc_set_duty(2, output.right_voltage / 12.0f);
+ vesc_set_duty(3, output.right_voltage / 12.0f);
+
+ static int i = 0;
+ ++i;
+ if (i > 20) {
+ i = 0;
+ if (lost_any_channel) {
+ printf("200Hz loop, disabled %d %d %d %d %d\n",
+ (int)(convert_input_width(0) * 1000),
+ (int)(convert_input_width(1) * 1000),
+ (int)(convert_input_width(2) * 1000),
+ (int)(convert_input_width(3) * 1000),
+ (int)(convert_input_width(4) * 1000));
+ } else {
+ printf(
+ "TODO(Austin): 200Hz loop %d %d %d %d %d, lr, %d, %d velocity %d "
+ "ADC %" PRIu16 " %" PRIu16 " enc %d/1000 %s from %d\n",
+ (int)(convert_input_width(0) * 1000),
+ (int)(convert_input_width(1) * 1000),
+ (int)(convert_input_width(2) * 1000),
+ (int)(convert_input_width(3) * 1000),
+ (int)(convert_input_width(4) * 1000),
+ static_cast<int>(output.left_voltage * 100),
+ static_cast<int>(output.right_voltage * 100),
+ static_cast<int>(polydrivetrain->velocity() * 100),
+ adc_readings.sin, adc_readings.cos, (int)(encoder.angle * 1000),
+ encoder.valid ? "T" : "f",
+ (int)(::std::sqrt(ConvertEncoderChannel(adc_readings.sin) *
+ ConvertEncoderChannel(adc_readings.sin) +
+ ConvertEncoderChannel(adc_readings.cos) *
+ ConvertEncoderChannel(adc_readings.cos)) *
+ 1000));
+ }
+ }
+ }
}
} // namespace
@@ -434,7 +540,7 @@
// Workaround for errata e7914.
(void)PIT_MCR;
PIT_MCR = 0;
- PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 1000) - 1;
+ PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 200) - 1;
PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
can_init(0, 1);
@@ -442,6 +548,9 @@
SetupPwmFtm(FTM0);
SetupPwmFtm(FTM3);
+ PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr);
+ global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release);
+
// Leave the LEDs on for a bit longer.
delay(300);
printf("Done starting up\n");
@@ -453,7 +562,8 @@
NVIC_ENABLE_IRQ(IRQ_FTM3);
NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
- DoReceiverTest2();
+ while (true) {
+ }
return 0;
}