Need help with 360 degree continuous rotation servo

jwells4

Hello,

I'm new to the forums, so this is my first post. I recently bought a 360-degree continuous rotation servo (data sheet here, and I'm having issues getting to work correctly with an Arduino project I'm using for research. I can get the servo to go to desired angles, except when I get close to 360, or try to do more than one rotation, like requiring a position of 540 degrees. In that case, the servo starts spinning wildly out of control until I power it down, and no amount of PID tuning can correct it.

I have quite a bit of experience tuning PID settings, so I'm fairly certain the issue lies in my coding. I followed the examples on the data sheet, making only a few slight changes to make it work in Arduino. The [relevant] code is attached below. The first is the function I'm using to control the servo, which is what comes directly from the data sheet. The only change I made was to the calculation of angle, because the equation on the data sheet seemed to make the motor spin in the opposite direction required. The equation is commented so it's still there and visible, however, and the modified version is just above it.

void servoAngleControl() 
{
  errorAngle = targetAngle - angle;
  /*********************************************************************************
  * From the Parallax 360 spec sheet
  *********************************************************************************/
  while(1)
  {
    tHigh = pulseIn(PIN_FEEDBACK, HIGH);
    tLow = pulseIn(PIN_FEEDBACK, LOW);
    tCycle = tHigh + tLow;
    if ( tCycle > 1000 && tCycle < 1200)
    {
      break;              //valid tCycle;
    }
  }
  
  dc = (100 * tHigh) / tCycle; 
  angle = ((dc - dcMin) * unitsFC) / (dcMax - dcMin + 1);
  //angle = (unitsFC - 1) - ((dc - dcMin) * unitsFC) / (dcMax - dcMin + 1);
  
  if(angle < 0) angle = 0;
  else if(angle > (unitsFC - 1)) angle = unitsFC - 1;

  // Since we might be looking at positive or negative measurements above +/- 360 degrees, this keeps track
  // of the number of full turns. q2Min is an abbreviation for quadrant 2 minimum, which is 90 degrees, and
  // q3Max (quadrant 3 maximum) is 270 degrees. So, if the previous theta measurement was in the 1st
  // quadrant, and the current theta measurement is in the 4th quadrant, we know we the angle
  // transitioned from a high value to a low value, so increase the turns count. Conversely, if the previous 
  // theta was in the first quadrant, and the new theta is in the fourth quadrant, we know that the angle
  // transitioned from a low to high value, so decrease the turns count.
  if((angle < q2min) && (pAngle > q3max)) // If 4th to 1st quadrant
    turns++; // Increment turns count
  else if((pAngle < q2min) && (angle > q3max)) // If in 1st to 4th quadrant
    turns --; // Decrement turns count

  // This takes the number of turns and the theta angle measurement, and constructs the total angle
  // measurement from zero, allowing for large angle values in the +/- 2,147,483,647 degree range. 
  if(turns >= 0)
    tAngle = (turns * unitsFC) + angle;
  else if(turns < 0)
    tAngle = ((turns + 1) * unitsFC) - (unitsFC - angle);

  // Since the 0 to 359 and 359 to 0 degree crossings rely on the angle from the previous time through the
  // loop, the value of thetaP (theta previous) is copied from the current theta angle before the next loop
  // repetition.
  pAngle = angle;

  /********************************************************************************/
  /********************************************************************************/

  result = pidLoop.Compute();
  if (result == 1)
  {
    if (errorAngle > 0) errorOffset = 30;
    else if (errorAngle < 0) errorOffset = -30;
    else errorOffset = 0;
    servoVal = 1480 - (outputVal + errorOffset);
    camControl.writeMicroseconds(servoVal); //Move the servo
  }
}

Am I doing something obviously wrong? Admittedly, I'm not good at programming, so I feel like the error definitely lies here. I would greatly appreciate any help. Thanks in advance!

jwells4

I made some changes that have definitely fixed some issues where I made mistakes either in math or typos. The code now properly sees the angle go above 360 and correctly computes the angle. However, it rapidly starts adding turns to the calculation for no reason. For instance, when it hits 360 and goes slightly beyond to where the computed angle is 0.38, the code is correctly returning 360.38. The next iteration, though, when the computed angle is 15.67, the code has added TWO turns, showing an actual angle of 735.67, instead of 375.67. It then goes on to rapidly add turns every loop iteration. Below is a sample of the Serial output from the Arduino IDE where you can see this happening. I know this is how I'm implementing the tracking of number of turns, but I'm at a loss.

compAngle: 348.15
Angle: 348.15
tAngle: 348.15
dc: 94.00
compAngle: 348.15
Angle: 348.15
tAngle: 348.15
dc: 94.00
compAngle: 0.38
Angle: 360.38
tAngle: 360.38
dc: 3.00
compAngle: 15.67
Angle: 735.67
tAngle: 735.67
dc: 7.00

The updated code is pasted below. I made a change to the angle calculation, where I made a typo and had not previously recognized it.

void servoAngleControl() 
{
  errorAngle = targetAngle - angle;
  /*********************************************************************************
  * From the Parallax 360 spec sheet
  *********************************************************************************/
  while(1)
  {
    tHigh = pulseIn(PIN_FEEDBACK, HIGH);
    tLow = pulseIn(PIN_FEEDBACK, LOW);
    tCycle = tHigh + tLow;
    if ( tCycle > 1000 && tCycle < 1200)
    {
      break;              //valid tCycle;
    }
  }
  
  dc = (100 * tHigh) / tCycle; 
  angle = ((dc - dcMin) * unitsFC) / (dcMax - dcMin);
  //angle = (unitsFC - 1) - ((dc - dcMin) * unitsFC) / (dcMax - dcMin + 1);
  Serial.print("compAngle: ");
  Serial.println(angle);
  
  if(angle < 0) angle = 0;
  else if(angle > (unitsFC - 1)) angle = unitsFC - 1;

  // Since we might be looking at positive or negative measurements above +/- 360 degrees, this keeps track
  // of the number of full turns. q2Min is an abbreviation for quadrant 2 minimum, which is 90 degrees, and
  // q3Max (quadrant 3 maximum) is 270 degrees. So, if the previous theta measurement was in the 1st
  // quadrant, and the current theta measurement is in the 4th quadrant, we know we the angle
  // transitioned from a high value to a low value, so increase the turns count. Conversely, if the previous 
  // theta was in the first quadrant, and the new theta is in the fourth quadrant, we know that the angle
  // transitioned from a low to high value, so decrease the turns count.
  if((angle < q2min) && (pAngle > q3max)) // If 4th to 1st quadrant
    turns++; // Increment turns count
  else if((pAngle < q2min) && (angle > q3max)) // If in 1st to 4th quadrant
    turns --; // Decrement turns count

  // This takes the number of turns and the theta angle measurement, and constructs the total angle
  // measurement from zero, allowing for large angle values in the +/- 2,147,483,647 degree range. 
  if(turns >= 0)
    tAngle = (turns * unitsFC) + angle;
  else if(turns < 0)
    tAngle = ((turns + 1) * unitsFC) - (unitsFC - angle);

  angle = tAngle;

  // Since the 0 to 359 and 359 to 0 degree crossings rely on the angle from the previous time through the
  // loop, the value of thetaP (theta previous) is copied from the current theta angle before the next loop
  // repetition.
  pAngle = angle;

  /********************************************************************************/
  /********************************************************************************/

  result = pidLoop.Compute();
  if (result == 1)
  {
    if (errorAngle > 0) errorOffset = 30;
    else if (errorAngle < 0) errorOffset = -30;
    else errorOffset = 0;
    servoVal = 1480 - (outputVal + errorOffset);
    camControl.writeMicroseconds(servoVal); //Move the servo
  }
  delay(20);
}