From 03e0786f6d0c7706afb197bacc88744ade9284a4 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Fri, 16 Jun 2023 19:06:35 -0700 Subject: [PATCH] Add general telemetry prompting to FakeCANBoard (#258) * Add general telemetry prompting to fakecanboard * Update deps --- src/CAN/CAN.cpp | 3 +- src/CAN/CANUtils.h | 5 +-- src/CAN/FakeCANBoard.cpp | 76 +++++++++++++++++++++++----------------- src/CMakeLists.txt | 2 +- update_deps.sh | 2 +- 5 files changed, 50 insertions(+), 38 deletions(-) diff --git a/src/CAN/CAN.cpp b/src/CAN/CAN.cpp index 2761293ff..6447b962f 100644 --- a/src/CAN/CAN.cpp +++ b/src/CAN/CAN.cpp @@ -70,7 +70,8 @@ const std::unordered_map telemCodeToTypeMap = { {PACKET_TELEMETRY_GYRO_X, telemtype_t::gyro_x}, {PACKET_TELEMETRY_GYRO_Y, telemtype_t::gyro_y}, {PACKET_TELEMETRY_GYRO_Z, telemtype_t::gyro_z}, - {PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch}}; + {PACKET_TELEMETRY_LIM_SW_STATE, telemtype_t::limit_switch}, + {PACKET_TELEMETRY_ADC_RAW, telemtype_t::adc_raw}}; // the telemetry map will store telemetry code instead of telem enum // this means unrecognized telemetry types won't cause UB diff --git a/src/CAN/CANUtils.h b/src/CAN/CANUtils.h index 045163b93..4be2c9037 100644 --- a/src/CAN/CANUtils.h +++ b/src/CAN/CANUtils.h @@ -48,8 +48,9 @@ enum class telemtype_t { gyro_x = PACKET_TELEMETRY_GYRO_X, gyro_y = PACKET_TELEMETRY_GYRO_Y, gyro_z = PACKET_TELEMETRY_GYRO_Z, - limit_switch = PACKET_TELEMETRY_LIM_SW_STATE - // TODO: add further telemetry types if required + limit_switch = PACKET_TELEMETRY_LIM_SW_STATE, + adc_raw = PACKET_TELEMETRY_ADC_RAW + // add further telemetry types if required }; /** diff --git a/src/CAN/FakeCANBoard.cpp b/src/CAN/FakeCANBoard.cpp index b8a8569fb..23ec9df9e 100644 --- a/src/CAN/FakeCANBoard.cpp +++ b/src/CAN/FakeCANBoard.cpp @@ -27,20 +27,17 @@ enum class TestMode { PIDVel, Encoder, LimitSwitch, - ScienceTelemetry, + Telemetry, ScienceMotors, ScienceServos }; -std::unordered_set modes = {static_cast(TestMode::ModeSet), - static_cast(TestMode::PWM), - static_cast(TestMode::PID), - static_cast(TestMode::Encoder), - static_cast(TestMode::PIDVel), - static_cast(TestMode::LimitSwitch), - static_cast(TestMode::ScienceTelemetry), - static_cast(TestMode::ScienceMotors), - static_cast(TestMode::ScienceServos)}; +std::unordered_set modes = { + static_cast(TestMode::ModeSet), static_cast(TestMode::PWM), + static_cast(TestMode::PID), static_cast(TestMode::Encoder), + static_cast(TestMode::PIDVel), static_cast(TestMode::LimitSwitch), + static_cast(TestMode::Telemetry), static_cast(TestMode::ScienceMotors), + static_cast(TestMode::ScienceServos)}; int prompt(std::string_view message) { std::string str; @@ -72,37 +69,32 @@ int main() { ss << static_cast(TestMode::PID) << " for PID\n"; ss << static_cast(TestMode::Encoder) << " for ENCODER\n"; ss << static_cast(TestMode::LimitSwitch) << " for LIMIT SWITCH\n"; - ss << static_cast(TestMode::ScienceTelemetry) << " for SCIENCE TELEMETRY\n"; + ss << static_cast(TestMode::Telemetry) << " for TELEMETRY\n"; ss << static_cast(TestMode::ScienceMotors) << " for SCIENCE MOTORS\n"; - ss << static_cast(TestMode::ScienceTelemetry) << " for SCIENCE SERVOS\n"; + ss << static_cast(TestMode::ScienceServos) << " for SCIENCE SERVOS\n"; int test_type = prompt(ss.str().c_str()); if (modes.find(test_type) == modes.end()) { std::cout << "Unrecognized response: " << test_type << std::endl; std::exit(1); } TestMode testMode = static_cast(test_type); - int serial = 0; - if (testMode == TestMode::PWM || testMode == TestMode::PID || - testMode == TestMode::Encoder || testMode == TestMode::LimitSwitch) { - serial = prompt("Enter motor serial"); - } bool mode_has_been_set = false; while (true) { if (testMode == TestMode::ModeSet) { - serial = prompt("Enter motor serial"); + int serial = prompt("Enter motor serial"); int mode = prompt("Enter mode (0 for PWM, 1 for PID)"); std::cout << "got " << serial << " and " << mode << std::endl; can::motor::setMotorMode(serial, mode == 0 ? motormode_t::pwm : motormode_t::pid); } else if (testMode == TestMode::PWM) { + int serial = prompt("Enter motor serial"); int pwm = prompt("Enter PWM"); can::motor::setMotorMode(serial, motormode_t::pwm); can::motor::setMotorPower(serial, static_cast(pwm)); } else if (testMode == TestMode::PID) { - // Don't send all five packets at once. On some motor boards, the CAN buffer - // only fits four packets. - + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); // AVR board firmware resets the angle target every time it receives a // mode set packet, so we only want to send this once. // TODO: do we need to set the PPJR? @@ -126,6 +118,8 @@ int main() { static double vel_timeout; if (!mode_has_been_set) { + int serial = prompt("Enter motor serial"); + // set pid mode can::motor::setMotorMode(serial, motormode_t::pid); mode_has_been_set = true; @@ -179,7 +173,10 @@ int main() { motor->setMotorPower(0.0); } } else if (testMode == TestMode::Encoder) { + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); + int sensorType; do { sensorType = @@ -212,7 +209,9 @@ int main() { std::this_thread::sleep_for(20ms); } else if (testMode == TestMode::LimitSwitch) { static bool testLimits = false; + static int serial; if (!mode_has_been_set) { + serial = prompt("Enter motor serial"); testLimits = static_cast(prompt("Set limits? 1=yes,0=no")); if (testLimits) { int lo = prompt("Low position"); @@ -242,28 +241,39 @@ int main() { std::cout << "\33[2K\rEncoder value: " << encoderStr << std::flush; std::this_thread::sleep_for(20ms); } - } else if (testMode == TestMode::ScienceTelemetry) { + } else if (testMode == TestMode::Telemetry) { if (!mode_has_been_set) { - serial = 0x01; - can::deviceid_t id = std::make_pair(can::devicegroup_t::science, serial); + auto group = static_cast(prompt("Enter group code")); + int serial = prompt("Enter serial"); + can::deviceid_t deviceID = std::make_pair(group, serial); + auto telemType = static_cast(prompt("Enter telemetry type")); can::addDeviceTelemetryCallback( - id, can::telemtype_t::limit_switch, + deviceID, telemType, [](can::deviceid_t id, can::telemtype_t telemType, DataPoint data) { - std::cout << "Science: serial=" << std::hex + std::cout << "Telemetry: group=" << std::hex + << static_cast(id.first) << ", serial=" << std::hex << static_cast(id.second) << ", type=" << std::hex << static_cast(telemType) << ", data=" << std::dec << data.getDataOrElse(0) << std::endl; }); + int telemPeriod = prompt("Telemetry timing (ms)"); + bool useTimingPacket = + static_cast(prompt("What telemetry method?\n0 for pull packets\n1 " + "for telemetry timing packet")); + if (useTimingPacket) { + CANPacket packet; + AssembleTelemetryTimingPacket( + &packet, static_cast(deviceID.first), deviceID.second, + static_cast(telemType), telemPeriod); + can::sendCANPacket(packet); + } else { + can::scheduleTelemetryPull(deviceID, telemType, + std::chrono::milliseconds(telemPeriod)); + } mode_has_been_set = true; } - auto reqType = prompt("Enter telemetry type"); - int telemPeriod = prompt("Telemetry timing (ms)"); - CANPacket packet; - AssembleTelemetryTimingPacket(&packet, - static_cast(can::devicegroup_t::science), - serial, reqType, telemPeriod); - can::sendCANPacket(packet); + std::this_thread::sleep_for(1s); } else if (testMode == TestMode::ScienceMotors) { // CAREFUL, there are no safety checks / limit switches // 0: drill up/down (down is positive, ~500 PWM) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 293ac622c..c968f6a60 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -109,7 +109,7 @@ find_package(nlohmann_json 3.2.0 REQUIRED) # Find the CAN library; contains packet and protocol definitions and does not # actually require physical CAN to be present. -find_package(HindsightCAN 1.3.2 REQUIRED) +find_package(HindsightCAN 1.4.0 REQUIRED) find_package(Threads REQUIRED) find_package(OpenCV REQUIRED) diff --git a/update_deps.sh b/update_deps.sh index 41a632aab..2a3fdd0b8 100755 --- a/update_deps.sh +++ b/update_deps.sh @@ -4,5 +4,5 @@ sudo apt update sudo apt install -y build-essential unzip gnupg2 lsb-release git \ cmake libeigen3-dev libopencv-dev libopencv-contrib-dev \ libwebsocketpp-dev libboost-system-dev gpsd gpsd-clients libgps-dev nlohmann-json3-dev \ - catch2 urg-lidar rplidar ublox-linux hindsight-can=1.3.2 frozen libargparse-dev libavutil-dev \ + catch2 urg-lidar rplidar ublox-linux hindsight-can=1.4.0 frozen libargparse-dev libavutil-dev \ libx264-dev