sanified how shifters work and drivetrain logging in general
diff --git a/aos/build/queues/objects/q_file.rb b/aos/build/queues/objects/q_file.rb
index beaf8f0..f73b638 100644
--- a/aos/build/queues/objects/q_file.rb
+++ b/aos/build/queues/objects/q_file.rb
@@ -138,7 +138,7 @@
suite << StructStmt.parse(tokens)
else
tokens.qError(<<ERROR_MSG)
-expected a "package","import","queue","queue_group", or "message" statement rather
+expected a "package","import","queue","struct","queue_group", or "message" statement rather
than a #{token.data.inspect}, (whatever that is?)
oh! no! a feature request!?
Wot. Wot.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a074c22..e7aae01 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -10,11 +10,12 @@
#include "aos/common/queue.h"
#include "aos/controls/polytope.h"
#include "aos/common/commonmath.h"
+#include "aos/common/logging/queue_logging.h"
+
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/gyro_angle.q.h"
-#include "frc971/queues/piston.q.h"
#include "frc971/constants.h"
using frc971::sensors::gyro;
@@ -121,7 +122,6 @@
}
}
void PrintMotors() const {
- // LOG(DEBUG, "Left Power %f Right Power %f lg %f rg %f le %f re %f gyro %f\n", U[0], U[1], R[0], R[2], Y[0], Y[1], _gyro);
::Eigen::Matrix<double, 4, 1> E = loop_->R - loop_->X_hat;
LOG(DEBUG, "E[0, 0]: %f E[1, 0] %f E[2, 0] %f E[3, 0] %f\n", E(0, 0), E(1, 0), E(2, 0), E(3, 0));
}
@@ -308,53 +308,31 @@
}
if (position) {
+ GearLogging gear_logging;
// Switch to the correct controller.
// TODO(austin): Un-hard code 0.57
if (position->left_shifter_position < 0.57) {
if (position->right_shifter_position < 0.57 || right_gear_ == LOW) {
- LOG(DEBUG, "Loop Left low, Right low\n");
- loop_->set_controller_index(0);
+ gear_logging.left_loop_high = false;
+ gear_logging.right_loop_high = false;
+ loop_->set_controller_index(gear_logging.controller_index = 0);
} else {
- LOG(DEBUG, "Loop Left low, Right high\n");
- loop_->set_controller_index(1);
+ gear_logging.left_loop_high = false;
+ gear_logging.right_loop_high = true;
+ loop_->set_controller_index(gear_logging.controller_index = 1);
}
} else {
if (position->right_shifter_position < 0.57 || left_gear_ == LOW) {
- LOG(DEBUG, "Loop Left high, Right low\n");
- loop_->set_controller_index(2);
+ gear_logging.left_loop_high = true;
+ gear_logging.right_loop_high = false;
+ loop_->set_controller_index(gear_logging.controller_index = 2);
} else {
- LOG(DEBUG, "Loop Left high, Right high\n");
- loop_->set_controller_index(3);
+ gear_logging.left_loop_high = true;
+ gear_logging.right_loop_high = true;
+ loop_->set_controller_index(gear_logging.controller_index = 3);
}
}
- switch (left_gear_) {
- case LOW:
- LOG(DEBUG, "Left is in low\n");
- break;
- case HIGH:
- LOG(DEBUG, "Left is in high\n");
- break;
- case SHIFTING_UP:
- LOG(DEBUG, "Left is shifting up\n");
- break;
- case SHIFTING_DOWN:
- LOG(DEBUG, "Left is shifting down\n");
- break;
- }
- switch (right_gear_) {
- case LOW:
- LOG(DEBUG, "Right is in low\n");
- break;
- case HIGH:
- LOG(DEBUG, "Right is in high\n");
- break;
- case SHIFTING_UP:
- LOG(DEBUG, "Right is shifting up\n");
- break;
- case SHIFTING_DOWN:
- LOG(DEBUG, "Right is shifting down\n");
- break;
- }
+
// TODO(austin): Constants.
if (position->left_shifter_position > 0.9 && left_gear_ == SHIFTING_UP) {
left_gear_ = HIGH;
@@ -368,6 +346,10 @@
if (position->right_shifter_position < 0.1 && right_gear_ == SHIFTING_DOWN) {
right_gear_ = LOW;
}
+
+ gear_logging.left_state = left_gear_;
+ gear_logging.right_state = right_gear_;
+ LOG_STRUCT(DEBUG, "state", gear_logging);
}
}
@@ -433,19 +415,30 @@
const double right_motor_speed =
MotorSpeed(position_.right_shifter_position, current_right_velocity);
- // Reset the CIM model to the current conditions to be ready for when we shift.
- if (IsInGear(left_gear_)) {
- left_cim_->X_hat(0, 0) = left_motor_speed;
- LOG(DEBUG, "Setting left CIM to %f at robot speed %f\n", left_motor_speed,
- current_left_velocity);
+ {
+ CIMLogging logging;
+
+ // Reset the CIM model to the current conditions to be ready for when we
+ // shift.
+ if (IsInGear(left_gear_)) {
+ logging.left_in_gear = true;
+ left_cim_->X_hat(0, 0) = left_motor_speed;
+ } else {
+ logging.left_in_gear = false;
+ }
+ logging.left_motor_speed = left_motor_speed;
+ logging.left_velocity = current_left_velocity;
+ if (IsInGear(right_gear_)) {
+ logging.right_in_gear = true;
+ right_cim_->X_hat(0, 0) = right_motor_speed;
+ } else {
+ logging.right_in_gear = false;
+ }
+ logging.right_motor_speed = right_motor_speed;
+ logging.right_velocity = current_right_velocity;
+
+ LOG_STRUCT(DEBUG, "currently", logging);
}
- if (IsInGear(right_gear_)) {
- right_cim_->X_hat(0, 0) = right_motor_speed;
- LOG(DEBUG, "Setting right CIM to %f at robot speed %f\n",
- right_motor_speed, current_right_velocity);
- }
- LOG(DEBUG, "robot speed l=%f r=%f\n", current_left_velocity,
- current_right_velocity);
if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
// FF * X = U (steady state)
@@ -505,7 +498,8 @@
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
R_left(0, 0) = left_motor_speed;
- const double wiggle = (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
+ const double wiggle =
+ (static_cast<double>((counter_ % 4) / 2) - 0.5) * 3.5;
loop_->U(0, 0) =
::aos::Clip((R_left / Kv)(0, 0) + wiggle, -position_.battery_voltage,
@@ -528,14 +522,8 @@
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- }
- // Go in high gear if anything wants to be in high gear.
- // TODO(austin): Seperate these.
- if (left_gear_ == HIGH || left_gear_ == SHIFTING_UP ||
- right_gear_ == HIGH || right_gear_ == SHIFTING_UP) {
- shifters.MakeWithBuilder().set(false).Send();
- } else {
- shifters.MakeWithBuilder().set(true).Send();
+ output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
+ output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
}
}
@@ -600,7 +588,7 @@
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
if (gyro.FetchLatest()) {
- LOG(DEBUG, "gyro %f\n", gyro->angle);
+ LOG_STRUCT(DEBUG, "using", *gyro);
dt_closedloop.SetPosition(left_encoder, right_encoder, gyro->angle,
control_loop_driving);
} else {
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index d7bce50..adb9f94 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -48,6 +48,7 @@
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 1dcc947..443282c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -2,6 +2,23 @@
import "aos/common/control_loop/control_loops.q";
+struct GearLogging {
+ int8_t controller_index;
+ bool left_loop_high;
+ bool right_loop_high;
+ int8_t left_state;
+ int8_t right_state;
+};
+
+struct CIMLogging {
+ bool left_in_gear;
+ bool right_in_gear;
+ double left_motor_speed;
+ double right_motor_speed;
+ double left_velocity;
+ double right_velocity;
+};
+
queue_group Drivetrain {
implements aos.control_loops.ControlLoop;
@@ -28,6 +45,8 @@
message Output {
float left_voltage;
float right_voltage;
+ bool left_high;
+ bool right_high;
};
message Status {
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 4372098..e078e53 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -9,11 +9,9 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/gyro_angle.q.h"
-#include "frc971/queues/piston.q.h"
#include "frc971/autonomous/auto.q.h"
using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
using ::aos::input::driver_station::ButtonLocation;
@@ -32,9 +30,7 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {
- shifters.MakeWithBuilder().set(true).Send();
- }
+ Reader() {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index 57040c1..0af5e2b 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -9,13 +9,11 @@
#include "aos/common/time.h"
#include "aos/common/logging/queue_logging.h"
-#include "frc971/queues/piston.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
using ::aos::util::SimpleLogInterval;
using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shifters;
namespace frc971 {
namespace output {
@@ -42,6 +40,8 @@
SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
SetPWMOutput(5, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
+ SetSolenoid(1, drivetrain.output->left_high);
+ SetSolenoid(2, drivetrain.output->right_high);
} else {
DisablePWMOutput(2);
DisablePWMOutput(3);
@@ -49,11 +49,6 @@
DisablePWMOutput(6);
LOG_INTERVAL(drivetrain_old_);
}
- shifters.FetchLatest();
- if (shifters.get()) {
- SetSolenoid(1, shifters->set);
- SetSolenoid(2, !shifters->set);
- }
}
}
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index da8f545..19fb6d1 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -37,6 +37,7 @@
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
],
},
],
diff --git a/frc971/queues/piston.q b/frc971/queues/piston.q
deleted file mode 100644
index 5811958..0000000
--- a/frc971/queues/piston.q
+++ /dev/null
@@ -1,8 +0,0 @@
-package frc971.control_loops;
-
-message Piston {
- bool set;
-};
-
-queue Piston shifters;
-queue Piston hangers;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 13393a6..a67ffe2 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -3,7 +3,6 @@
'queue_files': [
'gyro_angle.q',
'photo_sensor.q',
- 'piston.q',
'to_log.q',
]
},