diff --git a/.gitignore b/.gitignore index 562ed72..f9b09a9 100644 --- a/.gitignore +++ b/.gitignore @@ -88,6 +88,7 @@ Thumbs.db *.pyc build/ +build_armhf/ doc/ .idea/ cmake-build-debug/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 5446560..c4aaf72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,13 @@ endif() # to correctly compile the codebase despite this line being in the # downstream CMake files. set (CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-psabi") + +option(GCC_DIAG_COLORS "Enable GCC Diagnostic Colors" ON) +if (GCC_DIAG_COLORS) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") +endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) diff --git a/appendages/CMakeLists.txt b/appendages/CMakeLists.txt index 064b31c..3be0960 100644 --- a/appendages/CMakeLists.txt +++ b/appendages/CMakeLists.txt @@ -5,11 +5,11 @@ project(appendages) file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/*.hpp") -add_library(${PROJECT_NAME} EXCLUDE_FROM_ALL ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +add_library(${PROJECT_NAME} STATIC EXCLUDE_FROM_ALL ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) target_link_libraries(${PROJECT_NAME} fmt json spdlog cppfs - cmd_messenger misc) + cmd_messenger units misc motor_controllers pid) target_include_directories(${PROJECT_NAME} PUBLIC include) set_property(GLOBAL PROPERTY AppendageRoot ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/appendages/include/appendages/bno055.hpp b/appendages/include/appendages/bno055.hpp new file mode 100644 index 0000000..17dccf0 --- /dev/null +++ b/appendages/include/appendages/bno055.hpp @@ -0,0 +1,61 @@ +#ifndef BNO055_HPP +#define BNO055_HPP + +#include +#include + +namespace rip +{ + namespace appendages + { + class Bno055 : public Appendage, public pid::PidInput + { + public: + + units::Angle getYaw(); + units::Angle getPitch(); + units::Angle getRoll(); + bool isCalibrated(); + + double get() override; + + void stop() override; + + bool diagnostic() override; + + protected: + friend class AppendageFactory; + + /** + * Function wrapper for the constructor so it can be pointed to + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + static std::shared_ptr create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + private: + /** + * Constructor + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + Bno055(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + std::shared_ptr m_yaw; + std::shared_ptr m_yaw_result; + std::shared_ptr m_pitch; + std::shared_ptr m_pitch_result; + std::shared_ptr m_roll; + std::shared_ptr m_roll_result; + std::shared_ptr m_calibrated; + std::shared_ptr m_caribrated_result; + + }; + } +} + +#endif //BNO055_HPP diff --git a/appendages/include/appendages/exceptions.hpp b/appendages/include/appendages/exceptions.hpp index c7ddb44..e2a7b23 100644 --- a/appendages/include/appendages/exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -17,8 +17,8 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef EXCEPTIONS_HPP -#define EXCEPTIONS_HPP +#ifndef APPENDAGES_EXCEPTIONS_HPP +#define APPENDAGES_EXCEPTIONS_HPP #include namespace rip @@ -28,7 +28,9 @@ namespace rip NEW_EX(CommandNotFound) NEW_EX(AppendageWithoutType) NEW_EX(AppendageWithoutLabel) - NEW_EX(AppendageWithId) + NEW_EX(AppendageWithoutId) + NEW_EX(AppendageMissingField) + NEW_EX(AppendageNotImplemented) } } -#endif // EXCEPTIONS_HPP +#endif // APPENDAGES_EXCEPTIONS_HPP diff --git a/appendages/include/appendages/ir_2018.hpp b/appendages/include/appendages/ir_2018.hpp new file mode 100644 index 0000000..29b5ac7 --- /dev/null +++ b/appendages/include/appendages/ir_2018.hpp @@ -0,0 +1,67 @@ +#ifndef IR_2018_HPP +#define IR_2018_HPP + +#include +#include +#include + + +#include + +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" + +#include + +namespace rip +{ + namespace appendages + { + class Ir2018 : public Appendage + { + public: + /** + * Reads the ir receiver for the 2018 challenge + * @returns Whether A, B, and C are right or left + */ + std::array read(); + + bool calibrate(); + + /** + * Stop + */ + virtual void stop() override; + + virtual bool diagnostic() override; + + protected: + friend class AppendageFactory; + + /** + * Function wrapper for the constructor so it can be pointed to + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + static std::shared_ptr create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + private: + /** + * Constructor + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + Ir2018(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + std::shared_ptr m_read; + std::shared_ptr m_read_result; + std::shared_ptr m_calibrate; + std::shared_ptr m_calibrate_result; + }; + } +} +#endif // IR_2018_HPP diff --git a/appendages/include/appendages/navx.hpp b/appendages/include/appendages/navx.hpp new file mode 100644 index 0000000..0fc6f6a --- /dev/null +++ b/appendages/include/appendages/navx.hpp @@ -0,0 +1,84 @@ +#ifndef NAVX_APPENDAGE_HPP +#define NAVX_APPENDAGE_HPP + +#include +#include +#include +#include +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" +#include +#include + +namespace rip +{ + namespace appendages + { + class NavX : public Appendage + { + public: + /** + * @brief returns yaw + * @return float + */ + units::Angle getYaw(); + /** + * @brief reports the pitch of the device + * @return float + */ + units::Angle getPitch(); + /** + * @brief reports the roll of the device + * @return float + */ + units::Angle getRoll(); + /** + * @brief gets the status of the device. + * @return code representing status of device + */ + char getStatus(); + /** + * Stop! ^0^ + */ + virtual void stop() override; + + virtual bool diagnostic() override; + + protected: + friend class AppendageFactory; + /** + * Function wrapper for the constructor so it can be pointed to + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + static std::shared_ptr create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + private: + /** + * Constructor + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + NavX(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + std::shared_ptr m_get_yaw; + std::shared_ptr m_get_yaw_result; + + std::shared_ptr m_get_pitch; + std::shared_ptr m_get_pitch_result; + + std::shared_ptr m_get_roll; + std::shared_ptr m_get_roll_result; + + std::shared_ptr m_get_status; + std::shared_ptr m_get_status_result; + + }; + } +} + +#endif // NAVX_APPENDAGE_HPP diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp new file mode 100644 index 0000000..9e86aa0 --- /dev/null +++ b/appendages/include/appendages/roboclaw.hpp @@ -0,0 +1,296 @@ +#ifndef ROBOCLAW_APPENDAGE_HPP +#define ROBOCLAW_APPENDAGE_HPP + +#include +#include +#include +#include +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" +#include +#include + +namespace rip +{ + namespace appendages + { + class Roboclaw : public Appendage + { + public: + /** + * @brief PID based drive with speed for motor 1 + * @param speed ticks / second + */ + void setM1Speed(int32_t speed); + /** + * @brief PID based drive with speed for motor 2 + * @param speed ticks / second + */ + void setM2Speed(int32_t speed); + /** + * @brief PID based drive with speed for motors 1 and 2 + * @param speed1 ticks / second + * @param speed2 ticks / second + */ + void setM1M2Speed(int32_t speed1, int32_t speed2); + /** + * @brief PID based drive with speed, accel for motor 1 + * @param accel ticks / second^2 + * @param speed ticks / second + */ + void setM1SpeedAccel(uint32_t accel, int32_t speed); + /** + * @brief PID based drive with speed, accel for motor 2 + * @param accel ticks / second^2 + * @param speed ticks / second + */ + void setM2SpeedAccel(uint32_t accel, int32_t speed); + /** + * @brief PID based drive with speed, accel for motors 1&2 + * @param accel ticks / second^2 + * @param speed1 ticks / second + * @param speed2 ticks / second + */ + void setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2); + /** + * @brief PID based drive with speed, dist for motor 1 + * @param speed ticks / second + * @param distance ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM1SpeedDist(int32_t speed, uint32_t distance, bool flag=1); + /** + * @brief PID based drive with speed, dist for motor 2 + * @param speed ticks / second + * @param distance ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM2SpeedDist(int32_t speed, uint32_t distance, bool flag=1); + /** + * @brief PID based drive with speed, dist for motors 1&2 + * @param speed1 ticks / second + * @param distance1 ticks + * @param speed2 ticks / second + * @param distance2 ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2, bool flag=1); + /** + * @brief PID based drive with speed, accel, dist for motor 1 + * @param accel ticks / second^2 + * @param speed ticks / second + * @param distance ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, bool flag=1); + /** + * @brief PID based drive with speed, accel, dist for motor 2 + * @param accel ticks / second^2 + * @param speed ticks / second + * @param distance ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, bool flag=1); + /** + * @brief PID based drive with speed, accel, dist for motors 1&2 + * @param accel ticks / second^2 + * @param speed1 ticks / second + * @param distance1 ticks + * @param speed2 ticks / second + * @param distance2 ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2, bool flag=1); + /** + * @brief PID based drive with speed, accel, decel, dist for motor 1 + * @param accel ticks / second^2 + * @param speed ticks / second + * @param deccel ticks / second^2 + * @param position ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, uint32_t position, bool flag=1); + /** + * @brief PID based drive with speed, accel, decel, dist for motors 2 + * @param accel ticks / second^2 + * @param speed ticks / second + * @param deccel ticks / second^2 + * @param position ticks + * @param flag 1 to apply instantly, 0 to buffer command + */ + void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position, bool flag=1); + /*void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, bool flag=1); + */ + /** + * @brief Reads the tick count from motor 1 + * @return ticks + */ + int32_t readM1Encoder(); + /** + * @brief Reads the tick count from motor 2 + * @return ticks + */ + int32_t readM2Encoder(); + /** + * @brief Reads the tick count from motors 1&2 + * @return ticks + */ + std::tuple readM1M2Encoders(); + /** + * @brief Reads encoder ticks/s for motor M1. + * @return Motor M1 ticks/ second + */ + int32_t readM1EncoderSpeed(); + /** + * @brief Reads encoder ticks per second for motor M1. + * @return Motor m2 ticks/ second + */ + int32_t readM2EncoderSpeed(); + /** + * @brief Reads encoder ticks/s for motors M1&M2. + * @return tuple containing int32_t for m1 & m2 respectively + */ + std::tuple readM1M2EncoderSpeed(); + /** + * @brief Resets encoders tick values. + */ + void resetEncoders(); + /** + * @brief: reads the number of commands in the buffer for + both motors, returns the buffer for just the specified motor. + Send: [Address, 47] + Receive: [BufferM1, BufferM2, CRC(2 bytes)] + + @returns Buffer length for motors 1 or 2 + * + */ + std::tuple getBuffers(); + /** + * @brief Sets the duty for motor M1. + * @param duty (-32727 - 32726) for motor 1 + */ + void setM1Duty(int16_t duty); + /** + * @brief Sets duty for motor m2. + * @param duty (-32727 - 32726) for motor 2 + */ + void setM2Duty(int16_t duty); + /** + * @brief sets duty for motors m1 and m2 + * @param duty1 (-32727 - 32726) for motor 1 + * @param duty2 (-32727 - 32726) for motor 2 + */ + void setM1M2Duty(int16_t duty1, int16_t duty2); + /** + * @brief sets the M1 PID constants for velocity on Roboclaw. + * @param Kp proportional + * @param Ki integral + * @param Kd derivative + * @param qpps maximum quadrature pulses per second + */ + void setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); + /** + * @brief sets the M2 PID constants for velocity on Roboclaw. + * @param Kp proportional + * @param Ki integral + * @param Kd derivative + * @param qpps Maximum quadrature pulses per second. + */ + void setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); + /** + * @brief Sets wheel speed based off provided angular velocity + * @param av unit of angular velocity + */ + void setAngularSpeed(bool motor, units::AngularVelocity av); + + /** + * @brief Returns the angle of the motor shaft + * @param motor 0 for m1, 1 for m2 + * @return offset angle of shaft relative to last encoder reset + */ + units::Angle getAngle(bool motor, bool continuous); + + + /** + * Stop! ^0^ + */ + virtual void stop() override; + + virtual bool diagnostic() override; + + protected: + friend class AppendageFactory; + + /** + * Function wrapper for the constructor so it can be pointed to + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + static std::shared_ptr create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + private: + /** + * Constructor + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + uint8_t m_address; + double m_ticks_per_rev; + + std::shared_ptr m_set_m1_speed; + std::shared_ptr m_set_m2_speed; + std::shared_ptr m_set_m1m2_speed; + + std::shared_ptr m_set_m1_speed_accel; + std::shared_ptr m_set_m2_speed_accel; + std::shared_ptr m_set_m1m2_speed_accel; + + std::shared_ptr m_set_m1_speed_dist; + std::shared_ptr m_set_m2_speed_dist; + std::shared_ptr m_set_m1m2_speed_dist; + + std::shared_ptr m_set_m1_speed_accel_dist; + std::shared_ptr m_set_m2_speed_accel_dist; + std::shared_ptr m_set_m1m2_speed_accel_dist; + + std::shared_ptr m_set_m1_speed_accel_decel_dist; + std::shared_ptr m_set_m2_speed_accel_decel_dist; + std::shared_ptr m_set_m1m2_speed_accel_decel_dist; + + std::shared_ptr m_read_m1_encoder; + std::shared_ptr m_read_m2_encoder; + std::shared_ptr m_read_m1m2_encoder; + std::shared_ptr m_read_m1_encoder_result; + std::shared_ptr m_read_m2_encoder_result; + std::shared_ptr m_read_m1m2_encoder_result; + + std::shared_ptr m_read_m1_encoder_speed; + std::shared_ptr m_read_m2_encoder_speed; + std::shared_ptr m_read_m1m2_encoder_speed; + std::shared_ptr m_read_m1_encoder_speed_result; + std::shared_ptr m_read_m2_encoder_speed_result; + std::shared_ptr m_read_m1m2_encoder_speed_result; + + std::shared_ptr m_reset_encoders; + std::shared_ptr m_get_buffers; + std::shared_ptr m_get_buffers_result; + + std::shared_ptr m_set_m1_duty; + std::shared_ptr m_set_m2_duty; + std::shared_ptr m_set_m1m2_duty; + + std::shared_ptr m_set_m1_velocity_pid; + std::shared_ptr m_set_m2_velocity_pid; + + }; + } +} + +#endif // ROBOCLAW_APPENDAGE_HPP diff --git a/appendages/include/appendages/servo.hpp b/appendages/include/appendages/servo.hpp index 30ccd70..47f0d50 100644 --- a/appendages/include/appendages/servo.hpp +++ b/appendages/include/appendages/servo.hpp @@ -25,6 +25,8 @@ namespace rip */ void write(int value); + void detach(); + /** * Stop */ @@ -55,6 +57,7 @@ namespace rip Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); std::shared_ptr m_write; + std::shared_ptr m_detach; }; } } diff --git a/appendages/json/bno055.json b/appendages/json/bno055.json new file mode 100644 index 0000000..63e5d76 --- /dev/null +++ b/appendages/json/bno055.json @@ -0,0 +1,3 @@ +{ + "address": "int" +} \ No newline at end of file diff --git a/appendages/json/ir2018.json b/appendages/json/ir2018.json new file mode 100644 index 0000000..0622edc --- /dev/null +++ b/appendages/json/ir2018.json @@ -0,0 +1,4 @@ +{ + "pin": "int", + "pullup": "string" +} diff --git a/appendages/json/navx.json b/appendages/json/navx.json new file mode 100644 index 0000000..49d1a20 --- /dev/null +++ b/appendages/json/navx.json @@ -0,0 +1,3 @@ +{ + +} diff --git a/appendages/json/roboclaw.json b/appendages/json/roboclaw.json new file mode 100644 index 0000000..639d3ba --- /dev/null +++ b/appendages/json/roboclaw.json @@ -0,0 +1,10 @@ +{ + "serial": "string", + "baudrate": "int", + "address": "int", + "ticks_per_rev": "float", + "core": [ + "address", + "ticks_per_rev" + ] +} diff --git a/appendages/json/servo.json b/appendages/json/servo.json new file mode 100644 index 0000000..d91013b --- /dev/null +++ b/appendages/json/servo.json @@ -0,0 +1,4 @@ +{ + "pin": "int", + "initialValue": "int" +} diff --git a/appendages/json/ultrasonic.json b/appendages/json/ultrasonic.json new file mode 100644 index 0000000..111bc0c --- /dev/null +++ b/appendages/json/ultrasonic.json @@ -0,0 +1,5 @@ +{ + "triggerPin": "int", + "echoPin": "int", + "minSleepMs": "int" +} diff --git a/appendages/src/appendage.cpp b/appendages/src/appendage.cpp index 081fa4d..ddc98a4 100644 --- a/appendages/src/appendage.cpp +++ b/appendages/src/appendage.cpp @@ -48,15 +48,23 @@ namespace rip if (config.find("label") == config.end()) { - throw AppendageWithoutLabel(fmt::format("appendage missing label")); + throw AppendageWithoutLabel(fmt::format("appendage of type {} is missing label", m_type)); } m_label = config["label"]; + if (config.find("id") == config.end() && config.find("index") == config.end()) + { + throw AppendageWithoutId(fmt::format("appendage labeled {} is missing an id or index", m_label)); + } + if (config.find("id") == config.end()) { - throw AppendageWithId(fmt::format("appendage missing id")); + m_id = config["index"]; + } + else + { + m_id = config["id"]; } - m_id = config["id"]; } } } diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index ae785d3..681e7da 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -5,6 +5,12 @@ // Appendages #include "appendages/digital_input.hpp" #include "appendages/analog_input.hpp" +#include "appendages/roboclaw.hpp" +#include "appendages/servo.hpp" +#include "appendages/ultrasonic.hpp" +#include "appendages/navx.hpp" +#include "appendages/ir_2018.hpp" +#include "appendages/bno055.hpp" #include "appendages/exceptions.hpp" @@ -29,8 +35,14 @@ namespace rip { throw AppendageWithoutType(fmt::format("appendage missing type")); } - - return m_constructors["type"](config, command_map, device); + std::string appendage_type = config["type"]; + if (m_constructors.find(appendage_type) == m_constructors.end()) + { + // we probably don't have this appendage type available to the factory + throw AppendageNotImplemented(fmt::format("Factory: no Constructor for appendage type {} !", appendage_type)); + } + misc::Logger::getInstance()->debug(fmt::format("Factory: Constructing an appendage of type {} ...", appendage_type)); + return m_constructors[appendage_type](config, command_map, device); } void AppendageFactory::registerAppendage(const std::string& type, std::function(const nlohmann::json&, @@ -45,6 +57,12 @@ namespace rip { registerAppendage("Digital Input", &DigitalInput::create); registerAppendage("Analog Input", &AnalogInput::create); + registerAppendage("Roboclaw", &Roboclaw::create); + registerAppendage("Servo", &Servo::create); + registerAppendage("Ultrasonic", &Ultrasonic::create); + registerAppendage("Ir2018", &Ir2018::create); + registerAppendage("NavX", &NavX::create); + registerAppendage("Bno055", &Bno055::create); } } } diff --git a/appendages/src/bno055.cpp b/appendages/src/bno055.cpp new file mode 100644 index 0000000..d47416f --- /dev/null +++ b/appendages/src/bno055.cpp @@ -0,0 +1,81 @@ +#include "appendages/bno055.hpp" + +#include + +namespace rip +{ + namespace appendages + { + Bno055::Bno055(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + : Appendage(config, device) + , m_yaw(createCommand("kGetYaw", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_yaw_result(createCommand("kGetYawResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_pitch(createCommand("kGetPitch", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_pitch_result(createCommand("kGetPitchResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_roll(createCommand("kGetRoll", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_roll_result(createCommand("kGetRollResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_calibrated(createCommand("kGetCalibrationStatus", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_caribrated_result(createCommand("kGetCalibrationStatusResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + + { + } + + units::Angle Bno055::getYaw() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_yaw, m_id); + return std::get<0>(messenger.receive(m_yaw_result)) * units::deg; + } + + units::Angle Bno055::getPitch() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_pitch, m_id); + return std::get<0>(messenger.receive(m_pitch_result)) * units::deg; + } + + units::Angle Bno055::getRoll() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_roll, m_id); + return std::get<0>(messenger.receive(m_roll_result)) * units::deg; + } + + bool Bno055::isCalibrated() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_calibrated, m_id); + return std::get<0>(messenger.receive(m_caribrated_result)); + } + + double Bno055::get() + { + return getYaw()(); + } + + void Bno055::stop() + { + } + + bool Bno055::diagnostic() + { + misc::Logger::getInstance()->debug("Starting IMU appendage diagnostic utility"); + misc::Logger::getInstance()->debug("Reading yaw, pitch, roll for 5s"); + std::chrono::time_point start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + misc::Logger::getInstance()->debug("Yaw: {} Pitch: {} Roll: {}", getYaw().to(units::deg), getPitch().to(units::deg), getRoll().to(units::deg)); + std::this_thread::sleep_for(std::chrono::milliseconds(125)); + } + misc::Logger::getInstance()->debug("IMU appendage diagnostics complete"); + + return true; + } + + std::shared_ptr Bno055::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + { + return std::dynamic_pointer_cast(std::shared_ptr(new Bno055(config, command_map, device))); + } + } +} \ No newline at end of file diff --git a/appendages/src/ir_2018.cpp b/appendages/src/ir_2018.cpp new file mode 100644 index 0000000..1039a79 --- /dev/null +++ b/appendages/src/ir_2018.cpp @@ -0,0 +1,66 @@ +#include "appendages/ir_2018.hpp" + +#include +#include +#include + +#include + +namespace rip +{ + namespace appendages + { + Ir2018::Ir2018(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + : Appendage(config, device) + , m_read(createCommand("kReadIr2018", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_read_result(createCommand("kReadIr2018Result", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_calibrate(createCommand("kCalibrateIr2018", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_calibrate_result(createCommand("kCalibrateIr2018Result", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + { + } + + std::array Ir2018::read() + { + std::array rv; + + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read, m_id); + char data = std::get<0>(messenger.receive(m_read_result)); + rv[0] = !(data & (1 << 4)); + rv[1] = data & (1 << 3); + if(rv[0]) + { + rv[2] = data & (1 << 0); + rv[3] = data & (1 << 1); + rv[4] = data & (1 << 2); + } + + misc::Logger::getInstance()->debug("Ir: {} {} {} {} {}", static_cast(data & 1 << 0), static_cast(data & 1 << 1), static_cast(data & 1 << 2), static_cast(data & 1 << 3), static_cast(data & 1 << 4)); + + return rv; + } + + bool Ir2018::calibrate() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_calibrate, m_id); + return std::get<0>(messenger.receive(m_calibrate_result)); + } + + void Ir2018::stop() + { + + } + + bool Ir2018::diagnostic() + { + // todo + return true; + } + + std::shared_ptr Ir2018::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + { + return std::dynamic_pointer_cast(std::shared_ptr(new Ir2018(config, command_map, device))); + } + } +} diff --git a/appendages/src/navx.cpp b/appendages/src/navx.cpp new file mode 100644 index 0000000..d445e84 --- /dev/null +++ b/appendages/src/navx.cpp @@ -0,0 +1,72 @@ +#include "appendages/navx.hpp" +#include +#include +#include +#include +#include +#include + +namespace rip +{ + namespace appendages + { + NavX::NavX(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + : Appendage(config, device), m_get_yaw(createCommand("kGetYaw", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString<>())), m_get_yaw_result(createCommand("kGetYawResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_get_pitch(createCommand("kGetPitch", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString<>())), m_get_pitch_result(createCommand("kGetPitchResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_get_roll(createCommand("kGetRoll", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString<>())), m_get_roll_result(createCommand("kGetRollResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_get_status(createCommand("kGetStatus", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString<>())), m_get_status_result(createCommand("kGetStatusResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + { + } + + std::shared_ptr NavX::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + { + return std::dynamic_pointer_cast(std::shared_ptr(new NavX(config, command_map, device))); + } + + units::Angle NavX::getYaw() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send<>(m_device, m_get_yaw); + return std::get<0>(messenger.receive(m_get_yaw_result)) * units::deg; + } + + units::Angle NavX::getPitch() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send<>(m_device, m_get_pitch); + return std::get<0>(messenger.receive(m_get_pitch_result)) * units::deg; + } + + units::Angle NavX::getRoll() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send<>(m_device, m_get_roll); + return std::get<0>(messenger.receive(m_get_roll_result)) * units::deg; + } + + char NavX::getStatus() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send<>(m_device, m_get_status); + return std::get<0>(messenger.receive(m_get_status_result)); + } + + void NavX::stop() + { + // Nothing stands in the way of the navx. The NavX cannot be stopped. + } + + bool NavX::diagnostic() + { + misc::Logger::getInstance()->debug("Starting NavX appendage diagnostic utility"); + misc::Logger::getInstance()->debug("Reading yaw, pitch, roll for 5s"); + std::chrono::time_point start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + misc::Logger::getInstance()->debug("Yaw: {} Pitch: {} Roll: {}", getYaw().to(units::deg), getPitch().to(units::deg), getRoll().to(units::deg)); + std::this_thread::sleep_for(std::chrono::milliseconds(125)); + } + misc::Logger::getInstance()->debug("NavX appendage diagnostics complete"); + + return true; + } + } +} diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp new file mode 100644 index 0000000..66ca992 --- /dev/null +++ b/appendages/src/roboclaw.cpp @@ -0,0 +1,652 @@ +#include "appendages/roboclaw.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include "appendages/exceptions.hpp" + +namespace rip +{ + namespace appendages + { + Roboclaw::Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + : Appendage(config, device), + m_set_m1_speed(createCommand("kSetM1Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed(createCommand("kSetM2Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed(createCommand("kSetM1M2Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_accel(createCommand("kSetM1SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel(createCommand("kSetM2SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel(createCommand("kSetM1M2SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_dist(createCommand("kSetM1SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_dist(createCommand("kSetM2SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_dist(createCommand("kSetM1M2SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_accel_dist(createCommand("kSetM1SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel_dist(createCommand("kSetM2SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_accel_decel_dist(createCommand("kSetM1SpeedAccelDecelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel_decel_dist(createCommand("kSetM2SpeedAccelDecelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDecelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder(createCommand("kReadM1Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder(createCommand("kReadM2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder(createCommand("kReadM1M2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder_result(createCommand("kReadM1EncoderResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder_result(createCommand("kReadM2EncoderResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder_result(createCommand("kReadM1M2EncoderResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder_speed(createCommand("kReadM1EncoderSpeed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder_speed(createCommand("kReadM2EncoderSpeed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder_speed(createCommand("kReadM1M2EncoderSpeed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder_speed_result(createCommand("kReadM1EncoderSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder_speed_result(createCommand("kReadM2EncoderSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder_speed_result(createCommand("kReadM1M2EncoderSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_reset_encoders(createCommand("kResetEncoders", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_get_buffers(createCommand("kGetBuffers", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_get_buffers_result(createCommand("kGetBuffersResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_duty(createCommand("kSetM1Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_duty(createCommand("kSetM2Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_duty(createCommand("kSetM1M2Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_velocity_pid(createCommand("kSetM2VelocityPID", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_velocity_pid(createCommand("kSetM2VelocityPID", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + { + if (config.find("address") == config.end()) + { + throw AppendageMissingField(fmt::format("Roboclaw: missing config field 'address'")); + } + m_address = config["address"]; + if (config.find("ticks_per_rev") == config.end()) + { + throw AppendageMissingField(fmt::format("Roboclaw: missing config field 'address'")); + } + m_ticks_per_rev = config.at("ticks_per_rev"); + } + + std::shared_ptr Roboclaw::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + { + return std::dynamic_pointer_cast(std::shared_ptr(new Roboclaw(config, command_map, device))); + } + + void Roboclaw::setM1Speed(int32_t speed) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed, m_id, + m_address, speed); + } + + void Roboclaw::setM2Speed(int32_t speed) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed, m_id, + m_address, speed); + } + + void Roboclaw::setM1M2Speed(int32_t speed1, int32_t speed2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed, m_id, + m_address, speed1, speed2); + } + + void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel, m_id, + m_address, accel, speed); + } + + void Roboclaw::setM2SpeedAccel(uint32_t accel, int32_t speed) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel, m_id, + m_address, accel, speed); + } + + void Roboclaw::setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_accel, m_id, + m_address, accel, speed1, speed2); + } + + void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_dist, m_id, + m_address, speed, distance, static_cast(flag)); + } + + void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_dist, m_id, + m_address, speed, distance, static_cast(flag)); + } + + void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, + uint32_t distance2, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_dist, m_id, + m_address, speed1, distance1, speed2, distance2, static_cast(flag)); + } + + void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel_dist, m_id, + m_address, accel, speed, distance, static_cast(flag)); + } + + void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel_dist, m_id, + m_address, accel, speed, distance, static_cast(flag)); + } + + void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, + int32_t speed2, uint32_t distance2, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_accel_dist, m_id, + m_address, accel, speed1, distance1, speed2, distance2, static_cast(flag)); + } + + void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, + uint32_t position, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel_decel_dist, m_id, + m_address, accel, speed, deccel, position, static_cast(flag)); + } + + void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, + uint32_t position, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel_decel_dist, m_id, + m_address, accel, speed, deccel, position, static_cast(flag)); + } + + /*void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, bool flag) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_accel_decel_dist, m_id, + m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, static_cast(flag)); + }*/ + + int32_t Roboclaw::readM1Encoder() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1_encoder, m_id, m_address); + return std::get<0>(messenger.receive(m_read_m1_encoder_result)); + } + + int32_t Roboclaw::readM2Encoder() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m2_encoder, m_id, m_address); + return std::get<0>(messenger.receive(m_read_m2_encoder_result)); + } + + std::tuple Roboclaw::readM1M2Encoders() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1m2_encoder, m_id, m_address); + return messenger.receive(m_read_m1m2_encoder_result); + } + + int32_t Roboclaw::readM1EncoderSpeed() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1_encoder_speed, m_id, m_address); + return std::get<0>(messenger.receive(m_read_m1_encoder_speed_result)); + } + + int32_t Roboclaw::readM2EncoderSpeed() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m2_encoder_speed, m_id, m_address); + return std::get<0>(messenger.receive(m_read_m2_encoder_speed_result)); + } + + std::tuple Roboclaw::readM1M2EncoderSpeed() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1m2_encoder_speed, m_id, m_address); + return messenger.receive(m_read_m1m2_encoder_speed_result); + } + + void Roboclaw::resetEncoders() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_reset_encoders, m_id, m_address); + } + + std::tuple Roboclaw::getBuffers() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_get_buffers, m_id, m_address); + return messenger.receive(m_get_buffers_result); + } + + void Roboclaw::setM1Duty(int16_t duty) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_duty, m_id, + m_address, duty); + } + + void Roboclaw::setM2Duty(int16_t duty) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_duty, m_id, + m_address, duty); + } + + void Roboclaw::setM1M2Duty(int16_t duty1, int16_t duty2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_duty, m_id, + m_address, duty1, duty2); + } + + void Roboclaw::setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_velocity_pid, m_id, + m_address, Kp, Ki, Kd, qpps); + } + + void Roboclaw::setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_velocity_pid, m_id, + m_address, Kp, Ki, Kd, qpps); + } + + units::Angle Roboclaw::getAngle(bool motor, bool continuous) + { + int32_t ticks; + units::Angle rv; + if(motor) + { + ticks = readM2Encoder(); + } + else + { + ticks = readM1Encoder(); + } + if(!continuous) + { + ticks %= static_cast(m_ticks_per_rev); + } + rv = (ticks/m_ticks_per_rev) * 360 * units::deg; + return rv; + } + + void Roboclaw::setAngularSpeed(bool motor, units::AngularVelocity av) + { + if(motor) + { + setM2Speed(av.to(units::deg/units::s) / 360 * m_ticks_per_rev); + } + else + { + setM1Speed(av.to(units::deg/units::s) / 360 * m_ticks_per_rev); + } + } + + void Roboclaw::stop() + { + setM1M2Duty(0, 0); + } + + bool Roboclaw::diagnostic() + { + int32_t ticks; + + std::chrono::time_point start_time = std::chrono::system_clock::now(); + misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); + //encoder reading diag + misc::Logger::getInstance()->debug("Read encoders for 5s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + start_time = std::chrono::system_clock::now(); + //buffer reading diag + misc::Logger::getInstance()->debug("Getting buffers for 2s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 2000) + { + misc::Logger::getInstance()->debug(fmt::format("Buffer M1: {} M2: {}", std::get<0>(getBuffers()), std::get<1>(getBuffers()))); + } + start_time = std::chrono::system_clock::now(); + //encoder resetting diag + misc::Logger::getInstance()->debug("Resetting encoders for 2s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 2000) + { + resetEncoders(); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + } + start_time = std::chrono::system_clock::now(); + //duty setting diag + misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1M2Duty(16000, 16000); + setM1Duty(16000); + setM2Duty(16000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed accel drive diag + misc::Logger::getInstance()->debug("Setting speed accel drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1M2SpeedAccel(12000, 12000, 12000); + setM1SpeedAccel(12000, 12000); + setM2SpeedAccel(12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + misc::Logger::getInstance()->debug("Setting speed drive (5s)"); + start_time = std::chrono::system_clock::now(); + //speed drive diag + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1Speed(12000); + setM2Speed(12000); + setM1M2Speed(12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed dist drive diag + misc::Logger::getInstance()->debug("Setting speed dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedDist(12000, 12000); + setM2SpeedDist(12000, 12000); + setM1M2SpeedDist(12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed accel dist drive diag + misc::Logger::getInstance()->debug("Setting speed accel dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedAccelDist(12000, 12000, 12000); + setM2SpeedAccelDist(12000, 12000, 12000); + setM1M2SpeedAccelDist(12000, 12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + /* + //speed accel decel dist drive diag + misc::Logger::getInstance()->debug("Setting speed accel decel dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedAccelDecelDist(12000, 12000, 12000, 12000); + setM2SpeedAccelDecelDist(12000, 12000, 12000, 12000); + + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + */ + + misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics complete"); + + return 1; + } + } +} diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index e20d711..10e4c8f 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -6,13 +6,31 @@ #include +using namespace rip::utilities; + namespace rip { namespace appendages { Servo::Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) - , m_write(createCommand("kServoWrite", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_write( + createCommand( + "kSetServo", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::IntegerType>() + ) + ) + , m_detach( + createCommand( + "kDetachServo", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType>() + ) + ) { } @@ -24,12 +42,38 @@ namespace rip void Servo::stop() { + detach(); + } + void Servo::detach() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_detach, m_id); } bool Servo::diagnostic() { - return true; + int new_val = 0; + std::cout << " === DIAG CONTROLS: === " << '\n'; + std::cout << " '-1': Quit." << '\n'; + std::cout << " '-2': Detach servo." << '\n'; + std::cout << " '0-180': Set servo position." << '\n'; + while (new_val != -1) { + // write(0); + std::cout << " >>> Please enter a servo value (int) to write (-1 quits): "; + std::cin >> new_val; + std::cout << " >>> Working...\n"; + if (new_val == -1) break; + else if (new_val == -2) + { + detach(); + } + else + { + write(new_val); + } + } + return true; } std::shared_ptr Servo::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) diff --git a/appendages/src/ultrasonic.cpp b/appendages/src/ultrasonic.cpp index 20270b5..426e4a2 100644 --- a/appendages/src/ultrasonic.cpp +++ b/appendages/src/ultrasonic.cpp @@ -3,6 +3,10 @@ #include #include #include +#include +#include + +#include #include @@ -12,8 +16,8 @@ namespace rip { Ultrasonic::Ultrasonic(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) - , m_read(createCommand("kUltrasonicRead", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) - , m_read_result(createCommand("kUltrasonicReadResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_read(createCommand("kReadUltrasonic", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_read_result(createCommand("kReadUltrasonicResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) { } @@ -21,16 +25,25 @@ namespace rip { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read, m_id); - return std::get<0>(messenger.receive(m_read_result)) * units::in; + return std::get<0>(messenger.receive(m_read_result)) * units::cm; } void Ultrasonic::stop() { - + // don't need to do anything. } bool Ultrasonic::diagnostic() { + std::chrono::time_point start_time = std::chrono::system_clock::now(); + misc::Logger::getInstance()->info("Reading the ultrasonic value for 10s."); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) + { + misc::Logger::getInstance()->info(fmt::format("Distance: {} cm", read().to(units::cm))); + } + + misc::Logger::getInstance()->info("Ultrasonic diag finished."); return true; } diff --git a/appendages/xml/bno055.xml b/appendages/xml/bno055.xml new file mode 100644 index 0000000..a1e17bd --- /dev/null +++ b/appendages/xml/bno055.xml @@ -0,0 +1,57 @@ + + + Wire.h + Adafruit_Sensor.h + Adafruit_BNO055.h + + + + + + + + bno055s[$i$].begin(); + bno055s[$i$].setExtCrystalUse(true); + + + + + + /* Get a new sensor event */ + sensors_event_t event; + bno055s[indexNum].getEvent(&event); + + // In degrees + rv = event.orientation.x; + + + + + + /* Get a new sensor event */ + sensors_event_t event; + bno055s[indexNum].getEvent(&event); + + // In degrees + rv = event.orientation.y; + + + + + + /* Get a new sensor event */ + sensors_event_t event; + bno055s[indexNum].getEvent(&event); + + // In degrees + rv = event.orientation.z; + + + + + + rv = bno055s[indexNum].isFullyCalibrated(); + + + + diff --git a/appendages/xml/digital_input.xml b/appendages/xml/digital_input.xml index 913c199..3fb2341 100644 --- a/appendages/xml/digital_input.xml +++ b/appendages/xml/digital_input.xml @@ -8,7 +8,7 @@ - + rv = digitalRead(digital_input_pins[index_num]); diff --git a/appendages/xml/ir2018.xml b/appendages/xml/ir2018.xml new file mode 100644 index 0000000..23af599 --- /dev/null +++ b/appendages/xml/ir2018.xml @@ -0,0 +1,81 @@ + + + + + + + + + pinMode(ir_2018_pins[$i$], INPUT$pullup$); + + + + + + + rv = digitalRead(ir_2018_pins[indexNum]); + + + + + + long start = millis(); + + // Wait for start + while(pulseIn(ir_2018_pins[indexNum], LOW) < 8000) + { + // After a half second tell the pi we don't know + if(millis() - start > 500) + { + rv = 1 << 4; + cmdMessenger.sendBinCmd(kAcknowledge, kReadIr2018); + cmdMessenger.sendCmdStart(kReadIr2018Result); + cmdMessenger.sendCmdBinArg(rv); + cmdMessenger.sendCmdEnd(); + return; + } + } + + // Wait through empty pulses + for(int i = 0; i < 5; ++i) + { + // If any of the first 5 are 1 then the run + // hasn't started yet + if(pulseIn(ir_2018_pins[indexNum], HIGH) > 1000) + { + rv = (1 << 4) + (1 << 3); + cmdMessenger.sendBinCmd(kAcknowledge, kReadIr2018); + cmdMessenger.sendCmdStart(kReadIr2018Result); + cmdMessenger.sendCmdBinArg(rv); + cmdMessenger.sendCmdEnd(); + return; + } + } + + // Store pulse in bins + boolean bin[3]; + for(int i = 0; i < 3; ++i) + { + if(pulseIn(ir_2018_pins[indexNum], HIGH) > 1000) + { + bin[i] = true; + } + else + { + bin[i] = false; + } + } + + // Bit shifting + rv = 0; + for(int i = 0; i < 3; ++i) + { + if(bin[i]) + { + rv += 1 << i; + } + } + + + + diff --git a/appendages/xml/navx.xml b/appendages/xml/navx.xml new file mode 100644 index 0000000..17d5b60 --- /dev/null +++ b/appendages/xml/navx.xml @@ -0,0 +1,39 @@ + + + Wire.h + NavX.h + + + + + + + Wire.begin(); + + + + + + rv = NavX::getYaw(); + + + + + + rv = NavX::getPitch(); + + + + + + rv = NavX::getRoll(); + + + + + + rv = NavX::getStatus(); + + + + diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml new file mode 100644 index 0000000..7e452d0 --- /dev/null +++ b/appendages/xml/roboclaw.xml @@ -0,0 +1,378 @@ + + + RoboClaw.h + + + + + + + + + roboclaws[$i$].begin($baudrate$); + + + + + + + + if (!roboclaws[indexNum].SpeedM1(address, speed)) + { + cmdMessenger.sendBinCmd(kError, kSetM1Speed); + } + + + + + + + if (!roboclaws[indexNum].SpeedM2(address, speed)) + { + cmdMessenger.sendBinCmd(kError, kSetM2Speed); + } + + + + + + + + if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2Speed); + } + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1(address, accel, speed)) + { + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccel); + } + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM2(address, accel, speed)) + { + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccel); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccel); + } + + + + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1SpeedDist); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM2SpeedDist); + } + + + + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedDist); + } + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDist); + } + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDist); + } + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccelDist); + } + + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDecelDist); + } + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); + } + + + + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccelDecelDist); + } + + + + + + + + + uint32_t tmp; + + if (!roboclaws[indexNum].ReadEncoders(address, enc1, tmp)) + { + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); + } + + + + + + + uint32_t tmp; + + if (!roboclaws[indexNum].ReadEncoders(address, tmp, enc2)) + { + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); + } + + + + + + + + if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) + { + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); + } + + + + + + + + + bool valid; + uint8_t status; + enc1Speed = roboclaws[indexNum].ReadSpeedM1(address, &status, &valid); + if (!valid) + { + cmdMessenger.sendBinCmd(kError, kReadM1EncoderSpeed); + } + + + + + + + + bool valid; + uint8_t status; + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &valid); + if (!valid) + { + cmdMessenger.sendBinCmd(kError, kReadM2EncoderSpeed); + } + + + + + + + + bool validM1, validM2; + uint8_t status; + enc1Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &validM1); + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &validM2); + + if (!validM1 || !validM2) + { + cmdMessenger.sendBinCmd(kError, kReadM1M2EncoderSpeed); + } + + + + + + + + + if (!roboclaws[indexNum].DutyM1(address, duty)) + { + cmdMessenger.sendBinCmd(kError, kSetM1Duty); + } + + + + + + + if (!roboclaws[indexNum].DutyM2(address, duty)) + { + cmdMessenger.sendBinCmd(kError, kSetM2Duty); + } + + + + + + + + if (!roboclaws[indexNum].DutyM1M2(address, duty1, duty2)) + { + cmdMessenger.sendBinCmd(kError, kSetM1M2Duty); + } + + + + + + + + + + + + if (!roboclaws[indexNum].SetM1VelocityPID(address, kp, ki, kd, qpps)) + { + cmdMessenger.sendBinCmd(kError, kSetM1VelocityPID); + } + + + + + + + + + + if (!roboclaws[indexNum].SetM2VelocityPID(address, kp, ki, kd, qpps)) + { + cmdMessenger.sendBinCmd(kError, kSetM2VelocityPID); + } + + + + + + + + if (!roboclaws[indexNum].ReadBuffers(address, buff1, buff2)) + { + cmdMessenger.sendBinCmd(kError, kGetBuffers); + } + + + + + + if (!roboclaws[indexNum].ResetEncoders(address)) + { + cmdMessenger.sendBinCmd(kError, kGetBuffers); + } + + + + diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml new file mode 100644 index 0000000..0f46634 --- /dev/null +++ b/appendages/xml/servo.xml @@ -0,0 +1,40 @@ + + + Servo.h + + + + + + + + + + + + + servos[$i$].attach($pin$); + servos[$i$].write($initialValue$); + delay(500); + servos[$i$].detach(); + + + // Servo pin: $pin$ + + + + + + if (!servos[indexNum].attached()) { + servos[indexNum].attach(servo_pins[indexNum]); + } + servos[indexNum].write(value); + + + + + servos[indexNum].detach(); + + + + diff --git a/appendages/xml/ultrasonic.xml b/appendages/xml/ultrasonic.xml new file mode 100644 index 0000000..5789321 --- /dev/null +++ b/appendages/xml/ultrasonic.xml @@ -0,0 +1,43 @@ + + + NewPing.h + + + + + + + + + + + + + + + + + + + // Ultrasonic triggerPin: $triggerPin$ + + + // Ultrasonic echoPin: $echoPin$ + + + + + + unsigned long curtime = millis(); + if (curtime < sonar_prevtime[indexNum]+sonar_timeouts[indexNum]) + { + delay(sonar_prevtime[indexNum]+sonar_timeouts[indexNum] - curtime ); + } + //rv = sonar[indexNum].ping_cm(); + unsigned long median_time = sonar[indexNum].ping_median(sonar_ping_medians[indexNum]); + rv = NewPing::convert_cm(median_time); + sonar_prevtime[indexNum] = millis(); + + + + diff --git a/arduino_gen/CMakeLists.txt b/arduino_gen/CMakeLists.txt index 1604f43..8184399 100644 --- a/arduino_gen/CMakeLists.txt +++ b/arduino_gen/CMakeLists.txt @@ -3,16 +3,18 @@ project(arduino_gen) # Get all .cpp files except for main.cpp file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/*.hpp") list(REMOVE_ITEM ${PROJECT_NAME}_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp") +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/arduino_gen/*.hpp") # Create the arduino_gen library, set to c++11, link external libraries, and set include dir -add_library(_${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) +add_library(_${PROJECT_NAME} STATIC ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET _${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage -DTESTING") + target_link_libraries(_${PROJECT_NAME} gmock gtest googletest_rip_macros) endif() target_link_libraries(_${PROJECT_NAME} fmt json spdlog tinyxml2 cppfs misc) -target_link_libraries(_${PROJECT_NAME} gtest) target_include_directories(_${PROJECT_NAME} PUBLIC include) include(FileOutputs) diff --git a/arduino_gen/code_template.txt b/arduino_gen/code_template.txt index 2ccebfb..45b0607 100644 --- a/arduino_gen/code_template.txt +++ b/arduino_gen/code_template.txt @@ -10,7 +10,7 @@ const char LED = 13; {constructors} -enum +enum RIPenum : int16_t {{ {command_enums} }}; @@ -57,7 +57,7 @@ void attachCommandCallbacks() // Called when a received command has no attached function void unknownCommand() {{ - cmdMessenger.sendCmd(kError, kUnknown); + cmdMessenger.sendBinCmd(kError, kUnknown); }} // Called upon initialization of Spine to check connection diff --git a/arduino_gen/include/arduino_gen/appendage.hpp b/arduino_gen/include/arduino_gen/appendage.hpp index 9bc0c21..c1f64cb 100644 --- a/arduino_gen/include/arduino_gen/appendage.hpp +++ b/arduino_gen/include/arduino_gen/appendage.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -89,13 +90,15 @@ namespace rip * @exception ParameterInvalid This exception is thrown if a parameter is missing, * included but not in the template, or has the wrong type. */ - void testType() const; + void testType(); nlohmann::json m_data; - static std::map< std::string, std::map< std::string, std::string > > m_type_cache; + static std::map< std::string, nlohmann::json > m_type_cache; std::string m_appendage_data_folder; + + std::vector m_core_fields; }; } } diff --git a/arduino_gen/include/arduino_gen/arduino_gen.hpp b/arduino_gen/include/arduino_gen/arduino_gen.hpp index b734a74..207c5fe 100644 --- a/arduino_gen/include/arduino_gen/arduino_gen.hpp +++ b/arduino_gen/include/arduino_gen/arduino_gen.hpp @@ -1,7 +1,9 @@ #ifndef ARDUINO_GEN_H #define ARDUINO_GEN_H +#ifdef TESTING #include +#endif #include #include diff --git a/arduino_gen/include/arduino_gen/argument.hpp b/arduino_gen/include/arduino_gen/argument.hpp index 30e6481..aaae0fe 100644 --- a/arduino_gen/include/arduino_gen/argument.hpp +++ b/arduino_gen/include/arduino_gen/argument.hpp @@ -56,6 +56,8 @@ namespace rip std::string m_name; std::string m_type; std::string m_value; + std::string m_prefix; + std::string m_suffix; }; } // arduinogen } diff --git a/arduino_gen/include/arduino_gen/exceptions.hpp b/arduino_gen/include/arduino_gen/exceptions.hpp index 3a1c97e..6642034 100644 --- a/arduino_gen/include/arduino_gen/exceptions.hpp +++ b/arduino_gen/include/arduino_gen/exceptions.hpp @@ -1,5 +1,5 @@ -#ifndef EXCEPTIONS_HPP -#define EXCEPTIONS_HPP +#ifndef ARDUINO_GEN_EXCEPTIONS_HPP +#define ARDUINO_GEN_EXCEPTIONS_HPP #include #include @@ -45,4 +45,4 @@ namespace rip } } -#endif // EXCEPTIONS_HPP +#endif // ARDUINO_GEN_EXCEPTIONS_HPP diff --git a/arduino_gen/src/appendage.cpp b/arduino_gen/src/appendage.cpp index 5ea1382..b291952 100644 --- a/arduino_gen/src/appendage.cpp +++ b/arduino_gen/src/appendage.cpp @@ -11,8 +11,7 @@ namespace rip { namespace arduinogen { - std::map< std::string, std::map< std::string, std::string> > Appendage::m_type_cache = - std::map< std::string, std::map< std::string, std::string> >(); + std::map< std::string, nlohmann::json > Appendage::m_type_cache = std::map< std::string, nlohmann::json >(); Appendage::Appendage(nlohmann::json j, std::multimap< std::string, std::shared_ptr >& appendages, @@ -109,7 +108,7 @@ namespace rip } } - void Appendage::testType() const + void Appendage::testType() { std::string type = getType(); @@ -149,69 +148,83 @@ namespace rip nlohmann::json j; (*type_template.createInputStream()) >> j; - std::map< std::string, std::string > temp; - for (nlohmann::json::iterator it = j.begin(); it != j.end(); ++it) { - temp[it.key()] = it.value(); - } - m_type_cache[type] = temp; + m_type_cache[type] = j; } - std::map< std::string, std::string >& type_check = m_type_cache[type]; + nlohmann::json& type_check = m_type_cache[type]; // Check if the appendage has all the parameters specified by the tempate // and that they are the correct type - for(std::pair< std::string, std::string> type_parameter : type_check) + for (nlohmann::json::iterator type_parameter = type_check.begin(); type_parameter != type_check.end(); ++type_parameter) { - if(m_data.find(type_parameter.first) == m_data.end()) + if (type_parameter.key() != "core" && m_data.find(type_parameter.key()) == m_data.end()) { - throw AppendageDataException(fmt::format("{} missing on {}", type_parameter.first, label)); + throw AppendageDataException(fmt::format("{} missing on {}", type_parameter.key(), label)); } - nlohmann::json parameter = m_data[type_parameter.first]; + nlohmann::json parameter = m_data[type_parameter.key()]; + + if (type_parameter.key() == "core") + { + if (!type_parameter.value().is_array()) + { + throw AppendageDataException(fmt::format("Core field is not an array for type {}", type)); + } - if(type_parameter.second == "int") + nlohmann::json& core_keys = type_parameter.value(); + for (nlohmann::json::iterator core_key = core_keys.begin(); core_key != core_keys.end(); ++core_key) + { + if (m_data.find(core_key.value().get()) == m_data.end()) + { + throw AppendageDataException(fmt::format("{} missing core field {}", label, core_key.value().get())); + } + + m_core_fields.emplace_back(core_key.value().get()); + } + } + else if(type_parameter.value() == "int") { if(!parameter.is_number_integer()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an integer.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "float") + else if(type_parameter.value() == "float") { if(!parameter.is_number_float()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an integer.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "string") + else if(type_parameter.value() == "string") { if(!parameter.is_string()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be a string.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "bool") + else if(type_parameter.value() == "bool") { if(!parameter.is_boolean()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be a bool.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "object") + else if(type_parameter.value() == "object") { if(!parameter.is_object()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an object.", - type_parameter.first, label)); + type_parameter.key(), label)); } } else { - throw AppendageDataException(fmt::format("Unknown Type in template file for {}", type_parameter.first, + throw AppendageDataException(fmt::format("Unknown Type in template file for {}", type_parameter.key(), label)); } } @@ -234,6 +247,11 @@ namespace rip json["label"] = m_data["label"]; json["index"] = index; + for (std::string core_key : m_core_fields) + { + json[core_key] = m_data[core_key]; + } + return json; } } diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index 8051a65..9f43e51 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -117,64 +117,94 @@ namespace rip { device_folder.removeDirectoryRec(); } - device_folder.createDirectory(); - device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + if (!device_folder.createDirectory()) + { + throw FileIoException(fmt::format("Could not create device folder: \"{}\"", device_folder.path())); + } + // device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create src dir FileHandle source_folder = fs::open(fmt::format("{}/{}/{}", m_parent_folder, m_arduino, "src")); - source_folder.createDirectory(); - source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + if (!source_folder.createDirectory()) + { + throw FileIoException(fmt::format("Could not create source folder: \"{}\"", source_folder.path())); + } + // source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create ino file FileHandle source = fs::open(fmt::format("{0}/{1}/src/{1}.ino", m_parent_folder, m_arduino)); - source.writeFile(getArduinoCode()); + if (!source.writeFile(getArduinoCode())) + { + throw FileIoException(fmt::format("Could not create source file: \"{}\"", source.path())); + } source.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create config file FileHandle json_config = fs::open(fmt::format("{0}/{1}/{1}.json", m_parent_folder, m_arduino)); - json_config.writeFile(config); + if (!json_config.writeFile(config)) + { + throw FileIoException(fmt::format("Could not create json config: \"{}\"", json_config.path())); + } json_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create core file FileHandle core_config = fs::open(fmt::format("{0}/{1}/{1}_core.json", m_parent_folder, m_arduino)); - core_config.writeFile(getCoreConfig()); + if (!core_config.writeFile(getCoreConfig())) + { + throw FileIoException(fmt::format("Could not create core config: \"{}\"", core_config.path())); + } core_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create platformio.ini FileHandle platformio_ini = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); - platformio_ini.writeFile(platformio_str); + if (!platformio_ini.writeFile(platformio_str)) + { + throw FileIoException(fmt::format("Could not create platformio.ini: \"{}\"", platformio_ini.path())); + } platformio_ini.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create serial script FileHandle serial = fs::open(fmt::format("{0}/{1}/serial.sh", m_parent_folder, m_arduino)); - serial.writeFile(serial_str); + if (!serial.writeFile(serial_str)) + { + throw FileIoException(fmt::format("Could not create serial.sh: \"{}\"", serial.path())); + } serial.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create upload script FileHandle upload = fs::open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino)); - upload.writeFile(upload_str); + if (!upload.writeFile(upload_str)) + { + throw FileIoException(fmt::format("Could not create upload.sh: \"{}\"", upload.path())); + } upload.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); } std::string ArduinoGen::getArduinoCode() { - std::unique_ptr code_template_istream = cppfs::fs::open("code_template.txt").createInputStream(); + cppfs::FileHandle code_template_fh = cppfs::fs::open("code_template.txt"); + if (!code_template_fh.exists() || !code_template_fh.isFile()) + { + throw FileIoException("code_template.txt does not exist"); + } + + std::unique_ptr code_template_istream = code_template_fh.createInputStream(); std::string code_template(std::istreambuf_iterator(*code_template_istream), {}); return fmt::format(code_template, @@ -359,10 +389,10 @@ namespace rip } std::string rv = ""; - + int i = 0; for (auto it : sorted_commands) { - rv += fmt::format("\t{},\n", it.second); + rv += fmt::format("\t{} = {},\n", it.second, i++); } rv.pop_back(); // Remove last newline diff --git a/arduino_gen/src/argument.cpp b/arduino_gen/src/argument.cpp index f8d6640..38f672a 100644 --- a/arduino_gen/src/argument.cpp +++ b/arduino_gen/src/argument.cpp @@ -27,10 +27,31 @@ namespace rip m_value = ""; } + m_prefix = ""; + m_suffix = ""; + if (m_type == "string_literal") + { + try + { + m_prefix = getAttribute("prefix")->Value(); + } + catch (AttributeException) + { } + + try + { + m_suffix = getAttribute("suffix")->Value(); + } + catch (AttributeException) + { } + } + if(m_type != "float" && m_type != "int" && m_type != "bool" && - m_type != "string") + m_type != "string" && + m_type != "long" && + m_type != "string_literal") { throw AttributeException(fmt::format("Constructor argument unknown type on line number {}", xml->GetLineNum())); @@ -53,13 +74,13 @@ namespace rip std::string Argument::toString(std::shared_ptr appendage) const { - if (appendage->has(m_name)) - { - if(!appendage->isType(m_name, m_type)) - { - // TODO(Andrew): throw exception - } - return appendage->getString(m_name); + if (appendage->has(m_name)) + { + if (!appendage->isType(m_name, m_type == "string_literal" ? "string" : m_type)) + { + // TODO(Andrew): throw exception + } + return m_prefix + appendage->getString(m_name) + m_suffix; } else { diff --git a/arduino_gen/src/constructor.cpp b/arduino_gen/src/constructor.cpp index 806ac3b..aaa095e 100644 --- a/arduino_gen/src/constructor.cpp +++ b/arduino_gen/src/constructor.cpp @@ -66,10 +66,10 @@ namespace rip { if (appendage->has(argument.getName())) { - if (appendage->isType(argument.getName(), "string")) - { - rv += fmt::format("\"{}\"", argument.toString(appendage)); - } + if (argument.getType() == "string") + { + rv += fmt::format("\"{}\"", argument.toString(appendage)); + } else { rv += argument.toString(appendage); diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 69920f6..11c025b 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -6,7 +6,11 @@ #include #include +#include "misc/exception_base.hpp" #include "arduino_gen/arduino_gen.hpp" +#include "misc/exception_base.hpp" + +#include int main(int argc, char* argv[]) { @@ -96,7 +100,7 @@ int main(int argc, char* argv[]) { ag.readConfig(args::get(config)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to read the config file.\n" << e.what() << std::endl; return EXIT_FAILURE; @@ -106,7 +110,7 @@ int main(int argc, char* argv[]) { ag.generateOutput(!args::get(noCopy)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to generate the output.\n" << e.what() << std::endl; return EXIT_FAILURE; @@ -140,5 +144,7 @@ int main(int argc, char* argv[]) } } + std::system(fmt::format("chmod -R g+w {}/{}", args::get(parent_folder), args::get(arduino)).c_str()); + return EXIT_SUCCESS; } diff --git a/arduino_gen/test/test_arduino_gen.cpp b/arduino_gen/test/test_arduino_gen.cpp index e04bef8..cf724fa 100644 --- a/arduino_gen/test/test_arduino_gen.cpp +++ b/arduino_gen/test/test_arduino_gen.cpp @@ -541,7 +541,7 @@ namespace rip "\n" "\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -594,7 +594,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -636,7 +636,7 @@ namespace rip "\n" "\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -689,7 +689,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -733,7 +733,7 @@ namespace rip "\tNewPing(1, 2, 200)\n" "};\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -788,7 +788,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -845,7 +845,7 @@ namespace rip "\tNewPing(3, 4, 200)\n" "};\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -902,7 +902,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -965,7 +965,7 @@ namespace rip "\tNewPing(1, 2, 200)\n" "};\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -1024,7 +1024,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1336,7 +1336,7 @@ namespace rip "\tNewPing(1, 2, 200)\n" "};\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -1395,7 +1395,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1548,30 +1548,31 @@ namespace rip // Check the file permissions #ifndef _WIN32 - EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); - EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // TODO setGID support + // EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); #endif // Change the platformio.ini file @@ -1645,7 +1646,7 @@ namespace rip "\tNewPing(1, 2, 200)\n" "};\n" "\n" - "enum\n" + "enum RIPenum : int16_t\n" "{\n" "\tkAcknowledge,\n" "\tkError,\n" @@ -1704,7 +1705,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1857,30 +1858,31 @@ namespace rip // Check the file permissions #ifndef _WIN32 - EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); - EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // TODO setGID special bit support + // EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); #endif // Cleanup diff --git a/build-linux.sh b/build-linux.sh index 51bda5a..f974f7a 100755 --- a/build-linux.sh +++ b/build-linux.sh @@ -1,30 +1,72 @@ #!/bin/zsh set -E +BUILDDIR="build" +ENABLE_TESTING="ON" + +if [[ "$(dpkg --print-architecture)" == "armhf" ]]; then + BUILDDIR="${BUILDDIR}_armhf" +fi + while [[ "$1" != "" ]]; do case "$1" in - "--help"|"-h") - echo "$0