Log gear/shifting info in drivetrain status instead of seperate structs
Change-Id: Ib25174051fab63ec7b453da02ddf5f0a12e0145d
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 7a44b64..d42e6c6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -117,15 +117,16 @@
}
kf_.set_index(ControllerIndexFromGears());
- {
- GearLogging gear_logging;
- gear_logging.left_state = static_cast<uint32_t>(left_gear_);
- gear_logging.right_state = static_cast<uint32_t>(right_gear_);
- gear_logging.left_loop_high = MaybeHigh(left_gear_);
- gear_logging.right_loop_high = MaybeHigh(right_gear_);
- gear_logging.controller_index = kf_.index();
- LOG_STRUCT(DEBUG, "state", gear_logging);
+
+ // Set the gear-logging parts of the status
+ if (status) {
+ status->gear_logging.left_state = static_cast<uint32_t>(left_gear_);
+ status->gear_logging.right_state = static_cast<uint32_t>(right_gear_);
+ status->gear_logging.left_loop_high = MaybeHigh(left_gear_);
+ status->gear_logging.right_loop_high = MaybeHigh(right_gear_);
+ status->gear_logging.controller_index = kf_.index();
}
+
const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
if (is_latest_imu_values) {
const double rate = -::frc971::imu_values->gyro_y;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index da4fadc..76f5ec6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -144,6 +144,10 @@
// The angle of the robot relative to the ground.
double ground_angle;
+
+ // Information about shifting logic and curent gear, for logging purposes
+ GearLogging gear_logging;
+ CIMLogging cim_logging;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 91c2fc9..f61083b 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -269,42 +269,33 @@
loop_->mutable_X_hat() =
loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
}
+
+ // Housekeeping: set the shifting logging values to zero, because we're not shifting
+ left_motor_speed_ = 0.0;
+ right_motor_speed_ = 0.0;
+ current_left_velocity_ = 0.0;
+ current_right_velocity_ = 0.0;
} else {
- const double current_left_velocity =
+ current_left_velocity_ =
(position_.left_encoder - last_position_.left_encoder) / dt_config_.dt;
- const double current_right_velocity =
+ current_right_velocity_ =
(position_.right_encoder - last_position_.right_encoder) /
dt_config_.dt;
- const double left_motor_speed =
+ left_motor_speed_ =
MotorSpeed(dt_config_.left_drive, position_.left_shifter_position,
- current_left_velocity, left_gear_);
- const double right_motor_speed =
+ current_left_velocity_, left_gear_);
+ right_motor_speed_ =
MotorSpeed(dt_config_.right_drive, position_.right_shifter_position,
- current_right_velocity, right_gear_);
+ current_right_velocity_, right_gear_);
- {
- CIMLogging logging;
-
- // Reset the CIM model to the current conditions to be ready for when we
- // shift.
- logging.left_in_gear = IsInGear(left_gear_);
- logging.left_motor_speed = left_motor_speed;
- logging.left_velocity = current_left_velocity;
-
- logging.right_in_gear = IsInGear(right_gear_);
- logging.right_motor_speed = right_motor_speed;
- logging.right_velocity = current_right_velocity;
-
- LOG_STRUCT(DEBUG, "currently", logging);
- }
- goal_left_velocity_ = current_left_velocity;
- goal_right_velocity_ = current_right_velocity;
+ goal_left_velocity_ = current_left_velocity_;
+ goal_right_velocity_ = current_right_velocity_;
// Any motor is not in gear. Speed match.
::Eigen::Matrix<double, 1, 1> R_left;
::Eigen::Matrix<double, 1, 1> R_right;
- R_left(0, 0) = left_motor_speed;
- R_right(0, 0) = right_motor_speed;
+ R_left(0, 0) = left_motor_speed_;
+ R_right(0, 0) = right_motor_speed_;
const double wiggle =
(static_cast<double>((counter_ % 30) / 15) - 0.5) * 8.0;
@@ -333,6 +324,14 @@
::frc971::control_loops::DrivetrainQueue::Status *status) {
status->left_velocity_goal = goal_left_velocity_;
status->right_velocity_goal = goal_right_velocity_;
+
+ status->cim_logging.left_in_gear = IsInGear(left_gear_);
+ status->cim_logging.left_motor_speed = left_motor_speed_;
+ status->cim_logging.left_velocity = current_left_velocity_;
+
+ status->cim_logging.right_in_gear = IsInGear(right_gear_);
+ status->cim_logging.right_motor_speed = right_motor_speed_;
+ status->cim_logging.right_velocity = current_right_velocity_;
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 570b87f..070b7eb 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -66,6 +66,12 @@
double goal_left_velocity_ = 0.0;
double goal_right_velocity_ = 0.0;
+
+ // Stored from the last iteration, for logging shifting logic.
+ double left_motor_speed_ = 0.0;
+ double right_motor_speed_ = 0.0;
+ double current_left_velocity_ = 0.0;
+ double current_right_velocity_ = 0.0;
};
} // namespace drivetrain