Hello, i’m making a PID loop drivetrain controller for our team this year and and i have some issue.
Firstly I’m using my inertial sensor to turn and everything works well with it, but when I add the drive portion and only set a value to turn and not drive, my robot still start to drive forward and turn. And I know why
My drive PID use the motor built in encoder to get the position of our motor while our turning PID use our inertial sensor to make our turn more accurate. So when the motor turn for the turning PID, it basically make the motor turn and change the position of the motor. Because of that, our motor tries to go back to the 0 degree position for the drive while trying to go to the 90 degree for the inertial.
Anyone has any idea on how I could fix the problem? If y’all have any idea how i could do it better i would also like to hear them.
the code:
this->DESIRED_DRIVE = 0;
this->DESIRED_TURN = -90;
void DRIVE_TRAIN::Pid_loop(){
while (1){
if (PID_ENABLED == true){
printf("yeah: \n%f", DESIRED_TURN);
if (SWITCH == true){
this->INERTIAL_ZERO = INERTIAL_ZERO + INERTIAL.heading(degrees);
this->RIGHT_SIDE.setPosition(0,degrees);
this->LEFT_SIDE.setPosition(0,degrees);
//INERTIAL.setHeading(0, degrees);
this->SWITCH = false;
}
double DRIVE_AVERAGE = (RIGHT_SIDE.position(degrees)+RIGHT_SIDE.position(degrees))/2;
double TURN_AVERAGE = INERTIAL.rotation(degrees); //(RIGHT_SIDE.position(degrees)-RIGHT_SIDE.position(degrees));
this->OUTPUT_DRIVE = DRIVE_PID.getOutput(DRIVE_AVERAGE,DESIRED_DRIVE);
this->OUTPUT_TURN = TURN_PID.getOutput(TURN_AVERAGE,DESIRED_TURN);
this->RIGHT_SIDE.spin(forward,this->OUTPUT_DRIVE-this->OUTPUT_TURN-0,voltageUnits::volt);
this->LEFT_SIDE.spin(forward,this->OUTPUT_DRIVE+this->OUTPUT_TURN-0,voltageUnits::volt);
}
}
}
Thank you in advance.