Auto leveling code?

Does anybody have some code written already that will use two shaft encoders to auto level an arm attachment? I have an arm with a claw on a pivot on the end and it works, but its too hard to control at the same time as the rest of the robot. I don’t know much about the programming yet, and we have a competition in less than two weeks so I was hoping someone could help.

I don’t use a shaft encoder on my arm. I use a potentiometer which allows for much more precise measurements (on a cortex: a difference every .06 degress instead of every 1 degree). Although, if you are using a lift that the motor goes more than about 270 degrees, it would be beneficial to use a shaft encoder.

My code is very simple for keeping the arm still:

void KeepArmStill()
  #define motorPower (arm.Position - SensorValue[arm.PotChannel]) //if the arm has gone down from the set position, motors go up etc.
  ArmMotors(abs(motorPower) > arm.Threshold ? motorPower : 0); //if the power > the threashold, the arm motors are set to that value, else they are 0

This code uses a PID loop to keep the arm still. Arm.Position is set to whatever you want your arm to hold still at. The arm.Threshold is so the motors aren’t being powered like at a value of 1. Right now it is set to 10. ArmMotors() just basicly sets the motors on the arm to that motor value.