Copied over and modified drivetrain framework.
Sorry for the truly massive commit, there's not really a good way
to split it up, as then things will stop compiling.
Needless to say, stuff does compile but remains as of yet
untested.
Among the changes:
- Copied over constants for drivetrain_motor_plant from 2012.
- Completely (for now) disabled control loop driving, as Brian
said those loops would be too much work to tune.
- Put stuff in the proper namespaces and fixed the bazillion
resulting broken refs.
- Copied autonomous framework, but it currectly does nothing.
(I intend for this to change in the future.)
In it's current state, the work on this branch should render
the robot driveable, but that's about it.
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
index 62c3657..a5f961f 100644
--- a/bot3/input/gyro_board_reader.cc
+++ b/bot3/input/gyro_board_reader.cc
@@ -7,11 +7,7 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "bot3/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -20,8 +16,7 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::shooter;
+using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro;
namespace bot3 {
@@ -35,7 +30,7 @@
// TODO(daniel): This might have to change if I find out that the gear ratios are different.
inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(15.0 / 34.0) /*gears*/ * (2 * M_PI);
}
@@ -164,9 +159,7 @@
.left_encoder(-drivetrain_translate(data->left_drive))
.Send();
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
- .Send();
+ //TODO (danielp): Add stuff for bot3 shooter.
}
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 4dec23e..6553e3a 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -8,11 +8,7 @@
#include "aos/common/sensors/sensor_unpacker.h"
#include "aos/common/sensors/sensor_receiver.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
@@ -21,14 +17,11 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
+using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro;
+using ::frc971::GyroBoardData;
-namespace frc971 {
+namespace bot3 {
namespace {
inline double drivetrain_translate(int32_t in) {
@@ -114,41 +107,6 @@
.right_encoder(drivetrain_translate(data->right_drive))
.left_encoder(-drivetrain_translate(data->left_drive))
.Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(!data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(!data->top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
- .top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(!data->bottom_disc)
- .bottom_disc_posedge_count(bottom_rise_count_)
- .bottom_disc_negedge_count(bottom_fall_count_)
- .bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
- .Send();
}
private:
@@ -287,12 +245,12 @@
LibUSB libusb_;
};
-} // namespace frc971
+} // namespace bot3
int main() {
::aos::Init();
- ::frc971::GyroSensorUnpacker unpacker;
- ::frc971::GyroSensorReceiver receiver(&unpacker);
+ ::bot3::GyroSensorUnpacker unpacker;
+ ::bot3::GyroSensorReceiver receiver(&unpacker);
while (true) {
receiver.RunIteration();
}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 8d75241..5beb3d0 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -10,13 +10,9 @@
'<(AOS)/atom_code/input/input.gyp:joystick_input',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
-
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
+ '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
],
},
{
@@ -26,12 +22,8 @@
'gyro_sensor_receiver.cc',
],
'dependencies': [
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(EXTERNALS):libusb',
@@ -47,12 +39,8 @@
'gyro_board_reader.cc',
],
'dependencies': [
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(EXTERNALS):libusb',
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
index da8a8e2..5efe8b4 100644
--- a/bot3/input/joystick_reader.cc
+++ b/bot3/input/joystick_reader.cc
@@ -7,21 +7,16 @@
#include "aos/atom_code/input/joystick_input.h"
#include "aos/common/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "bot3/autonomous/auto.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/queues/CameraTarget.q.h"
-using ::frc971::control_loops::drivetrain;
+using ::bot3::control_loops::drivetrain;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
-using ::frc971::control_loops::index_loop;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::hangers;
-using ::frc971::vision::target_angle;
+// using ::frc971::vision::target_angle;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -47,8 +42,6 @@
const ButtonLocation kForceFire(3, 12);
const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
-const ButtonLocation kDeployHangers(3, 1);
-
class Reader : public ::aos::input::JoystickInput {
public:
static const bool kWristAlwaysDown = false;
@@ -64,11 +57,11 @@
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
+ ::bot3::autonomous::autonomous.MakeWithBuilder()
.run_auto(true).Send();
} else if (data.NegEdge(ControlBit::kEnabled)) {
LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
+ ::bot3::autonomous::autonomous.MakeWithBuilder()
.run_auto(false).Send();
}
} else { // teleop
@@ -78,9 +71,11 @@
const double wheel = data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
- const double kThrottleGain = 1.0 / 2.5;
+ //const double kThrottleGain = 1.0 / 2.5;
if (data.IsPressed(kDriveControlLoopEnable1) ||
data.IsPressed(kDriveControlLoopEnable2)) {
+ LOG(INFO, "Control loop driving is currently not supported by this robot.\n");
+#if 0
static double distance = 0.0;
static double angle = 0.0;
static double filtered_goal_distance = 0.0;
@@ -99,7 +94,6 @@
//const double gyro_angle = Gyro.View().angle;
const double goal_theta = angle - wheel * 0.27;
const double goal_distance = distance + throttle * kThrottleGain;
- //TODO(danielp) Change this after a look in the CAD.
const double robot_width = 22.0 / 100.0 * 2.54;
const double kMaxVelocity = 0.6;
if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
@@ -115,6 +109,7 @@
is_high_gear = false;
LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+#endif
}
if (!(drivetrain.goal.MakeWithBuilder()
.steering(wheel)
@@ -131,7 +126,7 @@
if (data.PosEdge(kShiftLow)) {
is_high_gear = true;
}
-
+#if 0
::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
shooter_goal->velocity = 0;
@@ -163,7 +158,7 @@
shooter_goal->velocity = 375;
}
- //TODO (daniel) Modify this for hopper.
+ //TODO (daniel) Modify this for hopper and shooter.
::aos::ScopedMessagePtr<frc971::control_loops::IndexLoop::Goal> index_goal =
index_loop.goal.MakeMessage();
if (data.IsPressed(kFire)) {
@@ -194,15 +189,8 @@
index_goal.Send();
shooter_goal.Send();
+#endif
}
-
- static int hanger_cycles = 0;
- if (data.IsPressed(kDeployHangers)) {
- ++hanger_cycles;
- } else {
- hanger_cycles = 0;
- }
- hangers.MakeWithBuilder().set(hanger_cycles >= 10).Send();
}
};