Ok so for this PID code. On Auton settings does int desired value need to be 0 like int desired turn value. And in the end if I were to use a tracking wheel for no wheel slippage. Could I just switch the, for example, left motor or right motor with that? And would this essentially work, Like do you see any errors? And yes I understand I need to tune/find the correct kP, KI, Kd ,turnkP, turnkI, turnkD.
// PID system:
// Settings
//Must be tuned for your robot to move/work correctly
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
// Auton Settings
int desiredValue = 200;
int desiredTurnValue = 0;
int error = 0; // SensorValue - DesireValue : Positional Value
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error
int turnerror; // SensorValue - DesireValue : Positional Value
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error
bool resetDriveSensors = false;
// Variation modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
// Get Position of Both Motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////
// Average Of the Motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
// Potential
error = averagePosition - desiredValue;
// Derivative
derivative = error - previouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
totalerror += error;
double lateralMotorPower = (error * kP + derivative * kD + totalerror *kI);
////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////
//Turning movement PID
//////////////////////////////////////////////////
// Average Of the Motors
int turnDifference = leftMotorPosition - rightMotorPosition;
// Potential
turnerror = turnDifference - desiredTurnValue;
// Derivative
turnderivative = turnerror - turnpreviouserror;
// Integral : Velocity -> Position -> Absenent ( Position x Time )
turntotalerror += turnerror;
double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);
///////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//code
previouserror = error;
turnpreviouserror = turnerror;
vex::task::sleep(20);
}
return 1;
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
task whynot(drivePID);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 300;
desiredTurnValue = 600;
}