Merge in the changes James made at NASA.
This is the code that got the robot driving originally.
Conflicts:
bot3/control_loops/shooter/shooter_motor_plant.cc
bot3/input/gyro_sensor_receiver.cc
diff --git a/aos/atom_code/init.cc b/aos/atom_code/init.cc
index c4e6d44..b3668e8 100644
--- a/aos/atom_code/init.cc
+++ b/aos/atom_code/init.cc
@@ -81,7 +81,7 @@
void InitNRT() { DoInitNRT(aos_core_create::reference); }
void InitCreate() { DoInitNRT(aos_core_create::create); }
-void Init() {
+void Init(int relative_priority) {
if (getenv(kNoRealtimeEnvironmentVariable) == NULL) { // if nobody set it
LockAllMemory();
// Only let rt processes run for 1 second straight.
@@ -90,7 +90,7 @@
SetSoftRLimit(RLIMIT_RTPRIO, 40, false);
// Set our process to priority 40.
struct sched_param param;
- param.sched_priority = 40;
+ param.sched_priority = 30 + relative_priority;
if (sched_setscheduler(0, SCHED_FIFO, ¶m) != 0) {
Die("%s-init: setting SCHED_FIFO failed with %d (%s)\n",
program_invocation_short_name, errno, strerror(errno));
diff --git a/aos/atom_code/init.h b/aos/atom_code/init.h
index ffb7afc..b35a3b8 100644
--- a/aos/atom_code/init.h
+++ b/aos/atom_code/init.h
@@ -6,7 +6,9 @@
// Does the non-realtime parts of the initialization process.
void InitNRT();
// Initializes everything, including the realtime stuff.
-void Init();
+// relative_priority adjusts the priority of this process relative to all of the
+// other ones (positive for higher priority).
+void Init(int relative_priority = 0);
// Same as InitNRT, except will remove an existing shared memory file and create
// a new one.
void InitCreate();
diff --git a/aos/common/network/network.gyp b/aos/common/network/network.gyp
index 279bb53..60b37aa 100644
--- a/aos/common/network/network.gyp
+++ b/aos/common/network/network.gyp
@@ -1,6 +1,18 @@
{
'targets': [
{
+ 'target_name': 'team_number',
+ 'type': 'static_library',
+ 'sources': [
+ 'team_number.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/atom_code.gyp:configuration',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'socket_so',
'type': 'shared_library',
'variables': {'no_rsync': 1},
diff --git a/aos/common/network/team_number.cc b/aos/common/network/team_number.cc
new file mode 100644
index 0000000..367f991
--- /dev/null
+++ b/aos/common/network/team_number.cc
@@ -0,0 +1,31 @@
+#include "aos/common/network/team_number.h"
+
+#include <netinet/in.h>
+#include <inttypes.h>
+
+#include "aos/common/once.h"
+#include "aos/atom_code/configuration.h"
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace network {
+namespace {
+
+uint16_t *DoGetTeamNumber() {
+ const in_addr &address = configuration::GetOwnIPAddress();
+ static uint16_t r =
+ (((address.s_addr & 0xFF00) >> 8) * 100) +
+ (((address.s_addr & 0xFF0000) >> 16) & 0xFF);
+ LOG(INFO, "team number is %" PRIu16 "\n", r);
+ return &r;
+}
+
+} // namespace
+
+uint16_t GetTeamNumber() {
+ static Once<uint16_t> once(DoGetTeamNumber);
+ return *once.Get();
+}
+
+} // namespace network
+} // namespace aos
diff --git a/aos/common/network/team_number.h b/aos/common/network/team_number.h
new file mode 100644
index 0000000..f250c85
--- /dev/null
+++ b/aos/common/network/team_number.h
@@ -0,0 +1,17 @@
+#ifndef AOS_COMMON_NETWORK_TEAM_NUMBER_H_
+#define AOS_COMMON_NETWORK_TEAM_NUMBER_H_
+
+#include <stdint.h>
+
+namespace aos {
+namespace network {
+
+// Retrieves the current team number based off of the network address.
+// This function will only do the complicated stuff once so it is cheap to call
+// repeatedly.
+uint16_t GetTeamNumber();
+
+} // namespace network
+} // namespace aos
+
+#endif // AOS_COMMON_NETWORK_TEAM_NUMBER_H_
diff --git a/bot3/atom_code/scripts/start_list.txt b/bot3/atom_code/scripts/start_list.txt
index 46ece83..f8a0ba7 100644
--- a/bot3/atom_code/scripts/start_list.txt
+++ b/bot3/atom_code/scripts/start_list.txt
@@ -2,7 +2,6 @@
MotorWriter
joystick_reader
drivetrain
-CRIOLogReader
auto
shooter
gyro_sensor_receiver
diff --git a/bot3/autonomous/autonomous.gyp b/bot3/autonomous/autonomous.gyp
index 7e2d5da..bf5f4ff 100644
--- a/bot3/autonomous/autonomous.gyp
+++ b/bot3/autonomous/autonomous.gyp
@@ -26,7 +26,7 @@
'<(AOS)/common/common.gyp:controls',
'<(AOS)/build/aos.gyp:logging',
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
diff --git a/bot3/control_loops/drivetrain/drivetrain.gyp b/bot3/control_loops/drivetrain/drivetrain.gyp
index 17e5a09..7cc5191 100644
--- a/bot3/control_loops/drivetrain/drivetrain.gyp
+++ b/bot3/control_loops/drivetrain/drivetrain.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
diff --git a/bot3/control_loops/shooter/shooter.cc b/bot3/control_loops/shooter/shooter.cc
index d1a07f1..fb78a10 100644
--- a/bot3/control_loops/shooter/shooter.cc
+++ b/bot3/control_loops/shooter/shooter.cc
@@ -11,12 +11,9 @@
ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
: aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
- loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
- history_position_(0),
- position_goal_(0.0),
- last_position_(0.0),
+ loop_(new StateFeedbackLoop<1, 1, 1>(MakeShooterLoop())),
last_velocity_goal_(0) {
- memset(history_, 0, sizeof(history_));
+ loop_->Reset();
}
/*static*/ const double ShooterMotor::dt = 0.01;
@@ -27,10 +24,12 @@
control_loops::ShooterLoop::Output *output,
control_loops::ShooterLoop::Status *status) {
double velocity_goal = goal->velocity;
- const double current_position =
+ // Our position here is actually a velocity.
+ average_velocity_ =
(position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
+// TODO (danielp): This must be modified for our index.
/* if (index_loop.status.FetchLatest() || index_loop.status.get()) {
if (index_loop.status->is_shooting) {
if (velocity_goal != last_velocity_goal_ &&
@@ -43,57 +42,21 @@
}*/
last_velocity_goal_ = velocity_goal;
- // Track the current position if the velocity goal is small.
- if (velocity_goal <= 1.0) {
- position_goal_ = current_position;
- }
-
- loop_->Y << current_position;
-
- // Add the position to the history.
- history_[history_position_] = current_position;
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- // Prevents integral windup by limiting the position error such that the
- // error can't produce much more than full power.
- const double kVelocityWeightScalar = 0.35;
- const double max_reference =
- (loop_->U_max(0, 0) - kVelocityWeightScalar *
- (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
- / loop_->K(0, 0) + loop_->X_hat(0, 0);
- const double min_reference =
- (loop_->U_min(0, 0) - kVelocityWeightScalar *
- (velocity_goal - loop_->X_hat(1, 0)) * loop_->K(0, 1))
- / loop_->K(0, 0) + loop_->X_hat(0, 0);
-
- position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
- min_reference);
- loop_->R << position_goal_, velocity_goal;
- position_goal_ += velocity_goal * dt;
+ loop_->Y << average_velocity_;
+ loop_->R << velocity_goal;
loop_->Update(position, output == NULL);
// Kill power at low velocity goals.
if (velocity_goal < 1.0) {
- loop_->U[0] = 0.0;
+ loop_->U(0) = 0.0;
} else {
- output_voltage = loop_->U[0];
+ output_voltage = loop_->U(0);
}
LOG(DEBUG,
- "PWM: %f, raw_pos: %f rotations: %f "
- "junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
- output_voltage, current_position,
- current_position / (2 * M_PI),
- (current_position - last_position_) / dt,
- loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
-
- // Calculates the velocity over the last kHistoryLength * .01 seconds
- // by taking the difference between the current and next history positions.
- int old_history_position = ((history_position_ == 0) ?
- kHistoryLength : history_position_) - 1;
- average_velocity_ = (history_[old_history_position] -
- history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+ "PWM: %f, raw_velocity: %f, xhat[0]: %f, R[0]: %f\n",
+ output_voltage, average_velocity_, loop_->X_hat[0], loop_->R[0]);
status->average_velocity = average_velocity_;
@@ -108,8 +71,6 @@
}
LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
- last_position_ = current_position;
-
if (output) {
output->voltage = output_voltage;
output->intake = goal->intake;
diff --git a/bot3/control_loops/shooter/shooter.gyp b/bot3/control_loops/shooter/shooter.gyp
index 56a4f44..2be0439 100644
--- a/bot3/control_loops/shooter/shooter.gyp
+++ b/bot3/control_loops/shooter/shooter.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'shooter_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
diff --git a/bot3/control_loops/shooter/shooter.h b/bot3/control_loops/shooter/shooter.h
index 7c402b9..16176de 100644
--- a/bot3/control_loops/shooter/shooter.h
+++ b/bot3/control_loops/shooter/shooter.h
@@ -33,18 +33,10 @@
private:
// The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<1, 1, 1>> loop_;
- // History array and stuff for determining average velocity and whether
- // we are ready to shoot.
- static const int kHistoryLength = 5;
- double history_[kHistoryLength];
- ptrdiff_t history_position_;
double average_velocity_;
- double position_goal_;
- double last_position_;
-
// For making sure it keeps spinning if we're shooting.
double last_velocity_goal_;
diff --git a/bot3/control_loops/shooter/shooter_lib_test.cc b/bot3/control_loops/shooter/shooter_lib_test.cc
index 062e507..fddc415 100644
--- a/bot3/control_loops/shooter/shooter_lib_test.cc
+++ b/bot3/control_loops/shooter/shooter_lib_test.cc
@@ -22,7 +22,7 @@
// Constructs a shooter simulation. I'm not sure how the construction of
// the queue (my_shooter_loop_) actually works (same format as wrist).
ShooterMotorSimulation()
- : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeShooterPlant())),
+ : shooter_plant_(new StateFeedbackPlant<1, 1, 1>(MakeShooterPlant())),
my_shooter_loop_(".bot3.control_loops.shooter",
0x78d8e372, ".bot3.control_loops.shooter.goal",
".bot3.control_loops.shooter.position",
@@ -45,7 +45,7 @@
shooter_plant_->Update();
}
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+ ::std::unique_ptr<StateFeedbackPlant<1, 1, 1>> shooter_plant_;
private:
ShooterLoop my_shooter_loop_;
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.cc b/bot3/control_loops/shooter/shooter_motor_plant.cc
index c71b96a..ab5dbc7 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.cc
+++ b/bot3/control_loops/shooter/shooter_motor_plant.cc
@@ -1,4 +1,4 @@
-#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "bot3/control_loops/shooter/shooter_motor_plant.h"
#include <vector>
@@ -7,40 +7,40 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients() {
- Eigen::Matrix<double, 2, 2> A;
- A << 1.0, 0.00994518845675, 0.0, 0.989057756738;
- Eigen::Matrix<double, 2, 1> B;
- B << 0.00267091861198, 0.533205953514;
- Eigen::Matrix<double, 1, 2> C;
- C << 1, 0;
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 1> A;
+ A << 0.989057756738;
+ Eigen::Matrix<double, 1, 1> B;
+ B << 0.533205953514;
+ Eigen::Matrix<double, 1, 1> C;
+ C << 0;
Eigen::Matrix<double, 1, 1> D;
D << 0;
Eigen::Matrix<double, 1, 1> U_max;
U_max << 12.0;
Eigen::Matrix<double, 1, 1> U_min;
U_min << -12.0;
- return StateFeedbackPlantCoefficients<2, 1, 1>(A, B, C, D, U_max, U_min);
+ return StateFeedbackPlantCoefficients<1, 1, 1>(A, B, C, D, U_max, U_min);
}
-StateFeedbackController<2, 1, 1> MakeShooterController() {
- Eigen::Matrix<double, 2, 1> L;
- L << 1.08905775674, 29.7111780621;
- Eigen::Matrix<double, 1, 2> K;
- K << 130.450278144, 2.19032372689;
- return StateFeedbackController<2, 1, 1>(L, K, MakeShooterPlantCoefficients());
+StateFeedbackController<1, 1, 1> MakeShooterController() {
+ Eigen::Matrix<double, 1, 1> L;
+ L << 29.7111780621;
+ Eigen::Matrix<double, 1, 1> K;
+ K << 0.758151303088;
+ return StateFeedbackController<1, 1, 1>(L, K, MakeShooterPlantCoefficients());
}
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<2, 1, 1>(MakeShooterPlantCoefficients());
- return StateFeedbackPlant<2, 1, 1>(plants);
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant() {
+ ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
+ plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeShooterPlantCoefficients());
+ return StateFeedbackPlant<1, 1, 1>(plants);
}
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop() {
- ::std::vector<StateFeedbackController<2, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<2, 1, 1>(MakeShooterController());
- return StateFeedbackLoop<2, 1, 1>(controllers);
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop() {
+ ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
+ controllers[0] = new StateFeedbackController<1, 1, 1>(MakeShooterController());
+ return StateFeedbackLoop<1, 1, 1>(controllers);
}
} // namespace control_loops
diff --git a/bot3/control_loops/shooter/shooter_motor_plant.h b/bot3/control_loops/shooter/shooter_motor_plant.h
index fa2237d..1ab9702 100644
--- a/bot3/control_loops/shooter/shooter_motor_plant.h
+++ b/bot3/control_loops/shooter/shooter_motor_plant.h
@@ -6,13 +6,13 @@
namespace bot3 {
namespace control_loops {
-StateFeedbackPlantCoefficients<2, 1, 1> MakeShooterPlantCoefficients();
+StateFeedbackPlantCoefficients<1, 1, 1> MakeShooterPlantCoefficients();
-StateFeedbackController<2, 1, 1> MakeShooterController();
+StateFeedbackController<1, 1, 1> MakeShooterController();
-StateFeedbackPlant<2, 1, 1> MakeShooterPlant();
+StateFeedbackPlant<1, 1, 1> MakeShooterPlant();
-StateFeedbackLoop<2, 1, 1> MakeShooterLoop();
+StateFeedbackLoop<1, 1, 1> MakeShooterLoop();
} // namespace control_loops
} // namespace bot3
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 31394da..bd36865 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -1,3 +1,5 @@
+#include <inttypes.h>
+
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/wrapping_counter.h"
@@ -20,60 +22,74 @@
namespace bot3 {
namespace {
-//TODO (danielp): Figure out whether the bigger gear is on the
-// encoder or not.
inline double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
(32.0 / 44.0 /*encoder gears*/) * // the encoders are on the wheels.
(3.5 /*wheel diameter*/ * 2.54 / 100 * M_PI);
}
-// TODO (danielp): This needs to be modified eventually.
inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
- (15.0 / 34.0) /*gears*/ * (2 * M_PI);
+ return static_cast<double>(in) / 500 /*counter rate*/ * (2 * M_PI);
+}
+
+// Translates values from the ADC into voltage.
+inline double adc_translate(uint16_t in) {
+ static const double kVRefN = 0;
+ static const double kVRefP = 3.3;
+ static const int kMaximumValue = 0x3FF;
+ return kVRefN +
+ (static_cast<double>(in) / static_cast<double>(kMaximumValue) *
+ (kVRefP - kVRefN));
+}
+
+inline double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+inline double battery_translate(uint16_t in) {
+ return adc_translate(in) * 80.8 / 17.8;
}
} // namespace
class GyroSensorReceiver : public USBReceiver {
- virtual void ProcessData() override {
- if (data()->robot_id != 0) {
- LOG(ERROR, "gyro board sent data for robot id %hhd!"
- " dip switches are %x\n",
- data()->robot_id, data()->base_status & 0xF);
- return;
- } else {
- LOG(DEBUG, "processing a packet dip switches %x\n",
- data()->base_status & 0xF);
- }
+ public:
+ GyroSensorReceiver() : USBReceiver(1) {}
+ virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
gyro.MakeWithBuilder()
- .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .angle(gyro_translate(data()->gyro_angle))
.Send();
+ LOG(DEBUG, "right drive: %f, left drive: %f\n",
+ drivetrain_translate(data()->main.shooter_angle),
+ drivetrain_translate(data()->main.indexer));
+ 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);
- shooter.position.MakeWithBuilder().position(drivetrain_translate(data()->main.shooter)).Send();
+ shooter.position.MakeWithBuilder()
+ .position(shooter_translate(data()->bot3.shooter_cycle_ticks));
+
+ LOG(DEBUG, "battery %f %f %" PRIu16 "\n",
+ battery_translate(data()->main.battery_voltage),
+ adc_translate(data()->main.battery_voltage),
+ data()->main.battery_voltage);
+ LOG(DEBUG, "halls l=%f r=%f\n",
+ adc_translate(data()->main.left_drive_hall),
+ adc_translate(data()->main.right_drive_hall));
}
-
- WrappingCounter top_rise_;
- WrappingCounter top_fall_;
- WrappingCounter bottom_rise_;
- WrappingCounter bottom_fall_delay_;
- WrappingCounter bottom_fall_;
};
} // namespace bot3
int main() {
- ::aos::Init();
+ ::aos::Init(frc971::USBReceiver::kRelativePriority);
::bot3::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
diff --git a/bot3/output/CameraServer.cc b/bot3/output/CameraServer.cc
deleted file mode 100644
index 939731d..0000000
--- a/bot3/output/CameraServer.cc
+++ /dev/null
@@ -1,93 +0,0 @@
-#include <string.h>
-
-#include "aos/atom_code/output/HTTPServer.h"
-#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
-#include "aos/atom_code/output/ctemplate_cache.h"
-#include "aos/common/messages/RobotState.q.h"
-#include "ctemplate/template.h"
-#include "aos/atom_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "aos/atom_code/configuration.h"
-
-#include "frc971/constants.h"
-
-RegisterTemplateFilename(ROBOT_HTML, "robot.html.tpl");
-
-namespace frc971 {
-
-class CameraServer : public aos::http::HTTPServer {
- public:
- CameraServer() : HTTPServer(::aos::configuration::GetRootDirectory(), 8080),
- buf_(NULL) {
- AddPage<CameraServer>("/robot.html", &CameraServer::RobotHTML, this);
- }
-
- private:
- evbuffer *buf_;
- bool Setup(evhttp_request *request, const char *content_type) {
- if (evhttp_add_header(evhttp_request_get_output_headers(request),
- "Content-Type", content_type) == -1) {
- LOG(WARNING, "adding Content-Type failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return false;
- }
- if (buf_ == NULL) buf_ = evbuffer_new();
- if (buf_ == NULL) {
- LOG(WARNING, "evbuffer_new() failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return false;
- }
- return true;
- }
- void RobotHTML(evhttp_request *request) {
- if (!Setup(request, "text/html")) return;
-
- ctemplate::TemplateDictionary dict(ROBOT_HTML);
- const char *host = evhttp_find_header(
- evhttp_request_get_input_headers(request), "Host");
- if (host == NULL) {
- evhttp_send_error(request, HTTP_BADREQUEST, "no Host header");
- return;
- }
- const char *separator = strchrnul(host, ':');
- size_t length = separator - host;
- // Don't include the last ':' (or the terminating '\0') or anything else
- // after it.
- dict.SetValue("HOST", ctemplate::TemplateString(host, length));
-
- if (!aos::robot_state.FetchLatest()) {
- LOG(WARNING, "getting a RobotState message failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- int center;
- if (!constants::camera_center(¢er)) {
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- dict.SetIntValue("CENTER", center);
-
- aos::http::EvhttpCtemplateEmitter emitter(buf_);
- if (!aos::http::get_template_cache()->
- ExpandWithData(ROBOT_HTML, ctemplate::STRIP_WHITESPACE,
- &dict, NULL, &emitter)) {
- LOG(ERROR, "expanding the template failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- if (emitter.error()) {
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- evhttp_send_reply(request, HTTP_OK, NULL, buf_);
- }
-};
-
-} // namespace frc971
-
-int main() {
- ::aos::InitNRT();
- ::frc971::CameraServer server;
- server.Run();
- ::aos::Cleanup();
-}
diff --git a/bot3/output/output.gyp b/bot3/output/output.gyp
index c207cfb..54f36e3 100644
--- a/bot3/output/output.gyp
+++ b/bot3/output/output.gyp
@@ -4,11 +4,11 @@
'target_name': 'CameraServer',
'type': 'executable',
'sources': [
- 'CameraServer.cc',
+ '<(DEPTH)/frc971/output/CameraServer.cc',
],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:http_server',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/messages/messages.gyp:aos_queues',
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 13aeb51..97c2310 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -3,7 +3,6 @@
#include "aos/common/control_loop/Timing.h"
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/autonomous/auto.q.h"
@@ -341,7 +340,8 @@
void HandleAuto() {
LOG(INFO, "Handling auto mode\n");
- double WRIST_UP;
+ const double WRIST_UP =
+ constants::GetValues().wrist_hall_effect_start_angle - 0.4;
const double WRIST_DOWN = -0.580;
const double WRIST_DOWN_TWO = WRIST_DOWN - 0.012;
const double ANGLE_ONE = 0.509;
@@ -356,15 +356,6 @@
//::aos::time::SleepFor(::aos::time::Time::InSeconds(20));
if (ShouldExitAuto()) return;
- ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get() ||
- !constants::wrist_hall_effect_start_angle(&WRIST_UP)) {
- LOG(ERROR, "Constants not ready\n");
- return;
- }
- WRIST_UP -= 0.4;
- LOG(INFO, "Got constants\n");
-
control_loops::drivetrain.position.FetchLatest();
while (!control_loops::drivetrain.position.get()) {
LOG(WARNING, "No previous drivetrain position packet, trying to fetch again\n");
diff --git a/frc971/autonomous/autonomous.gyp b/frc971/autonomous/autonomous.gyp
index cd8ce9c..526904c 100644
--- a/frc971/autonomous/autonomous.gyp
+++ b/frc971/autonomous/autonomous.gyp
@@ -29,7 +29,7 @@
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
'<(AOS)/common/util/util.gyp:trapezoid_profile',
diff --git a/frc971/constants.cc b/frc971/constants.cc
new file mode 100644
index 0000000..76a5f3b
--- /dev/null
+++ b/frc971/constants.cc
@@ -0,0 +1,135 @@
+#include "frc971/constants.h"
+
+#include <math.h>
+#include <stdint.h>
+#include <inttypes.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/once.h"
+#include "aos/common/network/team_number.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace frc971 {
+namespace constants {
+namespace {
+
+// It has about 0.029043 of gearbox slop.
+// For purposes of moving the end up/down by a certain amount, the wrist is 18
+// inches long.
+const double kCompWristHallEffectStartAngle = 1.285;
+const double kPracticeWristHallEffectStartAngle = 1.175;
+
+const double kWristHallEffectStopAngle = 100 * M_PI / 180.0;
+
+const double kPracticeWristUpperPhysicalLimit = 1.677562;
+const double kCompWristUpperPhysicalLimit = 1.677562;
+
+const double kPracticeWristLowerPhysicalLimit = -0.746128;
+const double kCompWristLowerPhysicalLimit = -0.746128;
+
+const double kPracticeWristUpperLimit = 1.615385;
+const double kCompWristUpperLimit = 1.615385;
+
+const double kPracticeWristLowerLimit = -0.746128;
+const double kCompWristLowerLimit = -0.746128;
+
+const double kWristZeroingSpeed = 0.125;
+const double kWristZeroingOffSpeed = 0.35;
+
+const int kAngleAdjustHallEffect = 2;
+
+// Angle measured from CAD with the top of the angle adjust at the top of the
+// wire guide is 0.773652098 radians.
+
+const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
+const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305172594, 1.5};
+
+const double kAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
+
+const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
+const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.270;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.302;
+
+const double kPracticeAngleAdjustUpperLimit = 0.87;
+const double kCompAngleAdjustUpperLimit = 0.87;
+
+const double kPracticeAngleAdjustLowerLimit = 0.31;
+const double kCompAngleAdjustLowerLimit = 0.28;
+
+const double kAngleAdjustZeroingSpeed = -0.2;
+const double kAngleAdjustZeroingOffSpeed = -0.5;
+
+const double kPracticeAngleAdjustDeadband = 0.4;
+const double kCompAngleAdjustDeadband = 0.65;
+
+const int kCompCameraCenter = -2;
+const int kPracticeCameraCenter = -5;
+
+const int kCompDrivetrainGearboxPinion = 19;
+const int kPracticeDrivetrainGearboxPinion = 17;
+
+const Values *DoGetValues() {
+ uint16_t team = ::aos::network::GetTeamNumber();
+ LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ switch (team) {
+ case kCompTeamNumber:
+ return new Values{kCompWristHallEffectStartAngle,
+ kWristHallEffectStopAngle,
+ kCompWristUpperLimit,
+ kCompWristLowerLimit,
+ kCompWristUpperPhysicalLimit,
+ kCompWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kCompAngleAdjustHallEffectStartAngle,
+ kAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kCompAngleAdjustDeadband,
+ kCompDrivetrainGearboxPinion,
+ kCompCameraCenter};
+ break;
+ case kPracticeTeamNumber:
+ return new Values{kPracticeWristHallEffectStartAngle,
+ kWristHallEffectStopAngle,
+ kPracticeWristUpperLimit,
+ kPracticeWristLowerLimit,
+ kPracticeWristUpperPhysicalLimit,
+ kPracticeWristLowerPhysicalLimit,
+ kWristZeroingSpeed,
+ kWristZeroingOffSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
+ kAngleAdjustZeroingOffSpeed,
+ kPracticeAngleAdjustDeadband,
+ kPracticeDrivetrainGearboxPinion,
+ kPracticeCameraCenter};
+ break;
+ default:
+ LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ }
+}
+
+} // namespace
+
+const Values &GetValues() {
+ static ::aos::Once<const Values> once(DoGetValues);
+ return *once.Get();
+}
+
+} // namespace constants
+} // namespace frc971
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
deleted file mode 100644
index 40f4e88..0000000
--- a/frc971/constants.cpp
+++ /dev/null
@@ -1,317 +0,0 @@
-#include "frc971/constants.h"
-
-#include <stddef.h>
-#include <math.h>
-
-#include "aos/common/inttypes.h"
-#include "aos/common/messages/RobotState.q.h"
-#include "aos/common/logging/logging.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-namespace frc971 {
-namespace constants {
-
-namespace {
-
-// It has about 0.029043 of gearbox slop.
-// For purposes of moving the end up/down by a certain amount, the wrist is 18
-// inches long.
-const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.175;
-
-const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
-const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
-
-const double kPracticeWristUpperPhysicalLimit = 1.677562;
-const double kCompWristUpperPhysicalLimit = 1.677562;
-
-const double kPracticeWristLowerPhysicalLimit = -0.746128;
-const double kCompWristLowerPhysicalLimit = -0.746128;
-
-const double kPracticeWristUpperLimit = 1.615385;
-const double kCompWristUpperLimit = 1.615385;
-
-const double kPracticeWristLowerLimit = -0.746128;
-const double kCompWristLowerLimit = -0.746128;
-
-const double kWristZeroingSpeed = 0.125;
-const double kWristZeroingOffSpeed = 0.35;
-
-const int kAngleAdjustHallEffect = 2;
-
-// Angle measured from CAD with the top of the angle adjust at the top of the
-// wire guide is 0.773652098 radians.
-
-const double kCompAngleAdjustHallEffectStartAngle[2] = {0.301170496, 1.5};
-const double kPracticeAngleAdjustHallEffectStartAngle[2] = {0.305172594, 1.5};
-
-const double kCompAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
-const double kPracticeAngleAdjustHallEffectStopAngle[2] = {0.1, 1.0};
-
-const double kPracticeAngleAdjustUpperPhysicalLimit = 0.904737;
-const double kCompAngleAdjustUpperPhysicalLimit = 0.904737;
-
-const double kPracticeAngleAdjustLowerPhysicalLimit = 0.270;
-const double kCompAngleAdjustLowerPhysicalLimit = 0.302;
-
-const double kPracticeAngleAdjustUpperLimit = 0.87;
-const double kCompAngleAdjustUpperLimit = 0.87;
-
-const double kPracticeAngleAdjustLowerLimit = 0.31;
-const double kCompAngleAdjustLowerLimit = 0.28;
-
-const double kAngleAdjustZeroingSpeed = -0.2;
-const double kAngleAdjustZeroingOffSpeed = -0.5;
-
-const double kPracticeAngleAdjustDeadband = 0.4;
-const double kCompAngleAdjustDeadband = 0.65;
-
-const int kCompCameraCenter = -2;
-const int kPracticeCameraCenter = -5;
-
-const int kCompDrivetrainGearboxPinion = 19;
-const int kPracticeDrivetrainGearboxPinion = 17;
-const int kBot3DrivetrainGearboxPinion = 15;
-
-struct Values {
- // Wrist hall effect positive and negative edges.
- double wrist_hall_effect_start_angle;
- double wrist_hall_effect_stop_angle;
-
- // Upper and lower extreme limits of travel for the wrist.
- double wrist_upper_limit;
- double wrist_lower_limit;
-
- // Physical limits. These are here for testing.
- double wrist_upper_physical_limit;
- double wrist_lower_physical_limit;
-
- // Zeroing speed.
- double wrist_zeroing_speed;
- // Zeroing off speed.
- double wrist_zeroing_off_speed;
-
- // AngleAdjust hall effect positive and negative edges.
- const double *angle_adjust_hall_effect_start_angle;
- const double *angle_adjust_hall_effect_stop_angle;
-
- // Upper and lower extreme limits of travel for the angle adjust.
- double angle_adjust_upper_limit;
- double angle_adjust_lower_limit;
- // Physical limits. These are here for testing.
- double angle_adjust_upper_physical_limit;
- double angle_adjust_lower_physical_limit;
-
- // Zeroing speed.
- double angle_adjust_zeroing_speed;
- // Zeroing off speed.
- double angle_adjust_zeroing_off_speed;
-
- // Deadband voltage.
- double angle_adjust_deadband;
-
- int drivetrain_gearbox_pinion;
-
- // what camera_center returns
- int camera_center;
-};
-
-Values *values = NULL;
-// Attempts to retrieve a new Values instance and stores it in values if
-// necessary.
-// Returns a valid Values instance or NULL.
-const Values *GetValues() {
- // TODO(brians): Make this use the new Once construct.
- if (values == NULL) {
- LOG(INFO, "creating a Constants for team %" PRIu16 "\n",
- ::aos::robot_state->team_id);
- switch (::aos::robot_state->team_id) {
- case kCompTeamNumber:
- values = new Values{kCompWristHallEffectStartAngle,
- kCompWristHallEffectStopAngle,
- kCompWristUpperLimit,
- kCompWristLowerLimit,
- kCompWristUpperPhysicalLimit,
- kCompWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kCompAngleAdjustHallEffectStartAngle,
- kCompAngleAdjustHallEffectStopAngle,
- kCompAngleAdjustUpperLimit,
- kCompAngleAdjustLowerLimit,
- kCompAngleAdjustUpperPhysicalLimit,
- kCompAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kCompAngleAdjustDeadband,
- kCompDrivetrainGearboxPinion,
- kCompCameraCenter};
- break;
- case kPracticeTeamNumber:
- values = new Values{kPracticeWristHallEffectStartAngle,
- kPracticeWristHallEffectStopAngle,
- kPracticeWristUpperLimit,
- kPracticeWristLowerLimit,
- kPracticeWristUpperPhysicalLimit,
- kPracticeWristLowerPhysicalLimit,
- kWristZeroingSpeed,
- kWristZeroingOffSpeed,
- kPracticeAngleAdjustHallEffectStartAngle,
- kPracticeAngleAdjustHallEffectStopAngle,
- kPracticeAngleAdjustUpperLimit,
- kPracticeAngleAdjustLowerLimit,
- kPracticeAngleAdjustUpperPhysicalLimit,
- kPracticeAngleAdjustLowerPhysicalLimit,
- kAngleAdjustZeroingSpeed,
- kAngleAdjustZeroingOffSpeed,
- kPracticeAngleAdjustDeadband,
- kPracticeDrivetrainGearboxPinion,
- kPracticeCameraCenter};
- break;
- default:
- LOG(ERROR, "unknown team #%" PRIu16 "\n",
- aos::robot_state->team_id);
- return NULL;
- }
- }
- return values;
-}
-
-} // namespace
-
-bool wrist_hall_effect_start_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_hall_effect_start_angle;
- return true;
-}
-bool wrist_hall_effect_stop_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_hall_effect_stop_angle;
- return true;
-}
-bool wrist_upper_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_upper_limit;
- return true;
-}
-
-bool wrist_lower_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_lower_limit;
- return true;
-}
-
-bool wrist_upper_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_upper_physical_limit;
- return true;
-}
-
-bool wrist_lower_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->wrist_lower_physical_limit;
- return true;
-}
-
-bool wrist_zeroing_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->wrist_zeroing_speed;
- return true;
-}
-
-bool wrist_zeroing_off_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->wrist_zeroing_off_speed;
- return true;
-}
-
-bool angle_adjust_hall_effect_start_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- angle[0] = values->angle_adjust_hall_effect_start_angle[0];
- angle[1] = values->angle_adjust_hall_effect_start_angle[1];
- return true;
-}
-bool angle_adjust_hall_effect_stop_angle(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- angle[0] = values->angle_adjust_hall_effect_stop_angle[0];
- angle[1] = values->angle_adjust_hall_effect_stop_angle[1];
- return true;
-}
-bool angle_adjust_upper_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->angle_adjust_upper_limit;
- return true;
-}
-
-bool angle_adjust_lower_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->angle_adjust_lower_limit;
- return true;
-}
-
-bool angle_adjust_upper_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->angle_adjust_upper_physical_limit;
- return true;
-}
-
-bool angle_adjust_lower_physical_limit(double *angle) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *angle = values->angle_adjust_lower_physical_limit;
- return true;
-}
-
-bool angle_adjust_zeroing_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->angle_adjust_zeroing_speed;
- return true;
-}
-
-bool angle_adjust_zeroing_off_speed(double *speed) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *speed = values->angle_adjust_zeroing_off_speed;
- return true;
-}
-
-bool angle_adjust_deadband(double *voltage) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *voltage = values->angle_adjust_deadband;
- return true;
-}
-
-bool drivetrain_gearbox_pinion(int *pinion) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *pinion = values->drivetrain_gearbox_pinion;
- return true;
-}
-
-bool camera_center(int *center) {
- const Values *const values = GetValues();
- if (values == NULL) return false;
- *center = values->camera_center;
- return true;
-}
-
-} // namespace constants
-} // namespace frc971
diff --git a/frc971/constants.h b/frc971/constants.h
index e530442..0c9bfdd 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -1,3 +1,6 @@
+#ifndef FRC971_CONSTANTS_H_
+#define FRC971_CONSTANTS_H_
+
#include <stdint.h>
namespace frc971 {
@@ -5,52 +8,64 @@
// Has all of the numbers that change for both robots and makes it easy to
// retrieve the values for the current one.
-//
-// All of the public functions to retrieve various values take a pointer to
-// store their output value into and assume that aos::robot_state->get() is
-// not null and is correct. They return true on success.
const uint16_t kCompTeamNumber = 5971;
const uint16_t kPracticeTeamNumber = 971;
-const uint16_t kBot3TeamNumber = 9971;
-// Sets *angle to how many radians from horizontal to the location of interest.
-bool wrist_hall_effect_start_angle(double *angle);
-bool wrist_hall_effect_stop_angle(double *angle);
-// These are the soft stops for up and down.
-bool wrist_lower_limit(double *angle);
-bool wrist_upper_limit(double *angle);
-// These are the hard stops. Don't use these for anything but testing.
-bool wrist_lower_physical_limit(double *angle);
-bool wrist_upper_physical_limit(double *angle);
+// This structure contains current values for all of the things that change.
+struct Values {
+ // Wrist hall effect positive and negative edges.
+ // How many radians from horizontal to the location of interest.
+ double wrist_hall_effect_start_angle;
+ double wrist_hall_effect_stop_angle;
-// Returns the speed to move the wrist at when zeroing in rad/sec
-bool wrist_zeroing_speed(double *speed);
-bool wrist_zeroing_off_speed(double *speed);
+ // Upper and lower extreme limits of travel for the wrist.
+ // These are the soft stops for up and down.
+ double wrist_upper_limit;
+ double wrist_lower_limit;
-bool angle_adjust_hall_effect_start_angle(double *angle);
-bool angle_adjust_hall_effect_stop_angle(double *angle);
-// These are the soft stops for up and down.
-bool angle_adjust_lower_limit(double *angle);
-bool angle_adjust_upper_limit(double *angle);
-// These are the hard stops. Don't use these for anything but testing.
-bool angle_adjust_lower_physical_limit(double *angle);
-bool angle_adjust_upper_physical_limit(double *angle);
+ // Physical limits. These are here for testing.
+ double wrist_upper_physical_limit;
+ double wrist_lower_physical_limit;
-// Returns speed to move the angle adjust when zeroing, in rad/sec
-bool angle_adjust_zeroing_speed(double *speed);
-bool angle_adjust_zeroing_off_speed(double *speed);
+ // Zeroing speed.
+ // The speed to move the wrist at when zeroing in rad/sec
+ double wrist_zeroing_speed;
+ // Zeroing off speed (in rad/sec).
+ double wrist_zeroing_off_speed;
-// Returns the deadband voltage to use.
-bool angle_adjust_deadband(double *voltage);
+ // AngleAdjust hall effect positive and negative edges.
+ // These are the soft stops for up and down.
+ const double (&angle_adjust_hall_effect_start_angle)[2];
+ const double (&angle_adjust_hall_effect_stop_angle)[2];
-// Sets *pinion to the number of teeth on the pinion that drives the drivetrain
-// wheels.
-bool drivetrain_gearbox_pinion(int *pinion);
+ // Upper and lower extreme limits of travel for the angle adjust.
+ double angle_adjust_upper_limit;
+ double angle_adjust_lower_limit;
+ // Physical limits. These are here for testing.
+ double angle_adjust_upper_physical_limit;
+ double angle_adjust_lower_physical_limit;
-// Sets *center to how many pixels off center the vertical line
-// on the camera view is.
-bool camera_center(int *center);
+ // The speed to move the angle adjust when zeroing, in rad/sec
+ double angle_adjust_zeroing_speed;
+ // Zeroing off speed.
+ double angle_adjust_zeroing_off_speed;
+
+ // Deadband voltage.
+ double angle_adjust_deadband;
+
+ // The number of teeth on the pinion that drives the drivetrain wheels.
+ int drivetrain_gearbox_pinion;
+
+ // How many pixels off center the vertical line
+ // on the camera view is.
+ int camera_center;
+};
+
+// Creates (once) a Values instance and returns a reference to it.
+const Values &GetValues();
} // namespace constants
} // namespace frc971
+
+#endif // FRC971_CONSTANTS_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index 3ecafa6..ffb98f2 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -2,7 +2,6 @@
#include <algorithm>
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -17,43 +16,23 @@
: aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
my_angle_adjust),
zeroed_joint_(MakeAngleAdjustLoop()) {
-}
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<2>::ConfigurationData config_data;
-bool AngleAdjustMotor::FetchConstants(
- ZeroedJoint<2>::ConfigurationData *config_data) {
- if (!constants::angle_adjust_lower_limit(
- &config_data->lower_limit)) {
- LOG(ERROR, "Failed to fetch the angle adjust lower limit constant.\n");
- return false;
- }
- if (!constants::angle_adjust_upper_limit(
- &config_data->upper_limit)) {
- LOG(ERROR, "Failed to fetch the angle adjust upper limit constant.\n");
- return false;
- }
- if (!constants::angle_adjust_hall_effect_start_angle(
- config_data->hall_effect_start_angle)) {
- LOG(ERROR, "Failed to fetch the hall effect start angle constants.\n");
- return false;
- }
- if (!constants::angle_adjust_zeroing_off_speed(
- &config_data->zeroing_off_speed)) {
- LOG(ERROR,
- "Failed to fetch the angle adjust zeroing off speed constant.\n");
- return false;
- }
- if (!constants::angle_adjust_zeroing_speed(
- &config_data->zeroing_speed)) {
- LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
- return false;
- }
+ config_data.lower_limit = GetValues().angle_adjust_lower_limit;
+ config_data.upper_limit = GetValues().angle_adjust_upper_limit;
+ memcpy(config_data.hall_effect_start_angle,
+ GetValues().angle_adjust_hall_effect_start_angle,
+ sizeof(config_data.hall_effect_start_angle));
+ config_data.zeroing_off_speed = GetValues().angle_adjust_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().angle_adjust_zeroing_speed;
- config_data->max_zeroing_voltage = 4.0;
- if (!constants::angle_adjust_deadband(&config_data->deadband_voltage)) {
- LOG(ERROR, "Failed to fetch the angle adjust deadband voltage constant.\n");
- return false;
+ config_data.max_zeroing_voltage = 4.0;
+ config_data.deadband_voltage = GetValues().angle_adjust_deadband;
+
+ zeroed_joint_.set_config_data(config_data);
}
- return true;
}
// Positive angle is up, and positive power is up.
@@ -69,15 +48,6 @@
output->voltage = 0;
}
- // Cache the constants to avoid error handling down below.
- ZeroedJoint<2>::ConfigurationData config_data;
- if (!FetchConstants(&config_data)) {
- LOG(WARNING, "Failed to fetch constants.\n");
- return;
- } else {
- zeroed_joint_.set_config_data(config_data);
- }
-
ZeroedJoint<2>::PositionData transformed_position;
ZeroedJoint<2>::PositionData *transformed_position_ptr =
&transformed_position;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.gyp b/frc971/control_loops/angle_adjust/angle_adjust.gyp
index c48ded3..323cb56 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.gyp
+++ b/frc971/control_loops/angle_adjust/angle_adjust.gyp
@@ -25,7 +25,7 @@
],
'dependencies': [
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'angle_adjust_loop',
],
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
index dfa8ad2..319e293 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -52,10 +52,6 @@
friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
friend class testing::AngleAdjustTest_DisableGoesUninitialized_Test;
- // Fetches and locally caches the latest set of constants.
- // Returns whether it succeeded or not.
- bool FetchConstants(ZeroedJoint<2>::ConfigurationData *config_data);
-
// The zeroed joint to use.
ZeroedJoint<2> zeroed_joint_;
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 51cd641..0ea7201 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -62,17 +62,15 @@
void SendPositionMessage() {
const double angle = GetPosition();
- double hall_effect_start_angle[2];
- ASSERT_TRUE(constants::angle_adjust_hall_effect_start_angle(
- hall_effect_start_angle));
- double hall_effect_stop_angle[2];
- ASSERT_TRUE(constants::angle_adjust_hall_effect_stop_angle(
- hall_effect_stop_angle));
-
::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
my_angle_adjust_loop_.position.MakeMessage();
position->angle = angle;
+ const double (&hall_effect_start_angle)[2] =
+ constants::GetValues().angle_adjust_hall_effect_start_angle;
+ const double (&hall_effect_stop_angle)[2] =
+ constants::GetValues().angle_adjust_hall_effect_stop_angle;
+
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
@@ -115,16 +113,10 @@
angle_adjust_plant_->U << last_voltage_;
angle_adjust_plant_->Update();
- // Assert that we are in the right physical range.
- double upper_physical_limit;
- ASSERT_TRUE(constants::angle_adjust_upper_physical_limit(
- &upper_physical_limit));
- double lower_physical_limit;
- ASSERT_TRUE(constants::angle_adjust_lower_physical_limit(
- &lower_physical_limit));
-
- EXPECT_GE(upper_physical_limit, angle_adjust_plant_->Y(0, 0));
- EXPECT_LE(lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_GE(constants::GetValues().angle_adjust_upper_physical_limit,
+ angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(constants::GetValues().angle_adjust_lower_physical_limit,
+ angle_adjust_plant_->Y(0, 0));
last_voltage_ = my_angle_adjust_loop_.output->voltage;
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 8e98e4b..1776056 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'drivetrain_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
],
diff --git a/frc971/control_loops/index/index.gyp b/frc971/control_loops/index/index.gyp
index 9cfb019..014f266 100644
--- a/frc971/control_loops/index/index.gyp
+++ b/frc971/control_loops/index/index.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'index_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
],
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index 41c33c1..64e3a20 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -27,7 +27,7 @@
'dependencies': [
'shooter_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
],
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 0ca5caa..8229c93 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -4,7 +4,6 @@
#include <algorithm>
-#include "aos/common/messages/RobotState.q.h"
#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"
@@ -17,42 +16,22 @@
WristMotor::WristMotor(control_loops::WristLoop *my_wrist)
: aos::control_loops::ControlLoop<control_loops::WristLoop>(my_wrist),
zeroed_joint_(MakeWristLoop()) {
-}
+ {
+ using ::frc971::constants::GetValues;
+ ZeroedJoint<1>::ConfigurationData config_data;
-bool WristMotor::FetchConstants(
- ZeroedJoint<1>::ConfigurationData *config_data) {
- if (::aos::robot_state.get() == NULL) {
- if (!::aos::robot_state.FetchNext()) {
- LOG(ERROR, "Failed to fetch robot state to get constants.\n");
- return false;
- }
- }
- if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
- LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
- return false;
- }
- if (!constants::wrist_upper_limit(&config_data->upper_limit)) {
- LOG(ERROR, "Failed to fetch the wrist upper limit constant.\n");
- return false;
- }
- if (!constants::wrist_hall_effect_start_angle(
- &config_data->hall_effect_start_angle[0])) {
- LOG(ERROR, "Failed to fetch the wrist start angle constant.\n");
- return false;
- }
- if (!constants::wrist_zeroing_off_speed(&config_data->zeroing_off_speed)) {
- LOG(ERROR, "Failed to fetch the wrist zeroing off speed constant.\n");
- return false;
- }
+ config_data.lower_limit = GetValues().wrist_lower_limit;
+ config_data.upper_limit = GetValues().wrist_upper_limit;
+ config_data.hall_effect_start_angle[0] =
+ GetValues().wrist_hall_effect_start_angle;
+ config_data.zeroing_off_speed = GetValues().wrist_zeroing_off_speed;
+ config_data.zeroing_speed = GetValues().wrist_zeroing_speed;
- if (!constants::wrist_zeroing_speed(&config_data->zeroing_speed)) {
- LOG(ERROR, "Failed to fetch the wrist zeroing speed constant.\n");
- return false;
- }
+ config_data.max_zeroing_voltage = 5.0;
+ config_data.deadband_voltage = 0.0;
- config_data->max_zeroing_voltage = 5.0;
- config_data->deadband_voltage = 0.0;
- return true;
+ zeroed_joint_.set_config_data(config_data);
+ }
}
// Positive angle is up, and positive power is up.
@@ -68,14 +47,6 @@
output->voltage = 0;
}
- // Cache the constants to avoid error handling down below.
- ZeroedJoint<1>::ConfigurationData config_data;
- if (!FetchConstants(&config_data)) {
- return;
- } else {
- zeroed_joint_.set_config_data(config_data);
- }
-
ZeroedJoint<1>::PositionData transformed_position;
ZeroedJoint<1>::PositionData *transformed_position_ptr =
&transformed_position;
diff --git a/frc971/control_loops/wrist/wrist.gyp b/frc971/control_loops/wrist/wrist.gyp
index 3beee8b..d66bf6c 100644
--- a/frc971/control_loops/wrist/wrist.gyp
+++ b/frc971/control_loops/wrist/wrist.gyp
@@ -28,7 +28,7 @@
'dependencies': [
'wrist_loop',
'<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
],
'export_dependent_settings': [
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index fe2601b..a3bad80 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -51,9 +51,6 @@
friend class testing::WristTest_NoWindupPositive_Test;
friend class testing::WristTest_NoWindupNegative_Test;
- // Fetches and locally caches the latest set of constants.
- bool FetchConstants(ZeroedJoint<1>::ConfigurationData *config_data);
-
// The zeroed joint to use.
ZeroedJoint<1> zeroed_joint_;
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index 4903a28..7d248ce 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -58,13 +58,6 @@
void SendPositionMessage() {
const double angle = GetPosition();
- double wrist_hall_effect_start_angle;
- ASSERT_TRUE(constants::wrist_hall_effect_start_angle(
- &wrist_hall_effect_start_angle));
- double wrist_hall_effect_stop_angle;
- ASSERT_TRUE(constants::wrist_hall_effect_stop_angle(
- &wrist_hall_effect_stop_angle));
-
::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
my_wrist_loop_.position.MakeMessage();
position->pos = angle;
@@ -72,8 +65,8 @@
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
- if (abs_position >= wrist_hall_effect_start_angle &&
- abs_position <= wrist_hall_effect_stop_angle) {
+ if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
+ abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
position->hall_effect = true;
} else {
position->hall_effect = false;
@@ -81,11 +74,14 @@
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ < wrist_hall_effect_start_angle ||
- last_position_ > wrist_hall_effect_stop_angle) &&
- position->hall_effect) {
+ if ((last_position_ <
+ constants::GetValues().wrist_hall_effect_start_angle ||
+ last_position_ >
+ constants::GetValues().wrist_hall_effect_stop_angle) &&
+ position->hall_effect) {
calibration_value_ =
- wrist_hall_effect_start_angle - initial_position_;
+ constants::GetValues().wrist_hall_effect_start_angle -
+ initial_position_;
}
position->calibration = calibration_value_;
@@ -99,17 +95,9 @@
wrist_plant_->U << last_voltage_;
wrist_plant_->Update();
- // Assert that we are in the right physical range.
- double wrist_upper_physical_limit;
- ASSERT_TRUE(constants::wrist_upper_physical_limit(
- &wrist_upper_physical_limit));
- double wrist_lower_physical_limit;
- ASSERT_TRUE(constants::wrist_lower_physical_limit(
- &wrist_lower_physical_limit));
-
- EXPECT_GE(wrist_upper_physical_limit,
+ EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
wrist_plant_->Y(0, 0));
- EXPECT_LE(wrist_lower_physical_limit,
+ EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
wrist_plant_->Y(0, 0));
last_voltage_ = my_wrist_loop_.output->voltage;
}
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index 597abee..6d5202c 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -1,14 +1,15 @@
{
'targets': [
{
- 'target_name': 'common',
+ 'target_name': 'constants',
'type': 'static_library',
'sources': [
- 'constants.cpp',
+ 'constants.cc',
],
'dependencies': [
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/common.gyp:once',
+ '<(AOS)/common/network/network.gyp:team_number',
],
}
],
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 42b50c8..411fb5e 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -1,3 +1,5 @@
+#include <inttypes.h>
+
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
#include "aos/common/util/wrapping_counter.h"
@@ -53,22 +55,33 @@
(1.0) /*gears*/ * (2 * M_PI);
}
+// Translates values from the ADC into voltage.
+inline double adc_translate(uint16_t in) {
+ static const double kVRefN = 0;
+ static const double kVRefP = 3.3;
+ static const int kMaximumValue = 0x3FF;
+ return kVRefN +
+ (static_cast<double>(in) / static_cast<double>(kMaximumValue) *
+ (kVRefP - kVRefN));
+}
+
+inline double gyro_translate(int64_t in) {
+ return in / 16.0 / 1000.0 / (180.0 / M_PI);
+}
+
+inline double battery_translate(uint16_t in) {
+ return adc_translate(in) * 80.8 / 17.8;
+}
+
} // namespace
class GyroSensorReceiver : public USBReceiver {
- virtual void ProcessData() override {
- if (data()->robot_id != 0) {
- LOG(ERROR, "gyro board sent data for robot id %hhd!"
- " dip switches are %x\n",
- data()->robot_id, data()->base_status & 0xF);
- return;
- } else {
- LOG(DEBUG, "processing a packet dip switches %x\n",
- data()->base_status & 0xF);
- }
+ public:
+ GyroSensorReceiver() : USBReceiver(2) {}
+ virtual void ProcessData(const ::aos::time::Time &/*timestamp*/) override {
gyro.MakeWithBuilder()
- .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .angle(gyro_translate(data()->gyro_angle))
.Send();
drivetrain.position.MakeWithBuilder()
@@ -117,6 +130,14 @@
.loader_top(data()->main.loader_top)
.loader_bottom(data()->main.loader_bottom)
.Send();
+
+ LOG(DEBUG, "battery %f %f %" PRIu16 "\n",
+ battery_translate(data()->main.battery_voltage),
+ adc_translate(data()->main.battery_voltage),
+ data()->main.battery_voltage);
+ LOG(DEBUG, "halls l=%f r=%f\n",
+ adc_translate(data()->main.left_drive_hall),
+ adc_translate(data()->main.right_drive_hall));
}
WrappingCounter top_rise_;
@@ -129,7 +150,7 @@
} // namespace frc971
int main() {
- ::aos::Init();
+ ::aos::Init(frc971::USBReceiver::kRelativePriority);
::frc971::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index d3e25a1..9a26bce 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -49,13 +49,11 @@
'dependencies': [
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/util/util.gyp:wrapping_counter',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:controls',
],
'export_dependent_settings': [
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(AOS)/common/util/util.gyp:wrapping_counter',
'<(AOS)/common/common.gyp:time',
],
},
diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
index 545e197..b70ff61 100644
--- a/frc971/input/usb_receiver.cc
+++ b/frc971/input/usb_receiver.cc
@@ -9,7 +9,8 @@
namespace frc971 {
-USBReceiver::USBReceiver() {
+USBReceiver::USBReceiver(uint8_t expected_robot_id)
+ : expected_robot_id_(expected_robot_id) {
Reset();
}
@@ -17,10 +18,25 @@
if (ReceiveData()) {
Reset();
} else {
- const ::aos::time::Time received_time = ::aos::time::Time::Now();
- if (phase_locker_.IsCurrentPacketGood(received_time, sequence_.count())) {
- LOG(DEBUG, "processing data\n");
- ProcessData();
+ if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
+ static const int kCountsPerSecond = 100000;
+ const ::aos::time::Time timestamp =
+ ::aos::time::Time(data()->timestamp / kCountsPerSecond,
+ (data()->timestamp * ::aos::time::Time::kNSecInSec /
+ kCountsPerSecond) %
+ ::aos::time::Time::kNSecInSec);
+
+ if (data()->robot_id != expected_robot_id_) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
+ " dip switches are %hhx\n",
+ data()->robot_id, expected_robot_id_, data()->dip_switches);
+ return;
+ } else {
+ LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
+ data()->dip_switches, data()->frame_number, timestamp.ToSeconds());
+ }
+
+ ProcessData(timestamp);
}
}
}
@@ -28,7 +44,7 @@
void USBReceiver::PhaseLocker::Reset() {
LOG(INFO, "resetting\n");
last_good_packet_time_ = ::aos::time::Time(0, 0);
- last_good_sequence_ = -1;
+ last_good_sequence_ = 0;
good_phase_ = guess_phase_ = kUnknownPhase;
guess_phase_good_ = guess_phase_bad_ = 0;
good_phase_early_ = good_phase_late_ = 0;
@@ -36,14 +52,14 @@
bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
const ::aos::time::Time &received_time,
- int32_t sequence) {
+ uint32_t sequence) {
if (last_good_packet_time_ != ::aos::time::Time(0, 0) &&
received_time - last_good_packet_time_ > kResetTime) {
LOG(WARNING, "no good packet received in too long\n");
Reset();
return false;
}
- if (last_good_sequence_ != -1 && sequence - last_good_sequence_ > 100) {
+ if (last_good_sequence_ != 0 && sequence - last_good_sequence_ > 100) {
LOG(WARNING, "skipped too many packets\n");
Reset();
return false;
@@ -158,6 +174,7 @@
}
void USBReceiver::TransferCallback(libusb::Transfer *transfer) {
+ transfer_received_time_ = ::aos::time::Time::Now();
if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
LOG(DEBUG, "transfer %p completed\n", transfer);
completed_transfer_ = transfer;
@@ -194,14 +211,27 @@
memcpy(data(), completed_transfer_->data(),
sizeof(GyroBoardData));
- int32_t count_before = sequence_.count();
- sequence_.Update(data()->sequence);
- if (count_before == 0) {
- LOG(INFO, "count starting at %" PRId32 "\n", sequence_.count());
- } else if (sequence_.count() - count_before != 1) {
- LOG(WARNING, "count went from %" PRId32" to %" PRId32 "\n",
- count_before, sequence_.count());
+ if (data()->unknown_frame) {
+ LOG(WARNING, "unknown frame number\n");
+ return true;
}
+ uint32_t frame_number_before = frame_number_;
+ frame_number_ = data()->frame_number;
+ if (frame_number_ < 0) {
+ LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
+ return true;
+ }
+ if (frame_number_before == 0) {
+ LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
+ } else if (frame_number_ - frame_number_before != 1) {
+ LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
+ frame_number_before, frame_number_);
+ }
+ if (frame_number_ < last_frame_number_) {
+ LOG(WARNING, "frame number went down\n");
+ return true;
+ }
+ last_frame_number_ = frame_number_;
return false;
}
@@ -225,7 +255,7 @@
c->Submit();
}
- sequence_.Reset();
+ last_frame_number_ = frame_number_ = 0;
phase_locker_.Reset();
}
diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
index cda7843..509b740 100644
--- a/frc971/input/usb_receiver.h
+++ b/frc971/input/usb_receiver.h
@@ -4,7 +4,7 @@
#include <memory>
#include "aos/common/time.h"
-#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/macros.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/input/gyro_board_data.h"
@@ -15,10 +15,14 @@
// us.
class USBReceiver {
public:
- USBReceiver();
+ USBReceiver(uint8_t expected_robot_id);
void RunIteration();
+ // The relative priority that tasks doing this should get run at (ie what to
+ // pass to ::aos::Init(int)).
+ static const int kRelativePriority = 5;
+
protected:
GyroBoardData *data() { return &data_; }
@@ -62,7 +66,7 @@
// Gets called for every packet received.
// Returns whether or not to process the values from this packet.
bool IsCurrentPacketGood(const ::aos::time::Time &received_time,
- int32_t sequence);
+ uint32_t sequence);
private:
// How many times the packet we guessed has to be close to right to use the
@@ -78,7 +82,7 @@
::aos::time::Time last_good_packet_time_{0, 0};
- int32_t last_good_sequence_;
+ uint32_t last_good_sequence_;
const int kUnknownPhase = -11;
// kUnknownPhase or the sequence number (%kPacketsPerLoopCycle) to
@@ -98,18 +102,24 @@
void Reset();
- virtual void ProcessData() = 0;
+ virtual void ProcessData(const ::aos::time::Time ×tamp) = 0;
+
+ const uint8_t expected_robot_id_;
GyroBoardData data_;
- ::aos::util::WrappingCounter sequence_;
+ int32_t last_frame_number_, frame_number_;
LibUSB libusb_;
::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
::std::unique_ptr<libusb::IsochronousTransfer> transfers_[kNumTransfers];
- // "Temporary" variable for holding a completed transfer to communicate that
- // information from the callback to the code that wants it.
+
+ // "Temporary" variables for communicating information about a transfer that
+ // finished from the callback to the rest of the code.
libusb::Transfer *completed_transfer_;
+ ::aos::time::Time transfer_received_time_{0, 0};
+
+ DISALLOW_COPY_AND_ASSIGN(USBReceiver);
};
} // namespace frc971
diff --git a/frc971/output/CameraServer.cc b/frc971/output/CameraServer.cc
index 939731d..93a83c7 100644
--- a/frc971/output/CameraServer.cc
+++ b/frc971/output/CameraServer.cc
@@ -3,7 +3,6 @@
#include "aos/atom_code/output/HTTPServer.h"
#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
#include "aos/atom_code/output/ctemplate_cache.h"
-#include "aos/common/messages/RobotState.q.h"
#include "ctemplate/template.h"
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
@@ -55,17 +54,7 @@
// after it.
dict.SetValue("HOST", ctemplate::TemplateString(host, length));
- if (!aos::robot_state.FetchLatest()) {
- LOG(WARNING, "getting a RobotState message failed\n");
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- int center;
- if (!constants::camera_center(¢er)) {
- evhttp_send_error(request, HTTP_INTERNAL, NULL);
- return;
- }
- dict.SetIntValue("CENTER", center);
+ dict.SetIntValue("CENTER", constants::GetValues().camera_center);
aos::http::EvhttpCtemplateEmitter emitter(buf_);
if (!aos::http::get_template_cache()->
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index 68b3630..01cbf84 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -8,10 +8,9 @@
],
'dependencies': [
'<(AOS)/atom_code/output/output.gyp:http_server',
- '../frc971.gyp:common',
+ '../frc971.gyp:constants',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/atom_code/atom_code.gyp:configuration',
],
'copies': [
diff --git a/gyro_board/src/usb/CAN.c b/gyro_board/src/usb/CAN.c
index 591c6fc..35f811f 100644
--- a/gyro_board/src/usb/CAN.c
+++ b/gyro_board/src/usb/CAN.c
@@ -231,6 +231,10 @@
vTaskDelete(NULL);
}
+ // Set all of the CAN stuff to run at CCLK/6, which translates to about 1 Mbit
+ // (1.042).
+ SC->PCLKSEL0 |= 3 << 26 | 3 << 28 | 3 << 30;
+
// Enable CAN
SC->PCONP |= PCONP_PCCAN1;
diff --git a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
index 2fa038e..e39a636 100644
--- a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
+++ b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
@@ -59,7 +59,7 @@
@param [in] dwIntr Bitmask of interrupts to wait for
*/
-static void Wait4DevInt(unsigned long dwIntr)
+void Wait4DevInt(unsigned long dwIntr)
{
// wait for specific interrupt
while ((USB->USBDevIntSt & dwIntr) != dwIntr);
@@ -107,7 +107,7 @@
@return the data
*/
-static unsigned char USBHwCmdRead(unsigned char bCmd)
+unsigned char USBHwCmdRead(unsigned char bCmd)
{
// write command code
USBHwCmd(bCmd);
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index 2dc2286..da8a4c5 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -44,7 +44,7 @@
CAN.c \
LPCUSB/usbinit.c \
LPCUSB/usbcontrol.c \
- LPCUSB/USB_SENSOR_STREAM.c \
+ usb_device.c \
LPCUSB/usbhw_lpc.c \
gyro.c \
LPCUSB/usbstdreq.c
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 87d67ad..b2ab48f 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -1,50 +1,39 @@
-// ****************************************************************************
-// CopyLeft qwerk Robotics unINC. 2010 All Rights Reserved.
-// ****************************************************************************
-
#include "analog.h"
-#include "FreeRTOS.h"
-#include "queue.h"
-#include "task.h"
+#include "LPC17xx.h"
void analog_init(void) {
- // b[1:0] CAN RD1 p0.0
- // b[3:2] CAN TD1 p0.1
- //PINCON->PINSEL0 = 0x00000005;
-
- // b[29:28] USB_DMIN p0.30
- // b[27:26] USB_DPLUS p0.29
- // b[21:20] AD0.3 p0.26
- // b[19:18] AD0.2 p0.25
- // PINCON->PINSEL1 = 0x14140000;
-
- // PINCON->PINSEL2 = 0x0;
-
- // b[31:30] AD0.5 p1.31
- // b[29:28] V_BUS p1.30
- // b[21:20] MCOB1 p1.26
- // b[19:18] MCOA1 p1.25
- // b[15:14] MCI1 p1.23
- // b[13:12] MCOB0 p1.22
- // b[09:08] MCI0 p1.20
- // b[07:06] MCOA0 p1.19
- // b[05:04] USB_UP_LED p1.18
- //PINCON->PINSEL3 = 0xE0145150;
SC->PCONP |= PCONP_PCAD;
// Enable AD0.0, AD0.1, AD0.2, AD0.3
- PINCON->PINSEL1 &= 0xFFC03FFF;
- PINCON->PINSEL1 |= 0x00D54000;
- ADC->ADCR = 0x00200500;
+ PINCON->PINSEL1 &= ~(3 << 14 | 3 << 16 | 3 << 18 | 3 << 20);
+ PINCON->PINSEL1 |= 1 << 14 | 1 << 16 | 1 << 18 | 1 << 20;
+ ADC->ADCR = (1 << 0 | 1 << 1 | 1 << 2 | 1 << 3) /* enable all 4 */ |
+ 7 << 8 /* 100MHz / 8 = 12.5MHz */ |
+ 1 << 16 /* enable burst mode */ |
+ 1 << 21 /* turn on ADC */;
}
+uint16_t analog(int channel) {
+ uint32_t value;
+ do {
+ switch (channel) {
+ case 0:
+ value = ADC->ADDR0;
+ break;
+ case 1:
+ value = ADC->ADDR1;
+ break;
+ case 2:
+ value = ADC->ADDR2;
+ break;
+ case 3:
+ value = ADC->ADDR3;
+ break;
+ default:
+ return 0xFFFF;
+ }
+ } while (!(value & 1 << 31));
-int analog(int channel) {
- ADC->ADCR = ((ADC->ADCR & 0xF8FFFF00) | (0x01000000 | (1 << channel)));
-
- // Poll until it is done.
- while(!(ADC->ADGDR & 0x80000000));
-
- return ((ADC->ADGDR & 0x0000FFF0) >> 4);
+ return (value >> 4) & 0x3FF;
}
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index 1de38e7..5a06290 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -3,7 +3,13 @@
#include <stdint.h>
+// Starts the hardware constantly doing conversions on all 4 of our analog
+// inputs.
void analog_init(void);
-int analog(int channel);
+
+// Retrieves the most recent reading on channel (0-3).
+// Returns 0xFFFF for invalid channel.
+// 0 means 0V and 0x3FF means 3.3V.
+uint16_t analog(int channel);
#endif // __ANALOG_H__
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index e5b8e95..317b0f5 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -3,6 +3,7 @@
// guards.
// This means that it can not #include anything else because it (sometimes) gets
// #included inside a namespace.
+// <stdint.h> must be #included by the containing file.
// In the gyro board code, fill_packet.h #includes this file.
// In the fitpc code, frc971/input/gyro_board_data.h #includes this file.
@@ -12,14 +13,31 @@
struct DATA_STRUCT_NAME {
int64_t gyro_angle;
+ // In units of 100,000 counts/second.
+ uint64_t timestamp;
+
union {
struct {
+ // This is the USB frame number for this data. It gets incremented on
+ // every packet sent.
+ // Negative numbers mean that the gyro board has no idea what the right
+ // answer is.
+ // This value going down at all indicates that the code on the gyro board
+ // dealing with it reset.
+ int32_t frame_number;
+
// Which robot (+version) the gyro board is sending out data for.
// We should keep this in the same place for all gyro board software
// versions so that the fitpc can detect when it's reading from a gyro
- // board set up for a different robot than it is.
- // 0 = 2013 competition/practice robot
- // 1 = 2013 3rd robot
+ // board set up for a different robot (or version) than it is.
+ // The numbers listed below each robot are the values that have been used
+ // for it.
+ //
+ // 2013 competition/practice robot
+ // 0
+ // 2 added battery measurement + drivetrain analog hall effects
+ // 2013 3rd robot
+ // 1
uint8_t robot_id;
// This information should also be kept in the same place from year to
// year so that the fitpc code can record the dip switch values when it
@@ -30,35 +48,27 @@
uint8_t dip_switch1 : 1;
uint8_t dip_switch2 : 1;
uint8_t dip_switch3 : 1;
- // If the current gyro_angle has been not updated because of a bad
- // reading from the sensor.
- uint8_t old_gyro_reading : 1;
- // If we're not going to get any more good gyro_angles.
- uint8_t bad_gyro : 1;
};
- uint8_t base_status;
+ uint8_t dip_switches;
+ };
+ struct {
+ // If the current gyro_angle has been not updated because of a bad
+ // reading from the sensor.
+ uint8_t old_gyro_reading : 1;
+ // If we're not going to get any more good gyro_angles.
+ uint8_t bad_gyro : 1;
+
+ // We're not sure what frame number this packet was sent in.
+ uint8_t unknown_frame : 1;
};
};
- uint16_t header;
+ uint64_t header;
};
- // This is a counter that gets incremented with each packet sent (and wraps
- // around when it reaches 255).
- uint8_t sequence;
+ // We are 64-bit aligned at this point.
union {
struct {
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- uint8_t loader_top : 1;
- uint8_t loader_bottom : 1;
- };
- uint16_t booleans;
- };
int32_t left_drive;
int32_t right_drive;
int32_t shooter_angle;
@@ -72,6 +82,10 @@
int32_t capture_wrist_rise;
int32_t capture_shooter_angle_rise;
+ uint16_t battery_voltage;
+ uint16_t left_drive_hall;
+ uint16_t right_drive_hall;
+
int8_t top_rise_count;
int8_t top_fall_count;
@@ -84,6 +98,15 @@
int8_t wrist_rise_count;
int8_t shooter_angle_rise_count;
+
+ struct {
+ uint8_t wrist_hall_effect : 1;
+ uint8_t angle_adjust_bottom_hall_effect : 1;
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
+ };
} main;
struct {
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
index c468173..83ef169 100644
--- a/gyro_board/src/usb/digital.c
+++ b/gyro_board/src/usb/digital.c
@@ -30,5 +30,9 @@
int is_bot3;
void digital_init(void) {
- is_bot3 = 0;
+ if (dip_switch(0) || dip_switch(1) || dip_switch(2) || dip_switch(3)) {
+ is_bot3 = 1;
+ } else {
+ is_bot3 = 0;
+ }
}
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index 17903cf..82f1b52 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -17,6 +17,15 @@
// before said wheel is deemed "stopped". (In secs)
static const uint8_t kWheelStopThreshold = 1;
+// The timer to use for timestamping sensor readings.
+// This is a constant to avoid hard-coding it in a lot of places, but there ARE
+// things (PCONP bits, IRQ numbers, etc) that have this value in them
+// implicitly.
+#define SENSOR_TIMING_TIMER TIM1
+// How many counts per second SENSOR_TIMING_TIMER should be.
+// This will wrap the counter about every 1/3 of a second.
+static const int kSensorTimingRate = 100000;
+
#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
int encoder_bits(int channel) {
switch (channel) {
@@ -437,7 +446,26 @@
}
}
+static volatile uint32_t sensor_timing_wraps = 0;
+
+void TIMER1_IRQHandler(void) {
+ SENSOR_TIMING_TIMER->IR = 1 << 0; // clear channel 0 match
+ ++sensor_timing_wraps;
+}
+
void encoder_init(void) {
+ // Set up the timer for timestamping sensor readings.
+ SC->PCONP |= 1 << 2;
+ SENSOR_TIMING_TIMER->PR = (configCPU_CLOCK_HZ / kSensorTimingRate) - 1UL;
+ SENSOR_TIMING_TIMER->TC = 1; // don't match the first time around
+ SENSOR_TIMING_TIMER->MR0 = 0; // match every time it wraps
+ SENSOR_TIMING_TIMER->MCR = 1 << 0; // interrupt on match channel 0
+ // Priority 4 is higher than any FreeRTOS-managed stuff (ie USB), but lower
+ // than encoders etc.
+ NVIC_SetPriority(TIMER1_IRQn, 4);
+ NVIC_EnableIRQ(TIMER1_IRQn);
+ SENSOR_TIMING_TIMER->TCR = 1; // enable it
+
// Setup the encoder interface.
SC->PCONP |= PCONP_PCQEI;
PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
@@ -447,26 +475,26 @@
// Wrap back to 0 when we wrap the int and vice versa.
QEI->QEIMAXPOS = 0xFFFFFFFF;
- // Set up encoder 4.
- GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ // Set up encoder 2.
+ GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
// Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
- encoder4_val = 0;
+ PINCON->PINSEL1 &= ~(0x3 << 12);
+ PINCON->PINSEL1 &= ~(0x3 << 10);
+ encoder2_val = 0;
- // Set up encoder 5.
- GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
+ // Set up encoder 3.
+ GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
// Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
- encoder5_val = 0;
-
+ PINCON->PINSEL1 &= ~(0x3 << 8);
+ PINCON->PINSEL1 &= ~(0x3 << 6);
+ encoder3_val = 0;
+
// Enable interrupts from the GPIO pins.
NVIC_EnableIRQ(EINT3_IRQn);
@@ -509,25 +537,26 @@
NVIC_EnableIRQ(EINT2_IRQn);
encoder1_val = 0;
- // Set up encoder 2.
- GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
+ // Set up encoder 4.
+ GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
// Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 12);
- PINCON->PINSEL1 &= ~(0x3 << 10);
- encoder2_val = 0;
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
- // Set up encoder 3.
- GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
+ // Set up encoder 5.
+ GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
// Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 8);
- PINCON->PINSEL1 &= ~(0x3 << 6);
- encoder3_val = 0;
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
+
xTaskCreate(vDelayCapture,
(signed char *) "SENSORs",
@@ -563,6 +592,10 @@
packet->bad_gyro = 0;
}
+ NVIC_DisableIRQ(TIMER1_IRQn);
+ packet->timestamp = ((uint64_t)sensor_timing_wraps << 32) | TIM1->TC;
+ NVIC_EnableIRQ(TIMER1_IRQn);
+
packet->dip_switch0 = dip_switch(0);
packet->dip_switch1 = dip_switch(1);
packet->dip_switch2 = dip_switch(2);
@@ -574,18 +607,24 @@
// because disabling and enabling is cheap (2 instructions) and we really rely
// on low interrupt latencies.
- packet->main.left_drive = encoder5_val;
- packet->main.right_drive = encoder4_val;
-
if (is_bot3) {
packet->robot_id = 1;
+ packet->main.left_drive = encoder3_val;
+ packet->main.right_drive = encoder2_val;
+
packet->bot3.shooter_cycle_ticks = shooter_cycle_ticks;
} else { // is main robot
- packet->robot_id = 0;
+ packet->robot_id = 2;
+
+ packet->main.left_drive = encoder5_val;
+ packet->main.right_drive = encoder4_val;
packet->main.shooter = encoder1_val;
packet->main.indexer = encoder3_val;
+ packet->main.battery_voltage = analog(3);
+ packet->main.left_drive_hall = analog(1);
+ packet->main.right_drive_hall = analog(2);
NVIC_DisableIRQ(EINT3_IRQn);
diff --git a/gyro_board/src/usb/gyro.c b/gyro_board/src/usb/gyro.c
index 866e890..ad49658 100644
--- a/gyro_board/src/usb/gyro.c
+++ b/gyro_board/src/usb/gyro.c
@@ -185,8 +185,9 @@
}
static portTASK_FUNCTION(gyro_read_task, pvParameters) {
- // Connect power and clock.
SC->PCONP |= PCONP_PCSSP0;
+
+ // Make sure that the clock is set up right.
SC->PCLKSEL1 &= ~(3 << 10);
SC->PCLKSEL1 |= 1 << 10;
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 3ac21c8..4e74ce7 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -149,8 +149,8 @@
setup_PLL1();
- // Set CAN to run at CCLK/6, which should have it running about 1 Mbit (1.042)
- SC->PCLKSEL0 = 0xff555555;
+ // Set everything to run at full CCLK by default.
+ SC->PCLKSEL0 = 0x55555555;
/* Configure the LEDs. */
vParTestInitialise();
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/usb_device.c
similarity index 68%
rename from gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
rename to gyro_board/src/usb/usb_device.c
index 8d4bff3..39c3cae 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/usb_device.c
@@ -32,9 +32,19 @@
#include <stdio.h>
#include <string.h>
-#include "usbapi.h"
-#include "usbdebug.h"
-#include "usbstruct.h"
+#include "LPCUSB/usbapi.h"
+#include "LPCUSB/usbdebug.h"
+#include "LPCUSB/usbstruct.h"
+
+// This file is marked private and most of the functions in its associated .c
+// file started out static, but we want to use some of them to do frame handling
+// stuff because we do special stuff with it (handle it ourselves for reduced
+// jitter and actually deal with the frame number correctly), so it's nice to
+// have the utility functions for accessing the hardware available instead of
+// having to rewrite them.
+#include "LPCUSB/usbhw_lpc.h"
+unsigned char USBHwCmdRead(unsigned char bCmd);
+void Wait4DevInt(unsigned long dwIntr);
#include "LPC17xx.h"
@@ -55,8 +65,6 @@
#define LE_WORD(x) ((x)&0xFF),((x)>>8)
-static struct DataStruct usbPacket;
-
static xQueueHandle xRxedChars = NULL, xCharsForTx = NULL;
// This gets cleared each time the ISR is entered and then checked as it's
@@ -144,6 +152,12 @@
0
};
+// Enables interrupts to write data instead of NAKing on the bulk in endpoints.
+// This is in a centralized place so that other NAK interrupts can be enabled
+// all of the time easily in the future.
+static void bulk_in_nak_int(int have_data) {
+ USBHwNakIntEnable(have_data ? INACK_BI : 0);
+}
/**
* Local function to handle incoming bulk data
@@ -179,8 +193,8 @@
(void) bEPStatus;
if (uxQueueMessagesWaitingFromISR(xCharsForTx) == 0) {
- // no more data, disable further NAK interrupts until next USB frame
- USBHwNakIntEnable(INACK_II);
+ // no more data
+ bulk_in_nak_int(0);
return;
}
@@ -245,31 +259,83 @@
return -1;
}
-/**
- * Interrupt handler
- *
- * Simply calls the USB ISR
- */
-void USB_IRQHandler(void) {
- higher_priority_task_woken = pdFALSE;
- USBHwISR();
- portEND_SWITCHING_ISR(higher_priority_task_woken);
-}
+// Instead of registering an lpcusb handler for this, we do it ourself so that
+// we can get the timing jitter down.
+static void HandleFrame(void) {
+ USB->USBDevIntClr = FRAME;
-static void USBFrameHandler(unsigned short wFrame) {
- (void) wFrame;
- if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
- // Data to send is available so enable interrupt instead of NAK on bulk in
- // too.
- USBHwNakIntEnable(INACK_BI | INACK_II);
+ static struct DataStruct sensor_values;
+ fillSensorPacket(&sensor_values);
+
+ static int32_t current_frame = -1;
+ static int guessed_frames = 0;
+
+ uint8_t error_status = USBHwCmdRead(CMD_DEV_READ_ERROR_STATUS);
+ if (error_status & PID_ERR) {
+ ++guessed_frames;
} else {
- USBHwNakIntEnable(INACK_II);
+ int16_t read_frame = USBHwCmdRead(CMD_DEV_READ_CUR_FRAME_NR);
+ USB->USBCmdCode = 0x00000200 | (CMD_DEV_READ_CUR_FRAME_NR << 16);
+ Wait4DevInt(CDFULL);
+ read_frame |= USB->USBCmdData << 8;
+
+ if (current_frame < 0) {
+ current_frame = read_frame;
+ guessed_frames = 0;
+ } else {
+ static const uint32_t kMaxReadFrame = 0x800;
+ static const uint32_t kReadMask = kMaxReadFrame - 1;
+ if ((current_frame & kReadMask) == read_frame) {
+ // This seems like it must mean that we didn't receive the SOF token.
+ ++guessed_frames;
+ } else {
+ guessed_frames = 0;
+ int16_t difference =
+ read_frame - (int16_t)((current_frame + 1) & kReadMask);
+ // If we're off by only a little.
+ if (difference > -10 && difference < 10) {
+ current_frame = ((current_frame + 1) & ~kReadMask) | read_frame;
+ // If we're ahead by only a little but we wrapped.
+ } else if (difference > kMaxReadFrame - 10) {
+ current_frame =
+ ((current_frame & ~kReadMask) - kMaxReadFrame) | read_frame;
+ // If we're behind by only a little but the packet counter wrapped.
+ } else if (difference < -(kMaxReadFrame - 10)) {
+ current_frame =
+ ((current_frame & ~kReadMask) + kMaxReadFrame) | read_frame;
+ } else {
+ // Give up and reset.
+ current_frame = -1;
+ }
+ }
+ }
}
- fillSensorPacket(&usbPacket);
- static uint8_t sequence = 0;
- usbPacket.sequence = sequence++;
- USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&usbPacket, DATA_PACKET_SIZE);
+ sensor_values.frame_number = current_frame + guessed_frames;
+ sensor_values.unknown_frame = guessed_frames > 10;
+
+ USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&sensor_values, DATA_PACKET_SIZE);
+
+ if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
+ // Data to send is available so enable interrupt instead of NAK.
+ bulk_in_nak_int(1);
+ } else {
+ bulk_in_nak_int(0);
+ }
+}
+
+void USB_IRQHandler(void) {
+ higher_priority_task_woken = pdFALSE;
+ uint32_t status = SC->USBIntSt;
+ if (status & USB_INT_REQ_HP) {
+ // We set the frame interrupt to get routed to the high priority line.
+ HandleFrame();
+ }
+ //if (status & USB_INT_REQ_LP) {
+ // Call lpcusb to let it handle all of the other interrupts.
+ USBHwISR();
+ //}
+ portEND_SWITCHING_ISR(higher_priority_task_woken);
}
void usb_init(void) {
@@ -298,8 +364,11 @@
USBHwRegisterEPIntHandler(BULK_IN_EP, DebugIn);
USBHwRegisterEPIntHandler(BULK_OUT_EP, DebugOut);
+ USB->USBDevIntPri = 1; // route frame interrupt to high priority line
+ USB->USBDevIntEn |= FRAME; // enable frame interrupt
+
// register frame handler
- USBHwRegisterFrameHandler(USBFrameHandler);
+ //USBHwRegisterFrameHandler(USBFrameHandler);
DBG("Starting USB communication\n");