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/actions/actions.gyp b/frc971/actions/actions.gyp
index 931d847..a0dca81 100644
--- a/frc971/actions/actions.gyp
+++ b/frc971/actions/actions.gyp
@@ -43,6 +43,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
],
},
{
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index 71698a4..548c5f3 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -5,13 +5,22 @@
#include "frc971/control_loops/shooter/shooter.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
namespace frc971 {
namespace actions {
+
ShootAction::ShootAction(actions::ShootActionQueueGroup* s)
: actions::ActionBase<actions::ShootActionQueueGroup>(s) {}
+double ShootAction::SpeedToAngleOffset(double speed) {
+ const frc971::constants::Values& values = frc971::constants::GetValues();
+ // scale speed to a [0.0-1.0] on something close to the max
+ return (speed/values.drivetrain_max_speed) * ShootAction::kOffsetRadians;
+}
+
+
void ShootAction::RunAction() {
if (!control_loops::shooter_queue_group.goal.MakeWithBuilder().shot_power(
shoot_action.goal->shot_power)
@@ -21,8 +30,10 @@
return;
}
+ control_loops::drivetrain.status.FetchLatest();
if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
- shoot_action.goal->shot_angle)
+ shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
.separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
LOG(WARNING, "sending claw goal failed\n");
return;
@@ -41,6 +52,22 @@
break;
}
+ // update the claw position to track velocity
+ // TODO(ben): the claw may never reach the goal if the velocity is
+ // continually changing, we will need testing to see
+ control_loops::drivetrain.status.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder().bottom_angle(
+ shoot_action.goal->shot_angle +
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed))
+ .separation_angle(0.0).intake(2.0).centering(1.0).Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return;
+ } else {
+ LOG(INFO, "Updating claw angle for velocity offset(%.4f).\n",
+ SpeedToAngleOffset(control_loops::drivetrain.status->robot_speed));
+ }
+
+
// Wait until we have a new status.
control_loops::shooter_queue_group.status.FetchNextBlocking();
control_loops::claw_queue_group.status.FetchNextBlocking();
diff --git a/frc971/actions/shoot_action.h b/frc971/actions/shoot_action.h
index b3a20d8..d080760 100644
--- a/frc971/actions/shoot_action.h
+++ b/frc971/actions/shoot_action.h
@@ -11,15 +11,20 @@
namespace frc971 {
namespace actions {
-class ShootAction : public ActionBase <actions::ShootActionQueueGroup> {
+class ShootAction : public ActionBase<actions::ShootActionQueueGroup> {
public:
- explicit ShootAction (actions::ShootActionQueueGroup * s);
+ explicit ShootAction(actions::ShootActionQueueGroup* s);
// Actually execute the action of moving the claw and shooter into position
// and actually firing them.
void RunAction();
+ // calc an offset to our requested shot based on robot speed
+ double SpeedToAngleOffset(double speed);
+
+ static constexpr double kOffsetRadians = 0.2;
+
protected:
};
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 9a6828f..48519ff 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -71,7 +71,9 @@
0.9, // start_fine_tune_pos
4.0,
},
- {0.01, 0.1} // shooter_action
+ {0.01, 0.1}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kCompTeamNumber:
@@ -105,7 +107,9 @@
4.0,
},
//TODO(james): Get realer numbers for shooter_action.
- {0.01, 0.1} // shooter_action
+ {0.01, 0.1}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
case kPracticeTeamNumber:
@@ -119,7 +123,6 @@
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
// ShooterLimits
- // TODO(ben): make these real numbers
{-0.001042, 0.294084, -0.001935, 0.303460, 0.0138401,
{-0.002, 0.000446, -0.002, 0.000446},
{-0.002, 0.009078, -0.002, 0.009078},
@@ -141,7 +144,9 @@
4.000000,
},
//TODO(james): Get realer numbers for shooter_action.
- {0.01, 0.1} // shooter_action
+ {0.01, 0.1}, // shooter_action
+ 0.02, // drivetrain done delta
+ 5.0 // drivetrain max speed
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 7d06a70..775ee10 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -112,6 +112,8 @@
double claw_separation_goal;
};
ShooterAction shooter_action;
+ double drivetrain_done_distance;
+ double drivetrain_max_speed;
};
// Creates (once) a Values instance and returns a reference to it.
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