Austin update the closed dt loop to keep valid estimates of velocity. we then use these in shoot action to set an offset in claw angle for shooting at velocity.
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 93e23cc..284f722 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -111,8 +111,34 @@
SetRawPosition(left, right);
}
+ void SetExternalMotors(double left_voltage, double right_voltage) {
+ loop_->U << left_voltage, right_voltage;
+ }
+
void Update(bool stop_motors) {
- loop_->Update(stop_motors);
+ if (_control_loop_driving) {
+ loop_->Update(stop_motors);
+ } else {
+ if (stop_motors) {
+ loop_->U.setZero();
+ loop_->U_uncapped.setZero();
+ }
+ loop_->UpdateObserver();
+ }
+ }
+
+ double GetEstimatedRobotSpeed() {
+ // lets just call the average of left and right velocities close enough
+ return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
+ }
+
+ double GetEstimatedLeftEncoder() {
+ // lets just call the average of left and right velocities close enough
+ return loop_->X_hat(0, 0);
+ }
+
+ double GetEstimatedRightEncoder() {
+ return loop_->X_hat(2, 0);
}
void SendMotors(Drivetrain::Output *output) {
@@ -581,7 +607,7 @@
void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
const Drivetrain::Position *position,
Drivetrain::Output *output,
- Drivetrain::Status * /*status*/) {
+ Drivetrain::Status * status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
@@ -615,14 +641,34 @@
}
}
dt_openloop.SetPosition(position);
- dt_closedloop.Update(output == NULL);
dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
+
if (control_loop_driving) {
+ dt_closedloop.Update(output == NULL);
dt_closedloop.SendMotors(output);
} else {
dt_openloop.SendMotors(output);
+ dt_closedloop.SetExternalMotors(output->left_voltage,
+ output->right_voltage);
+ dt_closedloop.Update(output == NULL);
}
+
+ // set the output status of the controll loop state
+ if (status) {
+ bool done = false;
+ if (goal) {
+ done = ((::std::abs(goal->left_goal -
+ dt_closedloop.GetEstimatedLeftEncoder()) <
+ constants::GetValues().drivetrain_done_distance) &&
+ (::std::abs(goal->right_goal -
+ dt_closedloop.GetEstimatedRightEncoder()) <
+ constants::GetValues().drivetrain_done_distance));
+ }
+ status->is_done = done;
+ status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
+ }
+
}
} // namespace control_loops
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 443282c..50d9dbf 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -45,12 +45,13 @@
message Output {
float left_voltage;
float right_voltage;
- bool left_high;
- bool right_high;
+ bool left_high;
+ bool right_high;
};
message Status {
bool is_done;
+ double robot_speed;
};
queue Goal goal;
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 001fd1e..8b1d9ad 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -89,6 +89,8 @@
self.Gr = self.G_high
# Control loop time step
self.dt = 0.01
+
+ print "THE NUMBER I WANT" + str((self.free_speed / 60.0 * 2.0 * numpy.pi) * self.G_high * self.r)
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
@@ -118,6 +120,7 @@
self.D = numpy.matrix([[0, 0],
[0, 0]])
+ #print "THE NUMBER I WANT" + str(numpy.linalg.inv(self.A_continuous) * -self.B_continuous * numpy.matrix([[12.0], [12.0]]))
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index ed6bc22..f289e81 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -323,13 +323,17 @@
//::std::cout << "Predict xhat before " << X_hat;
//::std::cout << "Measurement error is " << Y_ - C() * X_hat;
//X_hat = A() * X_hat + B() * U;
+ //::std::cout << "Predict xhat after " << X_hat;
+ UpdateObserver();
+ }
+
+ void UpdateObserver() {
if (new_y_) {
X_hat = (A() - L() * C()) * X_hat + L() * Y_ + B() * U;
new_y_ = false;
} else {
X_hat = A() * X_hat + B() * U;
}
- //::std::cout << "Predict xhat after " << X_hat;
}
// Sets the current controller to be index and verifies that it isn't out of