Pd drive function with distance sensor?

so ive been messing around with a distance sensor trying to see if i can set my target distance as whatever the sensor reads and was wondering if what im doing is right.

this is the code i threw together using my pd function

//this drives to whatever its lined up with and grabs it
//pd drive (with distance sensor)
void pdrivedis(double maxdist) {
double targetDistance=claw_dis.objectDistance(mm)-claw_mogo_dis;
double error = targetDistance - (lf.position(degrees)*tank_ratio);
double previousError = error;
double currentDistance= (lf.position(degrees)*tank_ratio);
while (std::abs(currentDistance)<std::abs(maxdist)) {
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(error);

currentDistance = (lf.position(degrees)*tank_ratio);
error = targetDistance - currentDistance;

double derivative = error - previousError;
previousError = error;

double speed = driveKp*error + driveKd*derivative;
set_tank(speed, speed);
if(claw_dis.objectDistance(mm)<claw_mogo_dis) {//checks for a mogo
  break;
}
vex::task::sleep(10);

}

}

i dont know if this would work, is there any better way i could do this?

Can you send your code? Make sure to put it in code tags

[Code]
Put your code here
[/Code]

Make sure the lines with [Code] and [/Code] are empty.

Formated code for everyone else
1 Like