modifications at Davis. SPLIT OUT LATER
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 82e8595..fb127b2 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -187,8 +187,8 @@
const double goal_velocity = 0.0;
const double epsilon = 0.01;
- profile.set_maximum_acceleration(2);
- profile.set_maximum_velocity(0.8);
+ profile.set_maximum_acceleration(1);
+ profile.set_maximum_velocity(0.6);
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
@@ -229,7 +229,7 @@
double WRIST_UP;
::aos::robot_state.FetchLatest();
- if (::aos::robot_state.get() &&
+ if (!::aos::robot_state.get() ||
!constants::wrist_hall_effect_start_angle(&WRIST_UP)) {
LOG(ERROR, "Constants not ready\n");
return;
@@ -237,7 +237,7 @@
WRIST_UP -= 0.4;
LOG(INFO, "Got constants\n");
const double WRIST_DOWN = -0.633;
- const double ANGLE_ONE = 0.50;
+ const double ANGLE_ONE = 0.5101;
const double ANGLE_TWO = 0.685;
StopDrivetrain();
@@ -245,8 +245,11 @@
SetWristGoal(WRIST_UP); // wrist must calibrate itself on power-up
SetAngle_AdjustGoal(ANGLE_ONE);
- SetShooterVelocity(405.0);
+ SetShooterVelocity(410.0);
WaitForIndexReset();
+
+ // Wait to get some more air pressure in the thing.
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(5.0));
PreloadIndex(); // spin to top and put 1 disc into loader
@@ -260,6 +263,8 @@
if (ShouldExitAuto()) return;
::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
+ return;
+
SetWristGoal(WRIST_DOWN);
SetAngle_AdjustGoal(ANGLE_TWO);
SetShooterVelocity(375.0);
@@ -272,8 +277,10 @@
if (ShouldExitAuto()) return;
WaitForIndex(); // ready to pick up discs
- SetDriveGoal(0.75);
- SetDriveGoal(2.75);
+ SetDriveGoal(3.5);
+ //SetDriveGoal(0.6);
+ //::aos::time::SleepFor(::aos::time::Time::InSeconds(0.25));
+ //SetDriveGoal(2.9);
if (ShouldExitAuto()) return;
PreloadIndex();
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 28878a2..966f1c8 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -17,7 +17,7 @@
namespace {
// It has about 0.029043 of gearbox slop.
-const double kCompWristHallEffectStartAngle = 1.260;
+const double kCompWristHallEffectStartAngle = 1.230;
const double kPracticeWristHallEffectStartAngle = 1.0872860614359172;
const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index bdcb8a3..df88a34 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -6,7 +6,6 @@
#include "aos/aos_core.h"
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "aos/common/inttypes.h"
@@ -542,7 +541,8 @@
}
if (position->bottom_disc_detect) {
- intake_voltage = transfer_voltage = 12.0;
+ intake_voltage = 0.0;
+ transfer_voltage = 12.0;
// Must wait until the disc gets out before we can change state.
safe_to_change_state = false;
@@ -585,7 +585,8 @@
for (auto frisbee = frisbees_.begin();
frisbee != frisbees_.end(); ++frisbee) {
if (!frisbee->has_been_indexed_) {
- intake_voltage = transfer_voltage = 12.0;
+ intake_voltage = 0.0;
+ transfer_voltage = 12.0;
// All discs must be indexed before it is safe to stop indexing.
safe_to_change_state = false;
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 392e630..97a0cef 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -9,6 +9,7 @@
#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
+#include "aos/atom_code/messages/DriverStationDisplay.h"
#include "frc971/constants.h"
#include "frc971/control_loops/wrist/wrist_motor_plant.h"
@@ -91,6 +92,9 @@
position->pos,
position->hall_effect ? "true" : "false",
zeroed_joint_.absolute_position());
+
+ ::aos::DriverStationDisplay::Send(3, "wrist: %6.4f ",
+ position->calibration);
}
if (output) {
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
index 6b50fad..bf3f906 100644
--- a/frc971/control_loops/wrist/wrist.gyp
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -33,6 +33,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:common',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(AOS)/atom_code/messages/messages.gyp:messages',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 84f20b5..237a755 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -120,9 +120,8 @@
static double angle_adjust_goal = 0.42;
if (Pressed(2, 5)) {
// long shot
- shooter_goal->velocity = 375;
+ shooter_goal->velocity = 131;
angle_adjust_goal = 0.70;
- angle_adjust_goal = 0.564;
} else if (Pressed(2, 3)) {
// medium shot
shooter_goal->velocity = 375;
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
index b241f50..2b134d0 100644
--- a/frc971/input/sensor_packer.cc
+++ b/frc971/input/sensor_packer.cc
@@ -36,6 +36,7 @@
unique_ptr< ::AnalogTrigger>(
new ::AnalogTrigger(1)),
::AnalogTriggerOutput::Type::kState)),
+ wrist_edge_position_(0),
wrist_notifier_(ReadEncoder, wrist_hall_effect_->source(),
new EncoderReadData(wrist_counter_.get(),
&wrist_edge_position_, &wrist_sync_)),