Controlling Parallax Servo 360 in a certain angle

Projects General Guidance
8

jdwells1982, thank you so much for your work on this, it was a very helpful for me! I took inspiration from this and from the code from Parallax and wrapped it in a function to handle multiple servos and the multi-turn functionality.

#define DC_MIN 29           // From Parallax spec sheet
#define DC_MAX 971          // From Parallax spec sheet
float GetServoAngle(int FeedbackPin, float AngleLast)
{
  unsigned long tCycle = 0;
  unsigned long tHigh = 0;
  unsigned long tLow = 0;
  float DC = 0;
  float THETA = 0;
  float THETALAST = 0;
  int TURNS = 0;
  int q2min = 360 / 4;                      // For checking if in 1st quadrant
  int q3max = q2min * 3;                      // For checking if in 4th quadrant

  TURNS = AngleLast / 360;
  THETALAST = AngleLast - TURNS*360;

  while(1) // Repeat until cycle time valid
  {
   tHigh = pulseIn(FeedbackPin, HIGH); // Measure high pulse
   tLow = pulseIn(FeedbackPin, LOW); // Measure low pulse
   tCycle = tHigh + tLow; // Calculate cycle time
   if((tCycle > 1000) && (tCycle < 1200)) break; // Cycle time valid? Break!
  }
  DC = (1000 * tHigh) / tCycle; // Calculate duty cycle

  THETA = ((DC - DC_MIN) * 360) / (DC_MAX - DC_MIN + 1); 
  // If transition from quadrant 4 to  
  // quadrant 1, increase turns count. 
  if((THETA < q2min) && (THETALAST > q3max))
    TURNS++;
  // If transition from quadrant 1 to  
  // quadrant 4, decrease turns count. 
  else if((THETALAST < q2min) && (THETA > q3max))
    TURNS --;

  // Construct the angle measurement from the turns count and
  // current theta value.
  
  return (TURNS * 360) + THETA;
  
}