| #include "frc971/input/sensor_packer.h" |
| |
| #include <arpa/inet.h> |
| |
| #include "aos/common/inttypes.h" |
| #include "aos/common/time.h" |
| |
| #include "frc971/control_loops/index/index.h" |
| |
| using ::std::unique_ptr; |
| namespace hardware = ::aos::crio::hardware; |
| |
| namespace frc971 { |
| |
| 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_edge_position_(0), |
| 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"); |
| } |
| |
| void SensorPacker::ReadEncoder(EncoderReadData *data) { |
| int32_t value = data->counter->Get(); |
| { |
| ::aos::MutexLocker locker(data->sync); |
| *data->output = value; |
| } |
| } |
| |
| void SensorPacker::TopDiscEdgeReader() { |
| while (true) { |
| top_disc_posedge_output_->source()-> |
| WaitForInterrupt(kInterruptTimeout); |
| int32_t position = index_counter_->Get(); |
| { |
| aos::MutexLocker locker(&top_disc_edge_sync_); |
| top_disc_posedge_position_ = position; |
| ++top_disc_posedge_count_; |
| } |
| top_disc_negedge_output_->source()-> |
| WaitForInterrupt(kInterruptTimeout); |
| position = index_counter_->Get(); |
| { |
| aos::MutexLocker locker(&top_disc_edge_sync_); |
| top_disc_negedge_position_ = position; |
| ++top_disc_negedge_count_; |
| } |
| } |
| } |
| |
| void SensorPacker::BottomDiscEdgeReader() { |
| static const aos::time::Time kBottomDiscNegedgeSleep = |
| aos::time::Time::InSeconds( |
| control_loops::IndexMotor::kBottomDiscIndexDelay); |
| while (true) { |
| bottom_disc_->source()->WaitForInterrupt(kInterruptTimeout); |
| if (bottom_disc_->Get()) { |
| aos::time::Time start = aos::time::Time::Now(); |
| { |
| aos::MutexLocker locker(&bottom_disc_edge_sync_); |
| ++bottom_disc_negedge_count_; |
| } |
| aos::time::SleepUntil(start + kBottomDiscNegedgeSleep); |
| int32_t position = index_counter_->Get(); |
| { |
| aos::MutexLocker locker(&bottom_disc_edge_sync_); |
| bottom_disc_negedge_wait_position_ = position; |
| ++bottom_disc_negedge_wait_count_; |
| } |
| } else { |
| { |
| aos::MutexLocker locker(&bottom_disc_edge_sync_); |
| ++bottom_disc_posedge_count_; |
| } |
| } |
| } |
| } |
| |
| void SensorPacker::PackInto(sensor_values *values) { |
| 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 |