Tune intaking and add support for picking up off the back
We now have a sequence of buttons which can be used to pick a disc up
off the back of the bot at the start of the match. That needs to be
converted to be autonomous.
Change-Id: I19d01a785f95182349722081e610ae303807a441
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 57eb4a2..310f41f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -45,7 +45,7 @@
const ButtonLocation kElevatorBack5(4, 6);
const ButtonLocation kElevatorIntaking(3, 4);
-const ButtonLocation kElevatorIntakingUp(3, 6);
+const ButtonLocation kElevatorOuttake(3, 6);
const ButtonLocation kRelease(4, 4);
const ButtonLocation kSuctionBall(3, 13);
@@ -243,14 +243,18 @@
wrist_angle_ = 0.0;
elevator_height_ = 0.36;
}
- //if (data.IsPressed(kElevatorIntaking)) {
- //}
+
+ if (data.IsPressed(kElevatorOuttake) ||
+ (data.IsPressed(kIntakeOut) &&
+ !superstructure_queue.status->has_piece)) {
+ new_superstructure_goal->intake.unsafe_goal = 0.959327;
+ }
+
if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
elevator_height_ = 0.29;
wrist_angle_ = 2.14;
- new_superstructure_goal->intake.unsafe_goal = 0.52;
if (data.IsPressed(kElevatorIntaking)) {
- new_superstructure_goal->roller_voltage = 6.0;
+ new_superstructure_goal->roller_voltage = 9.0;
} else {
new_superstructure_goal->roller_voltage = 0.0;
}
@@ -260,9 +264,29 @@
new_superstructure_goal->roller_voltage = 0.0;
}
- if (data.IsPressed(kElevatorIntakingUp)) {
- elevator_height_ = 0.29 + 0.3;
- wrist_angle_ = 2.14;
+ if (data.IsPressed(kElevatorOuttake)) {
+ new_superstructure_goal->roller_voltage = -6.0;
+ }
+
+ if (data.IsPressed(kElevatorBack1)) {
+ wrist_angle_ = -2.451824;
+ elevator_height_ = 0.430478;
+ //new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
+ //new_superstructure_goal->wrist.profile_params.max_acceleration = 20.0;
+ }
+ if (data.IsPressed(kElevatorBack2)) {
+ wrist_angle_ = -2.400;
+ elevator_height_ = 0.364108;
+ new_superstructure_goal->elevator.profile_params.max_velocity = 2.0;
+ new_superstructure_goal->elevator.profile_params.max_acceleration = 5.0;
+ new_superstructure_goal->wrist.profile_params.max_velocity = 0.35;
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
+ }
+ if (data.IsPressed(kElevatorBack3)) {
+ wrist_angle_ = -2.211173;
+ elevator_height_ = 0.25;
+ new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 2efb55b..193aaa9 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -359,7 +359,7 @@
void set_intake_roller_talon(
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
intake_rollers_talon_ = ::std::move(t);
- intake_rollers_talon_->ConfigContinuousCurrentLimit(20.0, 0);
+ intake_rollers_talon_->ConfigContinuousCurrentLimit(10.0, 0);
intake_rollers_talon_->EnableCurrentLimit(true);
}