You already effectively have a P control loop on this system, so adding the other components to your system wouldn’t be too bad, although I’ve found that simple P control is sufficient for this type of task. You might try adjusting your proportional gain (the 0.3 multiplier you have against your error calculation) to see if a higher or lower value helps.

I suggest only adding/subtracting from one of the sides while doing correction, the idea here is that one acts as a master/lead side while the other is a slave/follower. That’s what I used to do on my drivetrain control loops, you might find it to be more stable:

```
motor[lmobilebase] = liftPower + powerError;
motor[rmobilebase] = liftPower;
```

instead of

```
motor[lmobilebase] = liftPower + powerError;
motor[rmobilebase] = liftPower - powerError;
```

Here’s a solution packaged into a task that I’ve used for position matching on drivetrains:

```
task
taskDriveHold () {
driveHoldRunning = true;
while (true) {
if (leftDriveSetPoint == rightDriveSetPoint) {
float driveOut = pidCalculate (leftDrivePID, leftDriveSetPoint, SensorValue (leftDriveSensorPort));
float slaveOut = pidCalculate (driveSlavePID, SensorValue (leftDriveSensorPort), SensorValue (rightDriveSensorPort));
driveLeftDrive (driveOut);
driveRightDrive (driveOut + slaveOut);
} else {
driveLeftDrive (pidCalculate (leftDrivePID, leftDriveSetPoint, SensorValue (leftDriveSensorPort)));
driveRightDrive (pidCalculate (rightDrivePID, rightDriveSetPoint, SensorValue (rightDriveSensorPort)));
}
}
}
```

That code snippet uses a function called pidCalculate, here’s the implementation for that, along with a struct I use to package all the stored information that a PID controller needs:

```
typedef struct{
float m_fKP;
float m_fKI;
float m_fKD;
float m_innerBand;
float m_outerBand;
float m_fSigma;
float m_fLastValue;
unsigned long m_uliLastTime;
float m_fLastSetPoint;
} PID;
```

```
float
pidCalculate (PID pid, float fSetPoint, float fProcessVariable) {
float fDeltaTime = (float)(nPgmTime - pid.m_uliLastTime) / 1000.0;
pid.m_uliLastTime = nPgmTime;
float fDeltaPV = 0;
if(fDeltaTime > 0)
fDeltaPV = (fProcessVariable - pid.m_fLastValue) / fDeltaTime;
pid.m_fLastValue = fProcessVariable;
float fError = fSetPoint - fProcessVariable;
if(fabs(fError) > pid.m_innerBand && fabs(fError) < pid.m_outerBand)
pid.m_fSigma += fError * fDeltaTime;
if (fabs (fError) > pid.m_outerBand)
pid.m_fSigma = 0;
float fOutput = fError * pid.m_fKP
+ pid.m_fSigma * pid.m_fKI
- fDeltaPV * pid.m_fKD;
return fOutput;
}
```