Hello,
this is my first coding on my robotics and I’ve been struggling with my PID, for some reason my int target
that is used to set the target value in inches in the void just sets itself to zero regardless of what I set it to. plz, help me if you can. (also I have my encoders set to 0 at the start of auton)
my pid:
``void moveFwd(double target, double maxVoltage) {// scale 0 to 12 mvolt
//chassis_Set PIDControl;
// MACHINE STATE WHEN ENTERING FUNCTION, MACHINE STATE AFTER
double enterAngle = Gyro.rotation();
// CONVERSION FROM INCHES TO ENCODER TICKS
target = ((target * 360) / (4 * M_PI)) * (36 / 84);
double output = 0, Dt = 0, encoderAvrg = 0;
double prevError = 0, prevTime = t.time(msec);
// SLEW COMPONENTS
double volCapMax = 12, volCap = 0;
// SLEW TUNABLE
double acceleration = .07;
// PID COMPONENTS
double error = 6, integral = 0.01, derivetive = 0.1;
// CONSTATNTS
double Kp = .11, Ki = 0.009, Kd = .007, kt = 0;
// BOUNDS
double errorMargin = 4, integralBound = 20;
// CORECTION VARIABLES
double targetAngle = 0, correctionOutput = 0;
double enterVal = ((end1.position(rotationUnits::deg) + end2.position(rotationUnits::deg)) /2);
int sign = 1;
double sensorVal = ((end1.position(rotationUnits::deg) + end2.position(rotationUnits::deg)));
// CALCULATES ERROR FOR FUNCTION TO RUN
error = target - encoderAvrg;
std::cout << "error: " << error << std::endl;
while (fabs(error) > errorMargin || error == 0) {
std::cout << "power: " << output << std::endl;
std::cout << "error: " << error << std::endl;
std::cout << "target: " << target << std::endl;
sensorVal = ((end1.position(rotationUnits::deg) + end2.position(rotationUnits::deg)));
// AVERAGE BETWEEN ENCODER VALUES
if (sensorVal == 0){
encoderAvrg = sensorVal;
}
else {
encoderAvrg = sensorVal / 2;
}
// FINDS THE DIFFRENCE BETWEEN THE TARGET AND ROBOT
error = (target - sensorVal);
// CALCULATES DELTA TIME IN MSEC
Dt = (t.time(msec) - prevTime) / 1000;
// FINDS INTEGRAL WITH CONSTANT MULTIPLIER, AND ERROR
integral += error * Dt;
// FINDS DRVITIVE USING ERROR, AND PREVIOUS
// DIVIDES BY TIME
derivetive = (error - prevError) / Dt;
// UPDATES THE PREVIOUS ERROR
prevError = error;
if (fabs(error) > integralBound) {
integral = 0;
}
// CALCULATES THE VOLTS TO BE SENT TO THE MOTORS
output = error * Kp + integral * Ki + derivetive * Kd;
// FINDS IF POS OR NEG
sign = fabs(output) / output;
if (fabs(output) > maxVoltage) {
output = maxVoltage * sign;
}
// Slew rate
if (fabs(output) > volCapMax)
output = volCapMax * sign;
volCap += acceleration * sign;
if (fabs(volCap) > fabs(volCapMax)) {
volCap = volCapMax * sign;
}
if (fabs(output) > fabs(volCap)) {
output = volCap;
}
// DIFFRENCE IN MACHINE STATE
correctionOutput = targetAngle - (enterAngle - Gyro.rotation())*kt;
// CHECKS MACHINE STATE
if (fabs(enterAngle - Gyro.rotation()) > targetAngle) {
leftDrive.spin(directionType::fwd, output + correctionOutput, voltageUnits::volt);
rightDrive.spin(directionType::fwd, output - correctionOutput, voltageUnits::volt);
// UPDATES MACHINE STATE TO BE DESIRABLE
//PIDControl.move(output + correctionOutput, output - correctionOutput);
}
else if (fabs(enterAngle + Gyro.rotation()) < targetAngle) {
leftDrive.spin(directionType::fwd, output - correctionOutput, voltageUnits::volt);
rightDrive.spin(directionType::fwd, output + correctionOutput, voltageUnits::volt);
// UPDATES MACHINE STATE TO BE DESIRABLE
//PIDControl.move(output + correctionOutput, output - correctionOutput);
}
// IF MACHINE STATE DESIRABLE CONTINUE PID
else {
leftDrive.spin(directionType::fwd, output, voltageUnits::volt);
rightDrive.spin(directionType::fwd, output, voltageUnits::volt);
}
// DEBUGGING
// UPDATES PREVIOUS TIME
prevTime = t.time(msec);
// ALLOWS FOR UDATES TO VALUES
wait(15, msec);
}
// STOPS THE ROBOT PREVENTS FURTHER MOVEMENT
//PIDControl.rest();
leftDrive.stop(hold);
rightDrive.stop(hold);
}``
how I call it in the auton function:
moveFwd(50, 10);