2013 hardware interface work
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 6951760..fc2ddd7 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -11,10 +11,18 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/GyroAngle.q.h"
#include "frc971/queues/Piston.q.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
using ::frc971::control_loops::drivetrain;
using ::frc971::control_loops::shifters;
using ::frc971::sensors::gyro;
+using ::frc971::control_loops::wrist;
+using ::frc971::control_loops::index_loop;
+using ::frc971::control_loops::shooter;
+using ::frc971::control_loops::angle_adjust;
namespace frc971 {
@@ -91,6 +99,20 @@
if (PosEdge(1, 3)) {
is_high_gear = true;
}
+
+ // frisbee pickup is -0.634
+ wrist.goal.MakeWithBuilder().goal(-0.634).Send();
+
+ index_loop.goal.MakeWithBuilder()
+ .goal_state(Pressed(1, 4) ? 2 :
+ Pressed(1, 5) ? 3 :
+ Pressed(1, 10) ? 4 : 1).Send();
+
+ angle_adjust.goal.MakeWithBuilder()
+ .goal(Pressed(3, 1) ? 0.6 : 0.35).Send();
+
+ shooter.goal.MakeWithBuilder()
+ .velocity(Pressed(2, 9) ? 325.0 : 0.0).Send();
}
}
};
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index dcfc864..bd4e5b0 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -24,6 +24,11 @@
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/atom_code/atom_code.gyp:init',
+ '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
],
},
{
@@ -37,6 +42,12 @@
'<(AOS)/build/aos.gyp:libaos',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/common/network/network.gyp:socket',
+ '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:control_loops',
],
},
{
@@ -72,8 +83,10 @@
'<(AOS)/build/aos.gyp:libaos',
'<(AOS)/crio/shared_libs/shared_libs.gyp:interrupt_notifier',
'<(AOS)/common/common.gyp:mutex',
+ '<(AOS)/common/common.gyp:time',
'<(AOS)/crio/hardware/hardware.gyp:counter',
'<(AOS)/crio/hardware/hardware.gyp:digital_source',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_lib',
],
},
{
diff --git a/frc971/input/sensor_packer.cc b/frc971/input/sensor_packer.cc
index 865b25b..b241f50 100644
--- a/frc971/input/sensor_packer.cc
+++ b/frc971/input/sensor_packer.cc
@@ -7,13 +7,143 @@
#include "frc971/control_loops/index/index.h"
-using ::aos::MutexLocker;
+using ::std::unique_ptr;
+namespace hardware = ::aos::crio::hardware;
namespace frc971 {
-SensorPacker::SensorPacker() : lencoder(1, 2), rencoder(3, 4) {
- lencoder.Start();
- rencoder.Start();
+SensorPacker::SensorPacker()
+ : drive_left_counter_(::hardware::Counter::Create(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(3)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(4)),
+ ::CounterBase::EncodingType::k4X)),
+ drive_right_counter_(::hardware::Counter::Create(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(1)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(2)),
+ ::CounterBase::EncodingType::k4X)),
+
+ wrist_counter_(::hardware::Counter::Create(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(5)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(6)),
+ ::CounterBase::EncodingType::k4X)),
+ wrist_hall_effect_(new ::hardware::AnalogTriggerOutput(
+ unique_ptr< ::AnalogTrigger>(
+ new ::AnalogTrigger(1)),
+ ::AnalogTriggerOutput::Type::kState)),
+ wrist_notifier_(ReadEncoder, wrist_hall_effect_->source(),
+ new EncoderReadData(wrist_counter_.get(),
+ &wrist_edge_position_, &wrist_sync_)),
+
+ angle_adjust_counter_(::hardware::Counter::Create(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(7)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(8)),
+ ::CounterBase::EncodingType::k2X)),
+ angle_adjust_middle_hall_effect_(new ::hardware::AnalogTriggerOutput(
+ unique_ptr< ::AnalogTrigger>(
+ new ::AnalogTrigger(5)),
+ ::AnalogTriggerOutput::Type::kState)),
+ angle_adjust_bottom_hall_effect_(new ::hardware::AnalogTriggerOutput(
+ unique_ptr< ::AnalogTrigger>(
+ new ::AnalogTrigger(3)),
+ ::AnalogTriggerOutput::Type::kState)),
+ angle_adjust_middle_notifier_(
+ ReadEncoder,
+ angle_adjust_middle_hall_effect_->source(),
+ new EncoderReadData(angle_adjust_counter_.get(),
+ &angle_adjust_middle_edge_position_,
+ &angle_adjust_sync_)),
+ angle_adjust_bottom_notifier_(
+ ReadEncoder,
+ angle_adjust_bottom_hall_effect_->source(),
+ new EncoderReadData(angle_adjust_counter_.get(),
+ &angle_adjust_bottom_edge_position_,
+ &angle_adjust_sync_)),
+
+ shooter_counter_(::hardware::Counter::Create(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(9)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(10)),
+ ::CounterBase::EncodingType::k4X)),
+
+ index_counter_(new ::hardware::CounterCounter(
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(11)),
+ unique_ptr< ::hardware::DigitalSource>(
+ new ::hardware::DigitalInput(12)),
+ ::CounterBase::EncodingType::k2X)),
+ top_disc_edge_task_("TopDsc",
+ reinterpret_cast<FUNCPTR>(
+ StaticTopDiscEdgeReader),
+ 96),
+ top_disc_(new ::AnalogTrigger(2)),
+ top_disc_posedge_output_(new ::hardware::AnalogTriggerOutput(
+ unique_ptr< ::AnalogTriggerOutput>(
+ top_disc_->CreateOutput(
+ ::AnalogTriggerOutput::Type::kState)))),
+ top_disc_negedge_output_(new ::hardware::AnalogTriggerOutput(
+ unique_ptr< ::AnalogTriggerOutput>(
+ top_disc_->CreateOutput(
+ ::AnalogTriggerOutput::Type::kState)))),
+ top_disc_posedge_count_(0),
+ top_disc_negedge_count_(0),
+ top_disc_posedge_position_(0),
+ top_disc_negedge_position_(0),
+ bottom_disc_edge_task_("BotDsc",
+ reinterpret_cast<FUNCPTR>(
+ StaticBottomDiscEdgeReader),
+ 96),
+ bottom_disc_(new ::hardware::DigitalInput(13)),
+ bottom_disc_posedge_count_(0),
+ bottom_disc_negedge_count_(0),
+ bottom_disc_negedge_wait_count_(0),
+ bottom_disc_negedge_wait_position_(0) {
+ // 7, 8 = angle, down = -
+ // 5, 6 = wrist, down = +
+ // drive = flipped, l/r = flipped
+ // 9, 10 = angle adjust, out = -
+ // 11, 12 = indexing, up = -
+ // positive result should be up in the end
+ drive_left_counter_->counter_base()->Start();
+ drive_right_counter_->counter_base()->Start();
+
+ shooter_counter_->counter_base()->Start();
+
+ wrist_counter_->counter_base()->Start();
+ wrist_hall_effect_->source()->SetUpSourceEdge(true, true);
+ wrist_notifier_.Start();
+
+ angle_adjust_counter_->counter_base()->Start();
+ angle_adjust_middle_hall_effect_->source()->SetUpSourceEdge(true, true);
+ angle_adjust_bottom_hall_effect_->source()->SetUpSourceEdge(true, true);
+ angle_adjust_middle_notifier_.Start();
+ angle_adjust_bottom_notifier_.Start();
+
+ index_counter_->counter()->SetExternalDirectionMode();
+ index_counter_->counter_base()->Start();
+ top_disc_->SetLimitsVoltage(
+ ::hardware::AnalogTriggerOutput::kDefaultLowerVoltage,
+ ::hardware::AnalogTriggerOutput::kDefaultUpperVoltage);
+ top_disc_posedge_output_->source()->RequestInterrupts();
+ top_disc_posedge_output_->source()->SetUpSourceEdge(false, true);
+ top_disc_negedge_output_->source()->RequestInterrupts();
+ top_disc_negedge_output_->source()->SetUpSourceEdge(true, false);
+ top_disc_edge_task_.Start(reinterpret_cast<uintptr_t>(this));
+ // TODO(brians) change this to the correct constant
+ aos::time::SleepFor(aos::time::Time::InSeconds(0.25));
+ bottom_disc_->source()->RequestInterrupts();
+ bottom_disc_->source()->SetUpSourceEdge(true, true);
+ bottom_disc_edge_task_.Start(reinterpret_cast<uintptr_t>(this));
+ // TODO(brians) change this to the correct constant
+ aos::time::SleepFor(aos::time::Time::InSeconds(0.25));
printf("frc971::SensorPacker started\n");
}
@@ -76,8 +206,49 @@
}
void SensorPacker::PackInto(sensor_values *values) {
- values->lencoder = htonl(-lencoder.GetRaw());
- values->rencoder = -htonl(-rencoder.GetRaw());
+ values->shooter_encoder = shooter_counter_->Get();
+
+ values->drive_left_encoder = htonl(-drive_left_counter_->Get());
+ values->drive_right_encoder = -htonl(-drive_right_counter_->Get());
+
+ {
+ aos::MutexLocker locker(&wrist_sync_);
+ values->wrist_position = wrist_counter_->Get();
+ values->wrist_edge_position = wrist_edge_position_;
+ values->wrist_hall_effect = wrist_hall_effect_->Get();
+ }
+
+ {
+ aos::MutexLocker locker(&angle_adjust_sync_);
+ values->angle_adjust_position = angle_adjust_counter_->Get();
+ values->angle_adjust_middle_edge_position =
+ angle_adjust_middle_edge_position_;
+ values->angle_adjust_bottom_edge_position =
+ angle_adjust_bottom_edge_position_;
+ values->angle_adjust_middle_hall_effect =
+ angle_adjust_middle_hall_effect_->Get();
+ values->angle_adjust_bottom_hall_effect =
+ angle_adjust_bottom_hall_effect_->Get();
+ }
+
+ values->index_encoder = index_counter_->Get();
+ {
+ aos::MutexLocker locker(&top_disc_edge_sync_);
+ values->top_disc_posedge_count = top_disc_posedge_count_;
+ values->top_disc_negedge_count = top_disc_negedge_count_;
+ values->top_disc_posedge_position = top_disc_posedge_position_;
+ values->top_disc_negedge_position = top_disc_negedge_position_;
+ values->top_disc = top_disc_->GetTriggerState();
+ }
+ {
+ aos::MutexLocker locker(&bottom_disc_edge_sync_);
+ values->bottom_disc_negedge_wait_position =
+ bottom_disc_negedge_wait_position_;
+ values->bottom_disc_negedge_count = bottom_disc_negedge_count_;
+ values->bottom_disc_negedge_wait_count = bottom_disc_negedge_wait_count_;
+ values->bottom_disc_posedge_count = bottom_disc_posedge_count_;
+ values->bottom_disc = bottom_disc_->Get();
+ }
}
} // namespace frc971
diff --git a/frc971/input/sensor_packer.h b/frc971/input/sensor_packer.h
index 78f6bf6..18de129 100644
--- a/frc971/input/sensor_packer.h
+++ b/frc971/input/sensor_packer.h
@@ -16,7 +16,11 @@
#include "aos/common/sensors/sensor_packer.h"
#include "aos/crio/hardware/counter.h"
#include "aos/crio/hardware/digital_source.h"
+#include "aos/common/time.h"
+#include "aos/crio/hardware/counter.h"
+#include "aos/crio/hardware/digital_source.h"
+#include "frc971/control_loops/index/index.h"
#include "frc971/queues/sensor_values.h"
namespace frc971 {
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
index 8695785..bb99e2a 100644
--- a/frc971/input/sensor_unpacker.cc
+++ b/frc971/input/sensor_unpacker.cc
@@ -1,14 +1,25 @@
#include "frc971/input/sensor_unpacker.h"
#include <arpa/inet.h>
+#include <math.h>
#include "aos/common/inttypes.h"
#include "frc971/control_loops/DriveTrain.q.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#ifndef M_PI
#define M_PI 3.14159265358979323846
+#endif
using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::wrist;
+using ::frc971::control_loops::angle_adjust;
+using ::frc971::control_loops::shooter;
+using ::frc971::control_loops::index_loop;
namespace frc971 {
namespace {
@@ -19,12 +30,35 @@
(3.5 * 2.54 / 100.0 * M_PI);
}
+inline double wrist_translate(int32_t in) {
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
+}
+
+inline double angle_adjust_translate(int32_t in) {
+ static const double kCableDiameter = 0.060;
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 2.0 /*2x*/) *
+ ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
+ (2 * M_PI);
+}
+
+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);
+}
+
+inline double index_translate(int32_t in) {
+ return -static_cast<double>(in) / (128.0 /*cpr*/ * 2.0 /*2x*/) *
+ (1.0) /*gears*/ * (2 * M_PI);
+}
+
} // namespace
SensorUnpacker::SensorUnpacker() {}
void SensorUnpacker::UnpackFrom(sensor_values *values) {
- for (size_t i = 0; i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
+ for (size_t i = 0;
+ i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
values->encoders[i] = ntohl(values->encoders[i]);
}
@@ -37,6 +71,43 @@
.left_encoder(left_encoder)
.right_encoder(right_encoder)
.Send();
+
+ wrist.position.MakeWithBuilder()
+ .pos(wrist_translate(values->wrist_position))
+ .hall_effect(!values->wrist_hall_effect)
+ .calibration(wrist_translate(values->wrist_edge_position))
+ .Send();
+
+ angle_adjust.position.MakeWithBuilder()
+ .angle(angle_adjust_translate(values->angle_adjust_position))
+ .bottom_hall_effect(!values->angle_adjust_bottom_hall_effect)
+ .middle_hall_effect(!values->angle_adjust_middle_hall_effect && false)
+ .bottom_calibration(angle_adjust_translate(
+ values->angle_adjust_bottom_edge_position))
+ .middle_calibration(angle_adjust_translate(
+ values->angle_adjust_middle_edge_position))
+ .Send();
+
+ shooter.position.MakeWithBuilder()
+ .position(shooter_translate(values->shooter_encoder))
+ .Send();
+
+ index_loop.position.MakeWithBuilder()
+ .index_position(index_translate(values->index_encoder))
+ .top_disc_detect(!values->top_disc)
+ .top_disc_posedge_count(values->top_disc_posedge_count)
+ .top_disc_posedge_position(index_translate(
+ values->top_disc_posedge_position))
+ .top_disc_negedge_count(values->top_disc_negedge_count)
+ .top_disc_negedge_position(index_translate(
+ values->top_disc_negedge_position))
+ .bottom_disc_detect(!values->bottom_disc)
+ .bottom_disc_posedge_count(values->bottom_disc_posedge_count)
+ .bottom_disc_negedge_count(values->bottom_disc_negedge_count)
+ .bottom_disc_negedge_wait_position(index_translate(
+ values->bottom_disc_negedge_wait_position))
+ .bottom_disc_negedge_wait_count(values->bottom_disc_negedge_wait_count)
+ .Send();
}
} // namespace frc971