Get gyro board situation fixed.
- Finally got the dip switch thing working. (Not tested.)
- Fixed issues with 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/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index add717f..927e004 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"
@@ -22,55 +24,61 @@
// encoder or not.
inline double drivetrain_translate(int32_t in) {
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (44.0 / 32.0 /*encoder gears*/) * // the encoders are on the wheels.
+ (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);
+// 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(INFO, "Got encoder data: %lf, %lf\n",
- drivetrain_translate(data()->main.right_drive),
- drivetrain_translate(data()->main.left_drive));
+ 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.right_drive))
.left_encoder(-drivetrain_translate(data()->main.left_drive))
.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_;
- 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 219f501..c9cd8d1 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/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;
+ }
}