added infrastructure for changing between auto modes
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 5815a34..13e8e11 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -99,6 +99,8 @@
uint16_t battery_voltage_high, battery_voltage_low;
+ uint16_t auto_mode_selector;
+
HallEffectEdges pusher_distal, pusher_proximal;
struct {
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index c0e4ad6..73969c7 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -120,6 +120,8 @@
packet->main.battery_voltage_high = analog_get(5);
packet->main.battery_voltage_low = analog_get(3);
+ packet->main.auto_mode_selector = analog_get(4);
+
packet->main.ultrasonic_pulse_length = ultrasonic_length;
{
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index b9f52d3..6c70401 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -17,6 +17,7 @@
#include "frc971/actions/action_client.h"
#include "frc971/actions/shoot_action.h"
#include "frc971/actions/drivetrain_action.h"
+#include "frc971/queues/other_sensors.q.h"
#include "frc971/queues/hot_goal.q.h"
using ::aos::time::Time;
@@ -223,13 +224,31 @@
}
void HandleAuto() {
+ enum class AutoVersion : uint8_t {
+ kStraight,
+ kDoubleHot,
+ };
+
// The front of the robot is 1.854 meters from the wall
static const double kShootDistance = 3.15;
static const double kPickupDistance = 0.5;
static const double kTurnAngle = 0.3;
+
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
+ AutoVersion auto_version;
+ ::frc971::sensors::auto_mode.FetchLatest();
+ if (!::frc971::sensors::auto_mode.get()) {
+ LOG(WARNING, "not sure which auto mode to use\n");
+ auto_version = AutoVersion::kStraight;
+ } else {
+ auto_version =
+ (::frc971::sensors::auto_mode->voltage > 2.5) ? AutoVersion::kDoubleHot
+ : AutoVersion::kStraight;
+ }
+ LOG(INFO, "running auto %" PRIu8 "\n", auto_version);
+
::frc971::HotGoal start_counts;
hot_goal.FetchLatest();
bool start_counts_valid = true;
@@ -240,7 +259,7 @@
memcpy(&start_counts, hot_goal.get(), sizeof(start_counts));
LOG_STRUCT(INFO, "counts at start", start_counts);
}
- (void)start_counts_valid;
+ (void)start_counts_valid;
ResetDrivetrain();
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index 68a026c..bde28b8 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -29,6 +29,7 @@
'<(DEPTH)/frc971/actions/actions.gyp:action_client',
'<(DEPTH)/frc971/actions/actions.gyp:shoot_action_lib',
'<(DEPTH)/frc971/actions/actions.gyp:drivetrain_action_lib',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/frc971/queues/queues.gyp:hot_goal',
'<(AOS)/common/logging/logging.gyp:queue_logging',
],
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index a4f4f29..a171621 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -200,6 +200,10 @@
.sonar_distance(sonar_translate(data->main.ultrasonic_pulse_length))
.Send();
+ ::frc971::sensors::auto_mode.MakeWithBuilder()
+ .voltage(adc_translate(data->main.auto_mode_selector))
+ .Send();
+
drivetrain.position.MakeWithBuilder()
.right_encoder(drivetrain_translate(data->main.right_drive))
.left_encoder(-drivetrain_translate(data->main.left_drive))
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
index e986a10..7a33567 100644
--- a/frc971/queues/other_sensors.q
+++ b/frc971/queues/other_sensors.q
@@ -12,3 +12,8 @@
double angle;
};
queue GyroReading gyro_reading;
+
+message AutoMode {
+ double voltage;
+};
+queue AutoMode auto_mode;