Changes to make the robot work with intake and shooter.
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 5b31505..31394da 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -52,10 +52,10 @@
.angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data()->main.wrist))
- .left_encoder(drivetrain_translate(data()->main.shooter))
- .Send();
+ /*drivetrain.position.MakeWithBuilder()
+ .right_encoder(0)
+ .left_encoder(0)
+ .Send();*/
LOG(DEBUG, "right: %lf left: %lf angle: %lld \n",
drivetrain_translate(data()->main.wrist),
drivetrain_translate(data()->main.shooter), data()->gyro_angle);
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 3e1cfa7..b43fc48 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -12,6 +12,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/shooter/shooter.gyp:shooter_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
],
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index 5efe8b4..6922e1e 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -8,12 +8,14 @@
#include "aos/common/logging/logging.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/shooter/shooter_motor.q.h"
#include "bot3/autonomous/auto.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
#include "frc971/queues/CameraTarget.q.h"
using ::bot3::control_loops::drivetrain;
+using ::bot3::control_loops::shooter;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
// using ::frc971::vision::target_angle;
@@ -32,15 +34,10 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kMediumShot(3, 3);
-const ButtonLocation kShortShot(3, 6);
-const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
+const ButtonLocation kPush(3, 9);
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kIntake(3, 10);
-const ButtonLocation kForceFire(3, 12);
-const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+const ButtonLocation kFire(3, 3);
+const ButtonLocation kIntake(3, 4);
class Reader : public ::aos::input::JoystickInput {
public:
@@ -126,6 +123,23 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
+
+ // 3, 4, 9, 9 fires, 3 pickups
+
+ shooter.status.FetchLatest();
+ bool push = false;
+ double velocity = 0.0;
+ double intake = 0.0;
+ if (data.IsPressed(kPush) && shooter.status->ready) {
+ push = true;
+ }
+ if (data.IsPressed(kFire)) {
+ velocity = 250;
+ }
+ if (data.IsPressed(kIntake)) {
+ intake = 0.9;
+ }
+ shooter.goal.MakeWithBuilder().intake(intake).velocity(velocity).push(push).Send();
#if 0
::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();