needed for PID #include "rm_pico_dev/src/algorithms/smooth_pid.hpp" Get drivers object (controls basically everything) pico::Drivers *drivers = new pico::Drivers(); creates motor object pico::motor::DjiMotor motor_one = pico::motor::DjiMotor(drivers, pico::motor::MotorId::MOTOR1, pico::can::PioNum::CAN_BUS0, true, "ID1", 0, 0); initialize PID algorithm static pico::algorithms::SmoothPidConfig pid_conf_dt = {20, 0, 0, 0, 8000, 1, 0, 1, 0, 0, 0}; pico::algorithms::SmoothPid pidController = pico::algorithms::SmoothPid(pid_conf_dt); initialize CanBus to send motor information drivers->can.initialize(); adds motor_one to motorHandler class motor_one.initialize(); checks to see if a message is waiting drivers->motorHandler.pollCanData(); do the pid algorithm pidController.runControllerDerivateError(0 - motor_one.getShaftRPM(), 1); set up msg so its ready to be sent motor_one.setDesiredOutput(static_cast(pidController.getOutput())); send all msg to the motors drivers->motorHandler.encodeAndSendCanData(); Code link #include #include #include "pico/stdlib.h" #include "rm_pico_dev/src/algorithms/smooth_pid.hpp" int main(int argc, char const *argv[]) { pico::Drivers *drivers = new pico::Drivers(); stdio_init_all(); //init motor object pico::motor::DjiMotor motor_one = pico::motor::DjiMotor(drivers, pico::motor::MotorId::MOTOR1, pico::can::PioNum::CAN_BUS0, true, "ID1", 0, 0); // init PID algorithm // PID explained: https://www.youtube.com/watch?v=wkfEZmsQqiA static pico::algorithms::SmoothPidConfig pid_conf_dt = {20, 0, 0, 0, 8000, 1, 0, 1, 0, 0, 0}; pico::algorithms::SmoothPid pidController = pico::algorithms::SmoothPid(pid_conf_dt); // init CanBus to send motor information drivers->can.initialize(); //init motor motor_one.initialize(); while (1) { std::cout << "=============" << std::endl; sleep_ms(1050); std::cout << "poll" << std::endl; // checks to see if a msg is waiting drivers->motorHandler.pollCanData(); std::cout << "pid" << std::endl; // do the pid algorithm // PID explained: https://www.youtube.com/watch?v=wkfEZmsQqiA pidController.runControllerDerivateError(0 - motor_one.getShaftRPM(), 1); std::cout << "set output" << std::endl; // set up msg so its ready to be sent motor_one.setDesiredOutput(static_cast(pidController.getOutput())); std::cout << "send data" << std::endl; // send all msg to the motors drivers->motorHandler.encodeAndSendCanData(); } }

Servos