Assistance with Inertial Sensor + Turn PID

Programming Support
1

Hello, I am new to both the Inertial Sensor and PID. I’ve generated some code using them and was wondering if they would help perform precise turns. Could you let me know if my code will achieve this task and if it contains any errors? At the moment, I am only looking to perform relatively accurate turns. I’ll post my code down below!

motor LFront = motor(PORT1, ratio18_1, false);

motor LBack = motor(PORT5, ratio18_1, false);

motor RFront = motor(PORT15, ratio18_1, true);

motor RBack = motor(PORT19, ratio18_1, true);

motor inTake = motor(PORT18, ratio18_1, false);

motor flyWheel = motor(PORT17, ratio18_1, false);

controller Controller1 = controller(primary);

inertial Inertial = inertial(PORT21);

motor_group rightDrive(RFront, RBack);

motor_group leftDrive(LFront, LBack);

drivetrain Drivetrain = drivetrain(leftDrive, rightDrive, 319.19, 295, 40, mm, 1);

//turn left constants
double turnkP = 0.1;
double turnkI = 0.0;
double turnkD = 0.0;

//Desired value
int desiredTurnValue = 0;

//Inertial angle
int InertialAngle = 1;

//defining errors
int turnError; //SensorValue - DesiredValue : Positional value
int turnPrevError = 0; //Position 20ms ago
int turnDerivative; // error - prevError : speed
int turnTotalError = 0; //totalError = totalError + error

//variables for settings
bool enableTurnPID = true;
bool resetTurnSensors = false;

//Drive pid
int tPID(){

  if (resetTurnSensors) {
    resetTurnSensors = false;
    leftDrive.resetPosition();
    rightDrive.resetPosition();
    Inertial.setRotation(0, degrees);
  }


  while(resetTurnSensors){


    //Averaging position
    int TCurrentPosition = Inertial.heading(degrees);
    
    //Potential
    turnError = desiredTurnValue - TCurrentPosition;

    //Integral
    turnTotalError += turnError;

    //Derivative
    turnDerivative = turnError - turnPrevError; 

    //Lateral PID equation
    double turnSpeed = (turnError * turnkP + turnTotalError * turnkI + turnDerivative * turnkD);


    leftDrive.spin(reverse, turnSpeed, voltageUnits::volt);
    rightDrive.spin(fwd, turnSpeed, voltageUnits::volt);
    turnPrevError = turnError;

    vex::task::sleep(20);

    }

  return 1;
  }

void TurnPID(int degrees){
    resetTurnSensors = true;
    desiredTurnValue = degrees;
    InertialAngle += degrees;
    waitUntil(InertialAngle > degrees && InertialAngle < (degrees + 2));

  }

void auton_left1(){
  vex::task turnTest(tPID);
    TurnPID(90);
    vex::task stop(turnTest);
    wait(0.5, seconds);
}
2

This probably won’t move. Assuming you start at auton_left1, you set resetdrivesensors to true. The task running in the background senses that reset drive sensors is true, then sets them back to false and doesn’t run any robot-moving code. I think you want your while loop to begin with

while(resetdrivesensors == false)
1 Like
3

Your PID values look very low.

4

Thanks, I’ll make sure to try this! Would this be the only adjustment I’d need to make in my code?

5

Why don’t you tryitands.ee

6

Our robot hasn’t been finished yet, but once it’s complete, I’ll try it out and let you know how it goes!

8

Would these values be fine?:

double turnkP = 1 ;
double turnkI = 0.001;
double turnkD = 0.1;

9

It depends on your robot; you can’t really tune a PID without a robot.

10

Thanks for letting me know! I also had another question! Instead of saying: while(resetdrivesensors == false),
couldn’t I just say
while(enableTurnPID)?