void updatePID(){
double fwd, rtn;
rotationLeft=encoderLeft;
rotationRight=encoderRight;
forward_input=rotationLeft+rotationRight;
rotation_input=rotationLeft-rotationRight;
dist.Compute();
fwd=forward_output;
rotn.Compute();
rtn=rotation_output;
md.setM2Speed(-(fwd+rtn)); //right wheel
md.setM1Speed(-(fwd-rtn)); //Left Wheel
}
void forward(double block){ //where 1 block=10cm
reset();
//Sum of the right and left encoders, thus into 2
forward_setCnt=(block*1194)*2; //in 10 cm 1194 clock counts theoretically based on my wheel diameter and rotations
rotation_setCnt=0;
}
So what I am trying to do here is put two PID loops:
- The first one ensures that the robot has moved the correct distance forward.
- The other one is to ensure that leftwheelrotation=rightwheel rotation in order to make them move in a straight line. (i.e. not turn left or right)
md is my motor shield which drives my two wheels