Fixed DMA on the roboRIO.
Change-Id: Ib4f2e9dfb78640ea8d28e62528a9296962bd2cd9
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalInput.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalInput.cpp
index 8c6f39a..4ce8e72 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalInput.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalInput.cpp
@@ -140,10 +140,6 @@
HALReport(HALUsageReporting::kResourceType_DigitalInput,
input->GetChannelForRouting());
-
- printf("Source filter %d is %d\n", input->GetChannelForRouting(),
- getFilterSelect(input->m_digital_ports[input->GetChannelForRouting()],
- &status));
}
void DigitalGlitchFilter::Remove(DigitalSource *input) {
diff --git a/aos/externals/forwpilib/dma.cc b/aos/externals/forwpilib/dma.cc
index f775ff5..4517905 100644
--- a/aos/externals/forwpilib/dma.cc
+++ b/aos/externals/forwpilib/dma.cc
@@ -1,5 +1,7 @@
#include "dma.h"
+#include <string.h>
+
#include <algorithm>
#include <type_traits>
@@ -7,6 +9,7 @@
#include "AnalogInput.h"
#include "Encoder.h"
+
// Interface to the roboRIO FPGA's DMA features.
// Like tEncoder::tOutput with the bitfields reversed.
@@ -49,7 +52,9 @@
DMA::DMA() {
tRioStatusCode status = 0;
tdma_config_ = tDMA::create(&status);
+ tdma_config_->writeConfig_ExternalClock(false, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ NiFpga_WriteU32(0x10000, 0x1832c, 0x0);
if (status != 0) {
return;
}
@@ -79,7 +84,7 @@
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
-void DMA::Add(Encoder * /*encoder*/) {
+void DMA::Add(Encoder *encoder) {
tRioStatusCode status = 0;
if (manager_) {
@@ -87,9 +92,11 @@
"DMA::Add() only works before DMA::Start()");
return;
}
-
- fprintf(stderr, "DMA::Add(Encoder*) needs re-testing. aborting\n");
- abort();
+ if (encoder->GetFPGAIndex() >= 4) {
+ wpi_setErrorWithContext(
+ NiFpga_Status_InvalidParameter,
+ "FPGA encoder index is not in the 4 that get logged.");
+ }
// TODO(austin): Encoder uses a Counter for 1x or 2x; quad for 4x...
tdma_config_->writeConfig_Enable_Encoders(true, &status);
@@ -118,10 +125,6 @@
return;
}
- fprintf(stderr, "DMA::Add(AnalogInput*) needs testing. aborting\n");
- abort();
-
- // TODO(brian): Figure out if this math is actually correct.
if (input->GetChannel() <= 3) {
tdma_config_->writeConfig_Enable_AI0_Low(true, &status);
} else {
@@ -143,49 +146,62 @@
::std::find(trigger_channels_.begin(), trigger_channels_.end(), false);
if (index == trigger_channels_.end()) {
wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
- "DMA: No channels left");
+ "DMA: No channels left");
return;
}
*index = true;
const int channel_index = index - trigger_channels_.begin();
+ /*
+ printf(
+ "Allocating trigger on index %d, routing module %d, routing channel %d, "
+ "is analog %d\n",
+ channel_index, input->GetModuleForRouting(),
+ input->GetChannelForRouting(), input->GetAnalogTriggerForRouting());
+ */
- tdma_config_->writeConfig_ExternalClock(true, &status);
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
+ const bool is_external_clock =
+ tdma_config_->readConfig_ExternalClock(&status);
+ if (status == 0) {
+ if (!is_external_clock) {
+ tdma_config_->writeConfig_ExternalClock(true, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ if (status != 0) {
+ return;
+ }
+ }
+ } else {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
return;
}
+ nFPGA::nFRC_2015_1_0_A::tDMA::tExternalTriggers new_trigger;
+
+ new_trigger.FallingEdge = falling;
+ new_trigger.RisingEdge = rising;
+ new_trigger.ExternalClockSource_AnalogTrigger =
+ input->GetAnalogTriggerForRouting();
+ new_trigger.ExternalClockSource_AnalogTrigger = false;
+ new_trigger.ExternalClockSource_Module = input->GetModuleForRouting();
+ new_trigger.ExternalClockSource_Channel = input->GetChannelForRouting();
+
// Configures the trigger to be external, not off the FPGA clock.
- tdma_config_->writeExternalTriggers_ExternalClockSource_Channel(
- channel_index, input->GetChannelForRouting(), &status);
+ /*
+ tdma_config_->writeExternalTriggers(channel_index, new_trigger, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
- return;
- }
+ */
- tdma_config_->writeExternalTriggers_ExternalClockSource_Module(
- channel_index, input->GetModuleForRouting(), &status);
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
+ uint32_t current_triggers;
+ tRioStatusCode register_status = NiFpga_ReadU32(0x10000, 0x1832c, ¤t_triggers);
+ if (register_status != 0) {
+ wpi_setErrorWithContext(register_status, getHALErrorMessage(status));
return;
}
- tdma_config_->writeExternalTriggers_ExternalClockSource_AnalogTrigger(
- channel_index, input->GetAnalogTriggerForRouting(), &status);
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
- return;
- }
- tdma_config_->writeExternalTriggers_RisingEdge(channel_index, rising,
- &status);
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
- return;
- }
- tdma_config_->writeExternalTriggers_FallingEdge(channel_index, falling,
- &status);
- wpi_setErrorWithContext(status, getHALErrorMessage(status));
- if (status != 0) {
+ current_triggers = (current_triggers & ~(0xff << (channel_index * 8))) |
+ (new_trigger.value << (channel_index * 8));
+ register_status = NiFpga_WriteU32(0x10000, 0x1832c, current_triggers);
+ if (register_status != 0) {
+ wpi_setErrorWithContext(register_status, getHALErrorMessage(status));
return;
}
}
@@ -203,7 +219,7 @@
}
sample->dma_ = this;
- // memset(&sample->read_buffer_, 0, sizeof(read_buffer_));
+ // memset(&sample->read_buffer_, 0, sizeof(sample->read_buffer_));
manager_->read(sample->read_buffer_, capture_size_, timeout_ms,
&remainingBytes, &status);
@@ -288,7 +304,25 @@
capture_size_ = accum_size + 1;
}
- manager_.reset(new nFPGA::tDMAManager(0, queue_depth * capture_size_, &status));
+ manager_.reset(
+ new nFPGA::tDMAManager(0, queue_depth * capture_size_, &status));
+
+ if (0) {
+ for (int i = 0; i < 4; ++i) {
+ tRioStatusCode status = 0;
+ auto x = tdma_config_->readExternalTriggers(i, &status);
+ printf(
+ "index %d, FallingEdge: %d, RisingEdge: %d, "
+ "ExternalClockSource_AnalogTrigger: %d, ExternalClockSource_Module: "
+ "%d, ExternalClockSource_Channel: %d\n",
+ i, x.FallingEdge, x.RisingEdge, x.ExternalClockSource_AnalogTrigger,
+ x.ExternalClockSource_Module, x.ExternalClockSource_Channel);
+ if (status != 0) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ }
+ }
+
wpi_setErrorWithContext(status, getHALErrorMessage(status));
if (status != 0) {
return;
@@ -309,6 +343,7 @@
if (status != 0) {
return;
}
+
}
static_assert(::std::is_pod<DMASample>::value, "DMASample needs to be POD");
@@ -345,6 +380,12 @@
return -1;
}
+ if (encoder->GetFPGAIndex() >= 4) {
+ wpi_setStaticErrorWithContext(dma_,
+ NiFpga_Status_ResourceNotFound,
+ getHALErrorMessage(NiFpga_Status_ResourceNotFound));
+ }
+
uint32_t dmaWord =
read_buffer_[offset(kEnable_Encoders) + input->GetFPGAIndex()];
int32_t result = 0;
@@ -384,28 +425,32 @@
return raw / input->GetEncodingScale();
}
-int16_t DMASample::GetValue(AnalogInput *input) const {
+uint16_t DMASample::GetValue(AnalogInput *input) const {
if (offset(kEnable_Encoders) == -1) {
wpi_setStaticErrorWithContext(dma_,
NiFpga_Status_ResourceNotFound,
getHALErrorMessage(NiFpga_Status_ResourceNotFound));
- return -1;
+ return 0xffff;
}
uint32_t dmaWord;
- // TODO(brian): Figure out if this math is actually correct.
+ uint32_t channel = input->GetChannel();
if (input->GetChannel() <= 3) {
- dmaWord = read_buffer_[offset(kEnable_AI0_Low) + input->GetChannel()];
+ dmaWord = read_buffer_[offset(kEnable_AI0_Low) + channel / 2];
} else {
- dmaWord = read_buffer_[offset(kEnable_AI0_High) + input->GetChannel() - 4];
+ dmaWord = read_buffer_[offset(kEnable_AI0_High) + (channel - 4) / 2];
}
- // TODO(brian): Verify that this is correct.
+ if (channel % 1) {
+ return (dmaWord >> 16) & 0xffff;
+ } else {
+ return (dmaWord) & 0xffff;
+ }
return static_cast<int16_t>(dmaWord);
}
float DMASample::GetVoltage(AnalogInput *input) const {
- int16_t value = GetValue(input);
- if (value == -1) return 0.0;
+ uint16_t value = GetValue(input);
+ if (value == 0xffff) return 0.0;
uint32_t lsb_weight = input->GetLSBWeight();
int32_t offset = input->GetOffset();
float voltage = lsb_weight * 1.0e-9 * value - offset * 1.0e-9;
diff --git a/aos/externals/forwpilib/dma.h b/aos/externals/forwpilib/dma.h
index 446ded4..754b0a5 100644
--- a/aos/externals/forwpilib/dma.h
+++ b/aos/externals/forwpilib/dma.h
@@ -34,7 +34,7 @@
// Returns the {1, 2, or 4} X scaled value of the encoder in the sample.
int32_t Get(Encoder *input) const;
// Returns the raw 12-bit value from the ADC.
- int16_t GetValue(AnalogInput *input) const;
+ uint16_t GetValue(AnalogInput *input) const;
// Returns the scaled value of an analog input.
float GetVoltage(AnalogInput *input) const;
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 0ea2236..6c7ca3f 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -107,7 +107,7 @@
// Some kind of indicator light?
{
message->angle = new_angle;
- LOG_STRUCT(DEBUG, "collected", *message);
+ LOG_STRUCT(DEBUG, "collected while zeroing", *message);
}
zeroing_data[zeroing_index] = new_angle;
++zeroing_index;
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index c60feec..de1ed32 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -206,12 +206,13 @@
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(new DMASynchronizer(::std::move(dma)));
dma_synchronizer_->Add(&arm_left_encoder_);
- dma_synchronizer_->Add(&arm_right_encoder_);
dma_synchronizer_->Add(&elevator_left_encoder_);
+ dma_synchronizer_->Add(&arm_right_encoder_);
dma_synchronizer_->Add(&elevator_right_encoder_);
}
void operator()() {
+ LOG(INFO, "In sensor reader thread\n");
::aos::SetCurrentThreadName("SensorReader");
my_pid_ = getpid();
@@ -219,10 +220,11 @@
wrist_encoder_.Start();
dma_synchronizer_->Start();
+ LOG(INFO, "Things are now started\n");
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
- ::aos::time::PhasedLoopXMS(5, 9000);
+ ::aos::time::PhasedLoopXMS(5, 4000);
RunIteration();
}
@@ -504,8 +506,12 @@
class ClawWriter : public LoopOutputHandler {
public:
- void set_intake_talon(::std::unique_ptr<Talon> t) {
- intake_talon_ = ::std::move(t);
+ void set_left_intake_talon(::std::unique_ptr<Talon> t) {
+ left_intake_talon_ = ::std::move(t);
+ }
+
+ void set_right_intake_talon(::std::unique_ptr<Talon> t) {
+ right_intake_talon_ = ::std::move(t);
}
void set_wrist_talon(::std::unique_ptr<Talon> t) {
@@ -520,17 +526,20 @@
virtual void Write() override {
auto &queue = ::frc971::control_loops::claw_queue.output;
LOG_STRUCT(DEBUG, "will output", *queue);
- intake_talon_->Set(queue->intake_voltage / 12.0);
+ left_intake_talon_->Set(queue->intake_voltage / 12.0);
+ right_intake_talon_->Set(queue->intake_voltage / 12.0);
wrist_talon_->Set(queue->voltage / 12.0);
}
virtual void Stop() override {
LOG(WARNING, "Claw output too old.\n");
- intake_talon_->Disable();
+ left_intake_talon_->Disable();
+ right_intake_talon_->Disable();
wrist_talon_->Disable();
}
- ::std::unique_ptr<Talon> intake_talon_;
+ ::std::unique_ptr<Talon> left_intake_talon_;
+ ::std::unique_ptr<Talon> right_intake_talon_;
::std::unique_ptr<Talon> wrist_talon_;
};
@@ -543,38 +552,44 @@
class WPILibRobot : public RobotBase {
public:
+ ::std::unique_ptr<Encoder> encoder(int index) {
+ return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
+ Encoder::k4X);
+ }
virtual void StartCompetition() {
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
+ // TODO(austin): Compressor needs to use a spike.
::std::unique_ptr<Compressor> compressor(new Compressor());
compressor->SetClosedLoopControl(true);
SensorReader reader;
- // TODO(sensors): Replace all the 99s with real port numbers.
- reader.set_arm_left_encoder(
- make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_arm_left_index(make_unique<DigitalInput>(99));
- reader.set_arm_left_potentiometer(make_unique<AnalogInput>(99));
- reader.set_arm_right_encoder(
- make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_arm_right_index(make_unique<DigitalInput>(99));
- reader.set_arm_right_potentiometer(make_unique<AnalogInput>(99));
- reader.set_elevator_left_encoder(
- make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_elevator_left_index(make_unique<DigitalInput>(99));
- reader.set_elevator_left_potentiometer(make_unique<AnalogInput>(99));
- reader.set_elevator_right_encoder(
- make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_elevator_right_index(make_unique<DigitalInput>(99));
- reader.set_elevator_right_potentiometer(make_unique<AnalogInput>(99));
- reader.set_wrist_encoder(make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_wrist_index(make_unique<DigitalInput>(99));
- reader.set_wrist_potentiometer(make_unique<AnalogInput>(99));
- reader.set_left_encoder(make_unique<Encoder>(99, 99, false, Encoder::k4X));
- reader.set_right_encoder(make_unique<Encoder>(99, 99, false, Encoder::k4X));
+ LOG(INFO, "Creating the reader\n");
+ reader.set_arm_left_encoder(encoder(1));
+ reader.set_arm_left_index(make_unique<DigitalInput>(1));
+ reader.set_arm_left_potentiometer(make_unique<AnalogInput>(1));
+
+ reader.set_arm_right_encoder(encoder(5));
+ reader.set_arm_right_index(make_unique<DigitalInput>(5));
+ reader.set_arm_right_potentiometer(make_unique<AnalogInput>(5));
+
+ reader.set_elevator_left_encoder(encoder(0));
+ reader.set_elevator_left_index(make_unique<DigitalInput>(0));
+ reader.set_elevator_left_potentiometer(make_unique<AnalogInput>(0));
+
+ reader.set_elevator_right_encoder(encoder(4));
+ reader.set_elevator_right_index(make_unique<DigitalInput>(4));
+ reader.set_elevator_right_potentiometer(make_unique<AnalogInput>(4));
+
+ reader.set_wrist_encoder(encoder(6));
+ reader.set_wrist_index(make_unique<DigitalInput>(6));
+ reader.set_wrist_potentiometer(make_unique<AnalogInput>(6));
+
+ reader.set_left_encoder(encoder(2));
+ reader.set_right_encoder(encoder(3));
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));
GyroSender gyro_sender;
@@ -582,39 +597,41 @@
DrivetrainWriter drivetrain_writer;
drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
+ ::std::unique_ptr<Talon>(new Talon(8)));
drivetrain_writer.set_right_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(2)));
+ ::std::unique_ptr<Talon>(new Talon(0)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
// TODO(sensors): Get real PWM output and relay numbers for the fridge and
// claw.
FridgeWriter fridge_writer;
fridge_writer.set_left_arm_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ ::std::unique_ptr<Talon>(new Talon(6)));
fridge_writer.set_right_arm_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ ::std::unique_ptr<Talon>(new Talon(2)));
fridge_writer.set_left_elevator_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ ::std::unique_ptr<Talon>(new Talon(7)));
fridge_writer.set_right_elevator_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ ::std::unique_ptr<Talon>(new Talon(1)));
::std::thread fridge_writer_thread(::std::ref(fridge_writer));
ClawWriter claw_writer;
- claw_writer.set_intake_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ claw_writer.set_left_intake_talon(
+ ::std::unique_ptr<Talon>(new Talon(5)));
+ claw_writer.set_right_intake_talon(
+ ::std::unique_ptr<Talon>(new Talon(3)));
claw_writer.set_wrist_talon(
- ::std::unique_ptr<Talon>(new Talon(99)));
+ ::std::unique_ptr<Talon>(new Talon(4)));
::std::thread claw_writer_thread(::std::ref(claw_writer));
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
SolenoidWriter solenoid_writer(pcm);
- solenoid_writer.set_fridge_grabbers_top_front(pcm->MakeSolenoid(99));
- solenoid_writer.set_fridge_grabbers_top_back(pcm->MakeSolenoid(99));
- solenoid_writer.set_fridge_grabbers_bottom_front(pcm->MakeSolenoid(99));
- solenoid_writer.set_fridge_grabbers_bottom_back(pcm->MakeSolenoid(99));
- solenoid_writer.set_claw_pinchers(pcm->MakeSolenoid(99));
+ solenoid_writer.set_fridge_grabbers_top_front(pcm->MakeSolenoid(1));
+ solenoid_writer.set_fridge_grabbers_top_back(pcm->MakeSolenoid(1));
+ solenoid_writer.set_fridge_grabbers_bottom_front(pcm->MakeSolenoid(2));
+ solenoid_writer.set_fridge_grabbers_bottom_back(pcm->MakeSolenoid(3));
+ solenoid_writer.set_claw_pinchers(pcm->MakeSolenoid(0));
::std::thread solenoid_thread(::std::ref(solenoid_writer));
// Wait forever. Not much else to do...