imports taproot PID libraries

  #include "tap/algorithms/smooth_pid.hpp"
#include "tap/board/board.hpp"
#include "drivers_singleton.hpp"

define constants for the motor to use

  static constexpr tap::motor::MotorId MOTOR_ID = tap::motor::MOTOR1;
static constexpr tap::can::CanBus CAN_BUS = tap::can::CanBus::CAN_BUS1;
static constexpr int DESIRED_RPM = 3000;

create a timer object and pid controller object

  // timer object
tap::arch::PeriodicMilliTimer sendMotorTimeout(2);

// PID algorithm
// PID explained: <https://www.youtube.com/watch?v=wkfEZmsQqiA>
static tap::algorithms::SmoothPidConfig pid_config_dt = {20, 0, 0, 0, 8000, 1, 0, 1, 0};
tap::algorithms::SmoothPid pidController(pid_config_dt);

Get drivers and pass it into the motor object

  tap::Drivers *drivers = src::DoNotUse_getDrivers();

// motor object
tap::motor::DjiMotor motor(drivers, MOTOR_ID, CAN_BUS, false, "cool motor");

since we are using motors we also have to initialize CANBUS which is the primary way the type-c talks to the motors.

      Board::initialize();

    drivers->can.initialize();          // init CanBus to talk to motor
    motor.initialize();                 // init motor

Then whenever the sendMotorTimeout expires we want to send a command to the the motors.

This takes three steps:

  • calculating the error with the PID algorithm using runControllerDerivationError
  • set the motor object to go at the calculated output
  • send the message with encodeAndSendCanData
          if (sendMotorTimeout.execute())
        {
            // do the pid algorithm
            pidController.runControllerDerivateError(DESIRED_RPM - motor.getShaftRPM(), 1); 
            // set up msg so its ready to be sent
            motor.setDesiredOutput(static_cast<int32_t>(pidController.getOutput()));
            // send all msg to the motors
            drivers->djiMotorTxHandler.encodeAndSendCanData();                                            
        }

finally, outside of the timer, we have to read the position data coming from the motors continuously.

we do this with pollCanData

          drivers->canRxHandler.pollCanData();                                                            // checks to see if a msg is waiting
        modm::delay_us(10);

Code 

  #include "tap/algorithms/smooth_pid.hpp"
#include "tap/board/board.hpp"

#include "drivers_singleton.hpp"

static constexpr tap::motor::MotorId MOTOR_ID = tap::motor::MOTOR1;
static constexpr tap::can::CanBus CAN_BUS = tap::can::CanBus::CAN_BUS1;
static constexpr int DESIRED_RPM = 3000;

// timer object
tap::arch::PeriodicMilliTimer sendMotorTimeout(2);

// PID algorithm
// PID explained: <https://www.youtube.com/watch?v=wkfEZmsQqiA>
static tap::algorithms::SmoothPidConfig pid_config_dt = {20, 0, 0, 0, 8000, 1, 0, 1, 0};
tap::algorithms::SmoothPid pidController(pid_config_dt);

int main()
{
    tap::Drivers *drivers = src::DoNotUse_getDrivers();

    // motor object
    tap::motor::DjiMotor motor(drivers, MOTOR_ID, CAN_BUS, false, "cool motor");

    Board::initialize();

    drivers->can.initialize();          // init CanBus to talk to motor
    motor.initialize();                 // init motor

    while (1)
    {
        if (sendMotorTimeout.execute())
        {
            // do the pid algorithm
            pidController.runControllerDerivateError(DESIRED_RPM - motor.getShaftRPM(), 1);  
            // set up msg so its ready to be sent
            motor.setDesiredOutput(static_cast<int32_t>(pidController.getOutput()));
            // send all msg to the motors
            drivers->djiMotorTxHandler.encodeAndSendCanData();
        }

        drivers->canRxHandler.pollCanData();   // checks to see if a msg is waiting
        modm::delay_us(10);
    }

}