Auton turns with an inertial sensor

Programming Support VEXcode Pro V5 Text Tech Support
#1

I want to make precise turns with my auton and my inertial sensor but I am having trouble doing it. currently this is my code

double kP = 0.3; // tuning values
//double kI = 3200; //
double kD = 0.2; //
double inertialkP = 0.3; // 
//double inertialkI = 0.1; //
double inertialkD = 0.2; //
// default auton settings
int rightdesiredValue = 0;
int leftdesiredValue = 0;
int desiredinertialValue = 0;
// setting variables
int lefterror; // Motor value - desired value :: position
int righterror;
int rightprevError = 0; // previous position 20 ms ago 
int leftprevError = 0;
int rightderivative; // error-prevError :: speed
int leftderivative;
int totalError = 0; // total error = totalError + error
int inertialError;
int inertialprevError = 0;
int inertialDerivative;
int inertialTotalError = 0;
double intertialValue;
bool resetDriveSensor = false;
bool resetInertial = false;
bool inertialCorrection = true;
// enable PID
bool enablePID = true;
//
motor_group LeftMotors(Motor1,Motor2,Motor3);
motor_group RightMotors(Motor4, Motor5, Motor6);
int autoPID() {
  while (enablePID) {
    if (resetInertial) {
      resetInertial = false;
      Inertial8.setHeading(0, degrees);
    }
    while (resetDriveSensor) {
      resetDriveSensor = false;
      LeftMotors.setPosition(0,degrees);
      RightMotors.setPosition(0,degrees);
    
    
    }
    int leftmotorvalue = LeftMotors.position(degrees);
    int rightmotorvalue = RightMotors.position(degrees);

    //////////////////////////////////////////////
    /* LATERAL MOVEMENT */
    //////////////////////////////////////////////
    lefterror = leftdesiredValue - leftmotorvalue;

    righterror = rightdesiredValue - rightmotorvalue;
    //
    leftderivative = lefterror - leftprevError;
    rightderivative = righterror - rightprevError;

   
    double leftpower = (lefterror*kP + leftderivative * kD)/12.0;
    double rightpower = (righterror*kP + rightderivative * kD)/12.0; 

    
    
 
    
    Motor1.spin(forward, leftpower , volt);
    Motor2.spin(forward, leftpower , volt);
    Motor3.spin(forward, leftpower , volt);
    Motor4.spin(forward, rightpower , volt);
    Motor5.spin(forward, rightpower , volt);
    Motor6.spin(forward, rightpower , volt);
    
    leftprevError = lefterror; 
    rightprevError = righterror;
    inertialprevError = inertialError;
    vex::task::sleep(20); //refresh
  }
  

 return 1;
}
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  vex::task autoPid(autoPID);
  //auto
  
  resetDriveSensor = true;
  resetInertial = true;
  inertialCorrection = true;
  rightdesiredValue=-180;
  leftdesiredValue=180;
  desiredinertialValue = 90;
  vex::task::sleep(20);

  //continue
  //auto
  
}'
I was thinking to do something like this 
' 
if (Inertial8.heading() ! = desiredinertialValue) {
 LeftMotors.spin(fwd);
 RightMotors.spin(fwd);
}
#2
  1. If you are not already use the competition template
  2. I really don’t know why but many people new to PID create them as threads (I have yet to find an answer as to why) Your PID should really be a function that takes a desired distance, moves the robot to the desired distance and then ends. There should also be seperate functions for driving forward and turning.
  3. There is no reason to declare your PID specific variables as global.
  4. You will probably need higher PID values.
3 Likes