example_motor_pid_encoder.cpp

Set a function to be called when a threshold is reached.The encoder will pass the current encoder value and the direction of the motor (CW -> 1 , CCW -> 0)

#include "MyRIO.h"

using namespace myRIO;

int main() {
    if(!myRIO_init()) {std::cout << "Error initializing myRIO"; return -1;}
  
    Motor motor(PWMA1, CCW, 8.823);
    
    MotorPID motorLeftPid(1.75, 40);
    
    Time timer = Time::stopwatch();
    
    Log log("log.txt");
    
    double setpoint = 360;
    motor.setAngularSpeed(setpoint);
    motorPid.setSetpoint(setpoint);
    
    motor.setInterrupt([&](long enc, bool dir) {
        if(motor.getDefaultDirection()==CCW) enc = -enc;
        double correctedCmd = motorPid.compute(enc);
        motor.setAngularSpeedAndDirection(correctedCmd);
        log.println(timer.elapsed_ns(), setpoint, correctedCmd, motorPid.getAvgSpeed());
        timer.reset();
    }, 1);
  
    Time::wait_s(2);
}