So I was wondering how I could implement feedback with a set motion profile. This is my current code ( just for feedback on the left motor) :
double errorL = setpointL.position - Robot.encLeft.getDistance()*0.3048; // calculate error, convert to meters double error_derivL = (errorL-error_lastL)/dt - setpointL.velocity; //derivative error minus setpoint velocity speedL+=KD*error_derivL; speedL+=KP*errorL;
I got these calculations by watching Cheesy Poof’s 2015 talk on motion profiling. How can this take care of when the robot is turning though since the error is only using the position and not the angle given by the gyro…?