Remove goal_velocity from Goal.
Also change all doubles in Goal to floats to shrink message size.
Change-Id: I8db2df850b79efe923071e2d27d2ae9e04f4157e
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 1ba8bf8..67ee9f3 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -64,8 +64,6 @@
new_drivetrain_goal->controller_type = is_control_loop_driving ? 1 : 0;
new_drivetrain_goal->left_goal = current_left_goal;
new_drivetrain_goal->right_goal = current_right_goal;
- new_drivetrain_goal->left_velocity_goal = 0;
- new_drivetrain_goal->right_velocity_goal = 0;
new_drivetrain_goal->linear.max_velocity = 3.0;
new_drivetrain_goal->linear.max_acceleration = 20.0;
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 04ee334..ac1b3b7 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -34,9 +34,7 @@
.wheel(0.0)
.throttle(0.0)
.left_goal(initial_drivetrain_.left)
- .left_velocity_goal(0)
.right_goal(initial_drivetrain_.right)
- .right_velocity_goal(0)
.max_ss_voltage(max_drivetrain_voltage_)
.Send();
}
@@ -63,9 +61,7 @@
drivetrain_message->wheel = 0.0;
drivetrain_message->throttle = 0.0;
drivetrain_message->left_goal = initial_drivetrain_.left;
- drivetrain_message->left_velocity_goal = 0;
drivetrain_message->right_goal = initial_drivetrain_.right;
- drivetrain_message->right_velocity_goal = 0;
drivetrain_message->max_ss_voltage = max_drivetrain_voltage_;
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 2e78e51..c66b03e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -40,14 +40,14 @@
message Goal {
// Position of the steering wheel (positive = turning left when going
// forwards).
- double wheel;
- double wheel_velocity;
- double wheel_torque;
+ float wheel;
+ float wheel_velocity;
+ float wheel_torque;
// Position of the throttle (positive forwards).
- double throttle;
- double throttle_velocity;
- double throttle_torque;
+ float throttle;
+ float throttle_velocity;
+ float throttle_torque;
// True to shift into high, false to shift into low.
bool highgear;
@@ -66,12 +66,7 @@
double left_goal;
double right_goal;
- // Velocity goal for each drivetrain side (in m/s) when the closed-loop
- // controller is active.
- double left_velocity_goal;
- double right_velocity_goal;
-
- double max_ss_voltage;
+ float max_ss_voltage;
// Motion profile parameters.
// The control loop will profile if these are all non-zero.
@@ -127,10 +122,6 @@
double uncapped_left_voltage;
double uncapped_right_voltage;
- // The goal velocities for the polydrive controller.
- double left_velocity_goal;
- double right_velocity_goal;
-
// The voltage error for the left and right sides.
double left_voltage_error;
double right_voltage_error;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 5c1a8b4..17da3ab 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -392,8 +392,6 @@
goal->controller_type = 1;
goal->left_goal = 4.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 3.0;
goal->angular.max_velocity = 1.0;
@@ -423,8 +421,6 @@
goal->controller_type = 1;
goal->left_goal = -1.0;
goal->right_goal = 1.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 3.0;
goal->angular.max_velocity = 1.0;
@@ -454,8 +450,6 @@
goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 6.0;
goal->linear.max_acceleration = 4.0;
goal->angular.max_velocity = 2.0;
@@ -510,8 +504,6 @@
goal->controller_type = 1;
goal->left_goal = 5.0;
goal->right_goal = 4.0;
- goal->left_velocity_goal = 0.0;
- goal->right_velocity_goal = 0.0;
goal->linear.max_velocity = 1.0;
goal->linear.max_acceleration = 2.0;
goal->angular.max_velocity = 1.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
index aed0773..4101993 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.cc
@@ -37,8 +37,6 @@
control_loop_driving = false;
left_goal = 0.0f;
right_goal = 0.0f;
- left_velocity_goal = 0.0f;
- right_velocity_goal = 0.0f;
max_ss_voltage = 0.0f;
//linear.max_velocity = 0.0f;
//linear.max_acceleration = 0.0f;
@@ -80,8 +78,6 @@
estimated_right_velocity = 0.0f;
uncapped_left_voltage = 0.0f;
uncapped_right_voltage = 0.0f;
- left_velocity_goal = 0.0f;
- right_velocity_goal = 0.0f;
left_voltage_error = 0.0f;
right_voltage_error = 0.0f;
profiled_left_position_goal = 0.0f;
diff --git a/frc971/control_loops/drivetrain/drivetrain_uc.q.h b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
index 7fcdea1..f72b4be 100644
--- a/frc971/control_loops/drivetrain/drivetrain_uc.q.h
+++ b/frc971/control_loops/drivetrain/drivetrain_uc.q.h
@@ -44,8 +44,6 @@
bool control_loop_driving;
float left_goal;
float right_goal;
- float left_velocity_goal;
- float right_velocity_goal;
float max_ss_voltage;
};
@@ -86,8 +84,6 @@
float estimated_right_velocity;
float uncapped_left_voltage;
float uncapped_right_voltage;
- float left_velocity_goal;
- float right_velocity_goal;
float left_voltage_error;
float right_voltage_error;
float profiled_left_position_goal;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index cc3468a..f8f1a39 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -427,8 +427,6 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::PopulateStatus(
::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_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 0d01622..292f291 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -146,8 +146,7 @@
void DrivetrainMotorsSS::SetGoal(
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- unprofiled_goal_ << goal.left_goal, goal.left_velocity_goal, goal.right_goal,
- goal.right_velocity_goal, 0.0, 0.0, 0.0;
+ unprofiled_goal_ << goal.left_goal, 0.0, goal.right_goal, 0.0, 0.0, 0.0, 0.0;
if (::std::abs(goal.max_ss_voltage) < 0.01) {
max_voltage_ = kMaxVoltage;
} else {
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 46b20f6..849f4a5 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -105,8 +105,6 @@
.throttle(throttle)
.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 9bf98a2..7275d0d 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -129,8 +129,6 @@
.highgear(true)
.left_goal(left_goal_state(0, 0) + params.left_initial_position)
.right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
.Send();
}
if (ShouldCancel()) return true;
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index 3ed2cd1..ef59ba4 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -55,9 +55,7 @@
.controller_type(1)
.highgear(true)
.left_goal(left_initial_position)
- .left_velocity_goal(0)
.right_goal(right_initial_position)
- .right_velocity_goal(0)
.quickturn(false)
.Send();
}
@@ -70,9 +68,7 @@
.wheel(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
- .left_velocity_goal(0)
.right_goal(right_initial_position)
- .right_velocity_goal(0)
.Send();
}
@@ -103,8 +99,6 @@
.highgear(true)
.left_goal(left_goal)
.right_goal(right_goal)
- .left_velocity_goal(0.0)
- .right_velocity_goal(0.0)
.Send();
left_initial_position = left_goal;
right_initial_position = right_goal;
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index cc29bcb..9c32aa4 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -233,8 +233,6 @@
.controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal)
.right_goal(right_goal)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
deleted file mode 100644
index 22c668e..0000000
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ /dev/null
@@ -1,166 +0,0 @@
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/util/phased_loop.h"
-#include "aos/logging/logging.h"
-#include "aos/util/trapezoid_profile.h"
-#include "aos/commonmath.h"
-
-#include "bot3/actions/drivetrain_action.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-
-namespace bot3 {
-namespace actions {
-
-namespace chrono = ::std::chrono;
-
-DrivetrainAction::DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s)
- : ::frc971::actions::ActionBase
- <::frc971::actions::DrivetrainActionQueueGroup>(s) {}
-
-void DrivetrainAction::RunAction() {
- static const auto K = control_loops::MakeDrivetrainLoop().K();
-
- const double yoffset = action_q_->goal->y_offset;
- const double turn_offset =
- action_q_->goal->theta_offset * control_loops::kBot3TurnWidth / 2.0;
- LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
-
- // Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(chrono::milliseconds(10));
- ::aos::util::TrapezoidProfile turn_profile(chrono::milliseconds(10));
- const double goal_velocity = 0.0;
- const double epsilon = 0.01;
- ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
-
- profile.set_maximum_acceleration(action_q_->goal->maximum_acceleration);
- profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
- turn_profile.set_maximum_acceleration(
- 10.0 * control_loops::kBot3TurnWidth / 2.0);
- turn_profile.set_maximum_velocity(3.0 * control_loops::kBot3TurnWidth / 2.0);
-
- while (true) {
- // wait until next 10ms tick
- ::aos::time::PhasedLoop10MS(5000);
-
- control_loops::drivetrain.status.FetchLatest();
- if (control_loops::drivetrain.status.get()) {
- const auto &status = *control_loops::drivetrain.status;
- if (::std::abs(status.uncapped_left_voltage -
- status.uncapped_right_voltage) >
- 24) {
- LOG(DEBUG, "spinning in place\n");
- // They're more than 24V apart, so stop moving forwards and let it deal
- // with spinning first.
- profile.SetGoal(
- (status.estimated_left_position + status.estimated_right_position) /
- 2.0);
- } else {
- static const double divisor = K(0, 0) + K(0, 2);
- double dx_left, dx_right;
- if (status.uncapped_left_voltage > 12.0) {
- dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
- double dx;
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(FATAL, "no drivetrain status!\n");
- }
-
- const auto drive_profile_goal_state =
- profile.Update(yoffset, goal_velocity);
- const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
- left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
- right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
-
- if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
- ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
- break;
- }
- if (ShouldCancel()) return;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + action_q_->goal->left_initial_position,
- right_goal_state(0, 0) + action_q_->goal->right_initial_position);
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(false)
- .left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
- .right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return;
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
- if (ShouldCancel()) return;
- }
- while (true) {
- if (ShouldCancel()) return;
- const double kPositionThreshold = 0.05;
-
- const double left_error = ::std::abs(
- control_loops::drivetrain.status->estimated_left_position -
- (left_goal_state(0, 0) + action_q_->goal->left_initial_position));
- const double right_error = ::std::abs(
- control_loops::drivetrain.status->estimated_right_position -
- (right_goal_state(0, 0) + action_q_->goal->right_initial_position));
- const double velocity_error =
- ::std::abs(control_loops::drivetrain.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- control_loops::drivetrain.status.FetchNextBlocking();
- }
- LOG(INFO, "Done moving\n");
-}
-
-::std::unique_ptr<::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction() {
- return ::std::unique_ptr<
- ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>(
- new ::frc971::TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>(
- &::frc971::actions::drivetrain_action));
-}
-
-} // namespace actions
-} // namespace bot3
diff --git a/y2014_bot3/actions/drivetrain_action.h b/y2014_bot3/actions/drivetrain_action.h
deleted file mode 100644
index fc4b837..0000000
--- a/y2014_bot3/actions/drivetrain_action.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
-#define BOT3_ACTIONS_DRIVETRAIN_ACTION_H_
-
-#include <memory>
-
-#include "frc971/actions/action.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/drivetrain_action.q.h"
-
-namespace bot3 {
-namespace actions {
-
-class DrivetrainAction : public
- ::frc971::actions::ActionBase<::frc971::actions::DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainAction(::frc971::actions::DrivetrainActionQueueGroup* s);
-
- virtual void RunAction();
-};
-
-// Makes a new DrivetrainAction action.
-::std::unique_ptr<::frc971::TypedAction
- < ::frc971::actions::DrivetrainActionQueueGroup>>
-MakeDrivetrainAction();
-
-} // namespace actions
-} // namespace bot3
-
-#endif
diff --git a/y2014_bot3/actions/drivetrain_action_main.cc b/y2014_bot3/actions/drivetrain_action_main.cc
deleted file mode 100644
index 84042c6..0000000
--- a/y2014_bot3/actions/drivetrain_action_main.cc
+++ /dev/null
@@ -1,17 +0,0 @@
-#include <stdio.h>
-
-#include "aos/logging/logging.h"
-#include "aos/init.h"
-#include "bot3/actions/drivetrain_action.h"
-#include "frc971/actions/drivetrain_action.q.h"
-
-int main(int /*argc*/, char * /*argv*/[]) {
- ::aos::Init();
-
- bot3::actions::DrivetrainAction drivetrain(&::frc971::actions::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
-
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 0716a97..b805747 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -67,8 +67,6 @@
.controller_type(1)
.left_goal(left_current + side_distance_change)
.right_goal(right_current - side_distance_change)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending drivetrain goal failed\n");
}
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 6887a53..19cd065 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -167,8 +167,6 @@
.controller_type(is_control_loop_driving ? 1 : 0)
.left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
.right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}