Junior commit.
Changed range constants.
Change-Id: Ibcea5ddf00a0ea65f2d8a46a9612732dfbd705f4
diff --git a/y2016_bot3/actors/autonomous_actor.cc b/y2016_bot3/actors/autonomous_actor.cc
index 4167f2e..27dae0e 100644
--- a/y2016_bot3/actors/autonomous_actor.cc
+++ b/y2016_bot3/actors/autonomous_actor.cc
@@ -204,14 +204,12 @@
void AutonomousActor::MoveIntake(double intake_goal,
const ProfileParameters intake_params,
bool traverse_up, double roller_power) {
-
auto new_intake_goal =
::y2016_bot3::control_loops::intake_queue.goal.MakeMessage();
new_intake_goal->angle_intake = intake_goal;
- new_intake_goal->max_angular_velocity_intake =
- intake_params.max_velocity;
+ new_intake_goal->max_angular_velocity_intake = intake_params.max_velocity;
new_intake_goal->max_angular_acceleration_intake =
intake_params.max_acceleration;
@@ -219,7 +217,6 @@
new_intake_goal->voltage_top_rollers = roller_power;
new_intake_goal->voltage_bottom_rollers = roller_power;
- new_intake_goal->traverse_unlatched = true;
new_intake_goal->traverse_down = !traverse_up;
if (!new_intake_goal.Send()) {
@@ -241,13 +238,15 @@
if (::std::abs(control_loops::intake_queue.status->intake.goal_angle -
intake_goal_.intake) < kProfileError &&
- ::std::abs(control_loops::intake_queue.status->intake
- .goal_angular_velocity) < kProfileError) {
+ ::std::abs(
+ control_loops::intake_queue.status->intake.goal_angular_velocity) <
+ kProfileError) {
LOG(DEBUG, "Profile done.\n");
if (::std::abs(control_loops::intake_queue.status->intake.angle -
intake_goal_.intake) < kEpsilon &&
- ::std::abs(control_loops::intake_queue.status->intake
- .angular_velocity) < kEpsilon) {
+ ::std::abs(
+ control_loops::intake_queue.status->intake.angular_velocity) <
+ kEpsilon) {
LOG(INFO, "Near goal, done.\n");
return true;
}
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index e6d2bc2..403c215 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -38,7 +38,7 @@
true,
0.0,
0.25,
- 1.0};
+ 0.50};
return kDrivetrainConfig;
};
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index d3d288b..b3d86af 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -16,7 +16,7 @@
// The maximum voltage the intake roller will be allowed to use.
constexpr float kMaxIntakeTopVoltage = 12.0;
constexpr float kMaxIntakeBottomVoltage = 12.0;
-
+constexpr float kMaxIntakeRollersVoltage = 12.0;
}
// namespace
@@ -140,10 +140,10 @@
requested_intake = unsafe_goal->angle_intake;
}
- //Push the request out to the hardware.
+ // Push the request out to the hardware.
limit_checker_.UpdateGoal(requested_intake);
- // ESTOP if we hit the hard limits.
+ // ESTOP if we hit the hard limits.
if (intake_.CheckHardLimits() && output) {
state_ = ESTOP;
}
@@ -176,6 +176,7 @@
output->voltage_top_rollers = 0.0;
output->voltage_bottom_rollers = 0.0;
+ output->voltage_intake_rollers = 0.0;
if (unsafe_goal) {
// Ball detector lights.
@@ -190,6 +191,10 @@
output->voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
+ output->voltage_intake_rollers =
+ ::std::max(-kMaxIntakeRollersVoltage,
+ ::std::min(unsafe_goal->voltage_intake_rollers,
+ kMaxIntakeRollersVoltage));
output->voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
::std::min(unsafe_goal->voltage_bottom_rollers,
@@ -200,7 +205,6 @@
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
output->traverse_down = unsafe_goal->traverse_down;
}
}
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index a5b4da6..57d8f84 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -29,18 +29,19 @@
// the use.
// TODO(constants): Update these.
static constexpr ::frc971::constants::Range kIntakeRange{// Lower hard stop
- -0.5,
+ -0.4,
// Upper hard stop
- 2.90,
+ 2.80,
// Lower soft stop
- -0.300,
+ -0.28,
// Uppper soft stop
- 2.725};
+ 2.77};
struct IntakeZero {
- double pot_offset = 0.0;
- ::frc971::constants::ZeroingConstants zeroing{
- kZeroingSampleSize, kIntakeEncoderIndexDifference, 0.0, 0.3};
+ double pot_offset = 5.462409 + 0.333162;
+ ::frc971::constants::ZeroingConstants zeroing{kZeroingSampleSize,
+ kIntakeEncoderIndexDifference,
+ +(-0.291240 + 0.148604), 0.3};
};
} // namespace constants
namespace control_loops {
@@ -56,11 +57,12 @@
// TODO(Adam): Implement this class and delete it from here.
class LimitChecker {
- public:
- LimitChecker(IntakeArm *intake) : intake_(intake) {}
- void UpdateGoal(double intake_angle_goal);
- private:
- IntakeArm *intake_;
+ public:
+ LimitChecker(IntakeArm *intake) : intake_(intake) {}
+ void UpdateGoal(double intake_angle_goal);
+
+ private:
+ IntakeArm *intake_;
};
class Intake : public ::aos::controls::ControlLoop<control_loops::IntakeQueue> {
@@ -137,7 +139,6 @@
DISALLOW_COPY_AND_ASSIGN(Intake);
};
-
} // namespace intake
} // namespace control_loops
} // namespace y2016_bot3
diff --git a/y2016_bot3/control_loops/intake/intake.q b/y2016_bot3/control_loops/intake/intake.q
index b6b0bdc..55d102d 100644
--- a/y2016_bot3/control_loops/intake/intake.q
+++ b/y2016_bot3/control_loops/intake/intake.q
@@ -57,12 +57,10 @@
// Voltage to send to the rollers. Positive is sucking in.
float voltage_top_rollers;
float voltage_bottom_rollers;
+ float voltage_intake_rollers;
bool force_intake;
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
// If true, fire the traverse mechanism down.
bool traverse_down;
};
@@ -77,7 +75,6 @@
// The internal state of the state machine.
int32_t state;
-
// Estimated angle and angular velocitie of the intake.
JointState intake;
};
@@ -93,9 +90,8 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+ float voltage_intake_rollers;
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
// If true, fire the traverse mechanism down.
bool traverse_down;
};
diff --git a/y2016_bot3/joystick_reader.cc b/y2016_bot3/joystick_reader.cc
index 97bd089..02e6817 100644
--- a/y2016_bot3/joystick_reader.cc
+++ b/y2016_bot3/joystick_reader.cc
@@ -40,11 +40,11 @@
const ButtonLocation kTurn2(1, 11);
// Buttons on the lexan driver station to get things running on bring-up day.
-const ButtonLocation kIntakeDown(3, 11);
-const ButtonLocation kIntakeIn(3, 12);
+const ButtonLocation kIntakeDown(3, 5);
+const ButtonLocation kIntakeIn(3, 4);
const ButtonLocation kFire(3, 3);
-const ButtonLocation kIntakeOut(3, 9);
-const ButtonLocation kChevalDeFrise(3, 8);
+const ButtonLocation kIntakeOut(3, 3);
+const ButtonLocation kChevalDeFrise(3, 11);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -165,10 +165,8 @@
}
if (data.IsPressed(kChevalDeFrise)) {
- traverse_unlatched_ = false;
traverse_down_ = true;
} else {
- traverse_unlatched_ = true;
traverse_down_ = false;
}
@@ -179,23 +177,26 @@
new_intake_goal->max_angular_velocity_intake = 7.0;
new_intake_goal->max_angular_acceleration_intake = 40.0;
- // Granny mode
- /*
+#define GrannyMode false
+#if GrannyMode
new_intake_goal->max_angular_velocity_intake = 0.2;
new_intake_goal->max_angular_acceleration_intake = 1.0;
- */
+#endif
+
if (is_intaking_) {
- new_intake_goal->voltage_top_rollers = 12.0;
+ new_intake_goal->voltage_intake_rollers = -12.0;
+ new_intake_goal->voltage_top_rollers = -12.0;
new_intake_goal->voltage_bottom_rollers = 12.0;
} else if (is_outtaking_) {
- new_intake_goal->voltage_top_rollers = -12.0;
+ new_intake_goal->voltage_intake_rollers = 12.0;
+ new_intake_goal->voltage_top_rollers = 12.0;
new_intake_goal->voltage_bottom_rollers = -7.0;
} else {
+ new_intake_goal->voltage_intake_rollers = 0.0;
new_intake_goal->voltage_top_rollers = 0.0;
new_intake_goal->voltage_bottom_rollers = 0.0;
}
- new_intake_goal->traverse_unlatched = traverse_unlatched_;
new_intake_goal->traverse_down = traverse_down_;
new_intake_goal->force_intake = true;
@@ -233,7 +234,6 @@
bool was_running_ = false;
bool auto_running_ = false;
- bool traverse_unlatched_ = false;
bool traverse_down_ = false;
// If we're waiting for the subsystems to zero.
diff --git a/y2016_bot3/wpilib/wpilib_interface.cc b/y2016_bot3/wpilib/wpilib_interface.cc
index ad75401..b870e00 100644
--- a/y2016_bot3/wpilib/wpilib_interface.cc
+++ b/y2016_bot3/wpilib/wpilib_interface.cc
@@ -102,7 +102,7 @@
double intake_pot_translate(double voltage) {
return voltage * ::y2016_bot3::constants::kIntakePotRatio *
- (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+ (5.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
constexpr double kMaxDrivetrainEncoderPulsesPerSecond =
@@ -194,7 +194,7 @@
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
- drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
-drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
@@ -210,7 +210,7 @@
{
auto intake_message = intake_queue.position.MakeMessage();
CopyPotAndIndexPosition(intake_encoder_, &intake_message->intake,
- intake_translate, intake_pot_translate, false,
+ intake_translate, intake_pot_translate, true,
intake_pot_offset);
intake_message.Send();
@@ -286,11 +286,6 @@
traverse_ = ::std::move(s);
}
- void set_traverse_latch(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- traverse_latch_ = ::std::move(s);
- }
-
void operator()() {
compressor_->Start();
::aos::SetCurrentThreadName("Solenoids");
@@ -311,9 +306,7 @@
intake_.FetchLatest();
if (intake_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *intake_);
-
traverse_->Set(intake_->traverse_down);
- traverse_latch_->Set(intake_->traverse_unlatched);
}
}
@@ -333,8 +326,7 @@
private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> traverse_,
- traverse_latch_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> traverse_;
::std::unique_ptr<Compressor> compressor_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
@@ -345,12 +337,16 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
- void set_drivetrain_left_talon(::std::unique_ptr<Talon> t) {
- drivetrain_left_talon_ = ::std::move(t);
+ void set_drivetrain_left_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_left_talon_0_ = ::std::move(t0);
+ drivetrain_left_talon_1_ = ::std::move(t1);
}
- void set_drivetrain_right_talon(::std::unique_ptr<Talon> t) {
- drivetrain_right_talon_ = ::std::move(t);
+ void set_drivetrain_right_talon(::std::unique_ptr<Talon> t0,
+ ::std::unique_ptr<Talon> t1) {
+ drivetrain_right_talon_0_ = ::std::move(t0);
+ drivetrain_right_talon_1_ = ::std::move(t1);
}
private:
@@ -361,17 +357,22 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::drivetrain_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- drivetrain_left_talon_->Set(queue->left_voltage / 12.0);
- drivetrain_right_talon_->Set(-queue->right_voltage / 12.0);
+ drivetrain_left_talon_0_->Set(queue->left_voltage / 12.0);
+ drivetrain_left_talon_1_->Set(queue->left_voltage / 12.0);
+ drivetrain_right_talon_0_->Set(-queue->right_voltage / 12.0);
+ drivetrain_right_talon_1_->Set(-queue->right_voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_talon_->Disable();
- drivetrain_right_talon_->Disable();
+ drivetrain_left_talon_0_->Disable();
+ drivetrain_right_talon_0_->Disable();
+ drivetrain_left_talon_1_->Disable();
+ drivetrain_right_talon_1_->Disable();
}
- ::std::unique_ptr<Talon> drivetrain_left_talon_, drivetrain_right_talon_;
+ ::std::unique_ptr<Talon> drivetrain_left_talon_0_, drivetrain_right_talon_0_,
+ drivetrain_right_talon_1_, drivetrain_left_talon_1_;
};
class IntakeWriter : public ::frc971::wpilib::LoopOutputHandler {
@@ -380,6 +381,10 @@
intake_talon_ = ::std::move(t);
}
+ void set_intake_rollers_talon(::std::unique_ptr<Talon> t) {
+ intake_rollers_talon_ = ::std::move(t);
+ }
+
void set_top_rollers_talon(::std::unique_ptr<Talon> t) {
top_rollers_talon_ = ::std::move(t);
}
@@ -400,6 +405,7 @@
kMaxBringupPower) /
12.0);
top_rollers_talon_->Set(-queue->voltage_top_rollers / 12.0);
+ intake_rollers_talon_->Set(-queue->voltage_intake_rollers / 12.0);
bottom_rollers_talon_->Set(-queue->voltage_bottom_rollers / 12.0);
}
@@ -409,7 +415,7 @@
}
::std::unique_ptr<Talon> intake_talon_, top_rollers_talon_,
- bottom_rollers_talon_;
+ bottom_rollers_talon_, intake_rollers_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -430,14 +436,14 @@
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
SensorReader reader;
- reader.set_drivetrain_left_encoder(make_encoder(5));
- reader.set_drivetrain_right_encoder(make_encoder(6));
+ reader.set_drivetrain_left_encoder(make_encoder(0));
+ reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_intake_encoder(make_encoder(0));
+ reader.set_intake_encoder(make_encoder(2));
reader.set_intake_index(make_unique<DigitalInput>(0));
- reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
+ reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
- reader.set_ball_detector(make_unique<AnalogInput>(7));
+ reader.set_ball_detector(make_unique<AnalogInput>(5));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
@@ -446,23 +452,27 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
DrivetrainWriter drivetrain_writer;
+ // 2 and 3 are right. 0 and 1 are left
drivetrain_writer.set_drivetrain_left_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
+ ::std::unique_ptr<Talon>(new Talon(0)),
+ ::std::unique_ptr<Talon>(new Talon(1)));
drivetrain_writer.set_drivetrain_right_talon(
- ::std::unique_ptr<Talon>(new Talon(4)));
+ ::std::unique_ptr<Talon>(new Talon(2)),
+ ::std::unique_ptr<Talon>(new Talon(3)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
IntakeWriter intake_writer;
- intake_writer.set_intake_talon(::std::unique_ptr<Talon>(new Talon(3)));
- intake_writer.set_top_rollers_talon(::std::unique_ptr<Talon>(new Talon(1)));
+ intake_writer.set_intake_talon(::std::unique_ptr<Talon>(new Talon(7)));
+ intake_writer.set_top_rollers_talon(::std::unique_ptr<Talon>(new Talon(6)));
+ intake_writer.set_intake_rollers_talon(
+ ::std::unique_ptr<Talon>(new Talon(5)));
intake_writer.set_bottom_rollers_talon(
- ::std::unique_ptr<Talon>(new Talon(0)));
+ ::std::unique_ptr<Talon>(new Talon(4)));
::std::thread intake_writer_thread(::std::ref(intake_writer));
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
solenoid_writer.set_compressor(make_unique<Compressor>());