From 1a46e00903e21d75ec16306ff16006a6a9d54f10 Mon Sep 17 00:00:00 2001 From: DrDab Date: Thu, 25 Apr 2024 17:39:21 -0700 Subject: [PATCH] Unify CAN Motor implementation (#317) * RM unused CANMotor_AVR.cpp * Merge CANMotor_PSOC into CANMotor * Fixed function move * Removed double function * Removed unused CANMotor includes --- src/CAN/CANMotor.cpp | 33 +++++++++++++++++++++++ src/CAN/CANMotor_AVR.cpp | 37 -------------------------- src/CAN/CANMotor_PSOC.cpp | 56 --------------------------------------- src/CMakeLists.txt | 1 - 4 files changed, 33 insertions(+), 94 deletions(-) delete mode 100644 src/CAN/CANMotor_AVR.cpp delete mode 100644 src/CAN/CANMotor_PSOC.cpp diff --git a/src/CAN/CANMotor.cpp b/src/CAN/CANMotor.cpp index ca7aef5fd..6d8563e77 100644 --- a/src/CAN/CANMotor.cpp +++ b/src/CAN/CANMotor.cpp @@ -19,6 +19,39 @@ using robot::types::LimitSwitchData; namespace can::motor { +void initEncoder(deviceserial_t serial, bool invertEncoder, bool zeroEncoder, + int32_t pulsesPerJointRev, + std::optional telemetryPeriod) { + auto motorGroupCode = static_cast(devicegroup_t::motor); + CANPacket p; + AssembleEncoderInitializePacket(&p, motorGroupCode, serial, sensor_t::encoder, + invertEncoder, zeroEncoder); + sendCANPacket(p); + std::this_thread::sleep_for(1000us); + AssembleEncoderPPJRSetPacket(&p, motorGroupCode, serial, pulsesPerJointRev); + sendCANPacket(p); + std::this_thread::sleep_for(1000us); + if (telemetryPeriod) { + scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle, + telemetryPeriod.value()); + } +} + +void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint16_t adcLo, + uint16_t adcHi, + std::optional telemetryPeriod) { + CANPacket packet; + auto group = static_cast(devicegroup_t::motor); + AssemblePotHiSetPacket(&packet, group, serial, adcHi, posHi); + sendCANPacket(packet); + AssemblePotLoSetPacket(&packet, group, serial, adcLo, posLo); + sendCANPacket(packet); + if (telemetryPeriod) { + scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle, + telemetryPeriod.value()); + } +} + void emergencyStopMotors() { CANPacket p; AssembleGroupBroadcastingEmergencyStopPacket(&p, static_cast(0x0), diff --git a/src/CAN/CANMotor_AVR.cpp b/src/CAN/CANMotor_AVR.cpp deleted file mode 100644 index e2e167b74..000000000 --- a/src/CAN/CANMotor_AVR.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "CAN.h" -#include "CANMotor.h" -#include "CANUtils.h" - -#include -#include - -extern "C" { -#include -#include -#include -} - -using namespace std::chrono_literals; - -namespace can::motor { -void initEncoder(deviceserial_t serial, bool invertEncoder, bool zeroEncoder, - int32_t pulsesPerJointRev, - std::optional telemetryPeriod) { - auto motorGroupCode = static_cast(devicegroup_t::motor); - CANPacket p; - AssembleEncoderInitializePacket(&p, motorGroupCode, serial, sensor_t::encoder, - invertEncoder, zeroEncoder); - sendCANPacket(p); - std::this_thread::sleep_for(1000us); - AssembleEncoderPPJRSetPacket(&p, motorGroupCode, serial, pulsesPerJointRev); - sendCANPacket(p); - std::this_thread::sleep_for(1000us); - if (telemetryPeriod) { - AssembleTelemetryTimingPacket(&p, motorGroupCode, serial, - PACKET_TELEMETRY_ANG_POSITION, - telemetryPeriod.value().count()); - sendCANPacket(p); - std::this_thread::sleep_for(1000us); - } -} -} // namespace can::motor diff --git a/src/CAN/CANMotor_PSOC.cpp b/src/CAN/CANMotor_PSOC.cpp deleted file mode 100644 index b1d728ded..000000000 --- a/src/CAN/CANMotor_PSOC.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "CAN.h" -#include "CANMotor.h" -#include "CANUtils.h" - -#include -#include -#include -#include -#include -#include -#include - -extern "C" { -#include -#include -#include -} - -using namespace std::chrono_literals; -using std::chrono::milliseconds; - -namespace can::motor { - -void initEncoder(deviceserial_t serial, bool invertEncoder, bool zeroEncoder, - int32_t pulsesPerJointRev, - std::optional telemetryPeriod) { - auto motorGroupCode = static_cast(devicegroup_t::motor); - CANPacket p; - AssembleEncoderInitializePacket(&p, motorGroupCode, serial, sensor_t::encoder, - invertEncoder, zeroEncoder); - sendCANPacket(p); - std::this_thread::sleep_for(1000us); - AssembleEncoderPPJRSetPacket(&p, motorGroupCode, serial, pulsesPerJointRev); - sendCANPacket(p); - std::this_thread::sleep_for(1000us); - if (telemetryPeriod) { - scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle, - telemetryPeriod.value()); - } -} - -void initPotentiometer(deviceserial_t serial, int32_t posLo, int32_t posHi, uint16_t adcLo, - uint16_t adcHi, - std::optional telemetryPeriod) { - CANPacket packet; - auto group = static_cast(devicegroup_t::motor); - AssemblePotHiSetPacket(&packet, group, serial, adcHi, posHi); - sendCANPacket(packet); - AssemblePotLoSetPacket(&packet, group, serial, adcLo, posLo); - sendCANPacket(packet); - if (telemetryPeriod) { - scheduleTelemetryPull(std::make_pair(devicegroup_t::motor, serial), telemtype_t::angle, - telemetryPeriod.value()); - } -} -} // namespace can::motor diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4aaad4278..71dc0b90d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -167,7 +167,6 @@ target_link_libraries(utils ${OpenCV_LIBS}) # No clue why but CAN I/O goes to shit. Don't do it. add_library(can_interface STATIC) target_sources(can_interface PRIVATE - CAN/CANMotor_PSOC.cpp CAN/CANMotor.cpp CAN/CANUtils.cpp )