Controlling Parallax Servo 360 in a certain angle

I am trying to control parallax Servo 360 in a certain angle. Let say 0-45, 45-90,90-135…using Arduino board … Here is the program I am using… The problem is not rotating in the correct angle…any help would be appreciated…

//Servo to move 45 Degree angle

#include <Servo.h>

Servo myservo; // create servo object to control a servo
// twelve servo objects can be created on most boards
int analogPin = 3; // potentiometer wiper (middle terminal) connected to analog pin 3
int val = 0; // variable to store the value read

void setup() {
myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo.writeMicroseconds(1500); // set servo to mid-point
Serial.begin(9600); // setup serial
}

void loop() {
val = analogRead(analogPin); // read the input pin
Serial.println(val); // debug value

myservo.writeMicroseconds(1280); // Clockwise (faster to slower)
delay(250);
val = analogRead(analogPin); // read the input pin
Serial.println(val); // debug value

myservo.writeMicroseconds(1480); // Clockwise (faster to slower)
delay(1520);
}

(deleted)

Thank you. Yes it's a continuous rotation servo

I just got one of these myself and documentation for the Arduino is virtually non-existent. Here is what I have come up with. It is my first attempt at PID control so I am sure there is room for improvement. Also avoid the use of the serial monitor because it interferes with the sampling rate of the feedback. I hope this helps.

#include <Servo.h> //Servo library

//PID angle control for Parallax 360 Servo
Servo servo_test; //initialize a servo object for the connected servo
int pinFeedback = A0;

int target_angle = 180;
float tHigh = 0;
float tLow = 0;
int tCycle = 0;
float dc = 0;
float angle = 0; //Measured angle from feedback
float dcMin = 2.9; //From Parallax spec sheet
float dcMax = 97.1; //From Parallax spec sheet
float Kp = .7; //Proportional Gain, higher values for faster response, higher values contribute to overshoot.
float Ki = .2; //Integral Gain, higher values to converge faster to zero error, higher values produce oscillations. Higher values are more unstable near a target_angle = 0.
float iLimit = 5; //Arbitrary Anti-wind-up
float Kd = 1; //Derivative Gain, higher values dampen oscillations around target_angle. Higher values produce more holding state jitter. May need filter for error noise.
float prev_error = 0;
float prev_pError = 0;
float error = 0;
float pError = 0;
float iError = 0;

void setup()
{
servo_test.attach(9); // attach the signal pin of servo to pin9 of arduino
pinMode(A0, INPUT);
}

void loop()
{
//use line below to determine “center” of servo. When near values of 90, manually force servo by hand in both directions to see if it continues to turn.
//servo_test.write(85); //clockwise: max 0 - 90 min, counter-clockwise: min 96-180 max, may be different for your servo, 93 stopped.

while(1) //From Parallax spec sheet
{
tHigh = pulseIn(pinFeedback, HIGH);
tLow = pulseIn(pinFeedback, LOW);
tCycle = tHigh + tLow;
if ( tCycle > 1000 && tCycle < 1200)
{
break; //valid tCycle;
}
}

dc = (100 * tHigh) / tCycle; //From Parallax spec sheet, you are trying to determine the percentage of the HIGH in the pulse

angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

//Keep measured angles within bounds
if (angle < 0)
{
angle = 0;
}
else if(angle > 359)
{
angle = 359;
}

if (target_angle < 0)
{
target_angle = 360 + target_angle; //handles negative target_angles;
}

error = target_angle - angle;

if(error > 180)
{
error = error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
}
if (error < -180)
{
error = 360 - error - 360; //tells it to rotate in the other direction because it is a smaller angle that way.
}

// PID controller stuff, Adjust values of Kp, Ki, and Kd above to tune your system
float pError = Kp * error;
float iError = Ki * (error + prev_error);

if (iError > iLimit)
{
iError = iLimit;
}
if (iError < -iLimit)
{
iError = -iLimit;
}

prev_error = error;
float dError = Kd * (pError - prev_pError);
prev_pError = pError;

error = error / 2; //max 180 error will have max 90 offset value

int val = 93 - (Kp * error) - iError - dError; // 93 is the middle of my servo’s “no motion” dead-band

servo_test.write(val); //Move the servo

}

@JavelinPoint, Wanted you to know that I just re-used your code and it worked a treat! Thank you very much. Arduino examples for this little servo are sorely lacking.

int val = 93 - (Kp * error) - iError - dError; // 93 is the middle of my servo's "no motion" dead-band

For this line, I realize you're starting at 93 because that's the "dead zone" of the servo. If I want to use the Arduino PID library (by Brett Beauregard) for PID control of this servo, does anyone know how to implement this particular line and have it work correctly?

At first, I thought about just doing

val = 93 - Compute();

but I'm thinking that won't work out like it does in your code. I'm wondering if I can set the PID limits as follows:

pid_loop.SetOutputLimits(0,90);
val = 93 - Compute();

This way the max error in either direction puts the val at 3 (nearly max speed CW) or 183 (180 is max speed CCW). The only problem then is that I get 3 -183 instead of 0 - 180. I realize I can just then clamp the output to 3 - 180, but I'm not sure what to do about 0 -3, or if there's a much more sophisticated approach to this...

Here is a code that will work for the Parallax 360 servo that uses the built-in PID library. I’m not claiming it’s a great code or anything, but I spent a long time searching the internet for help with this servo, and a lot of time getting it to work correctly, so this is my attempt at contributing to the community.

It does need a bit more tuning of the PID gains for a little better angular accuracy, but overall it’s not a bad start for someone that needs something to work.

#include <Servo.h>                    // Servo Library
#include <PID_v1.h>                   // PID Library

// PID variables
double TARGET_ANGLE = 160.0;       // Target angle to achieve
double ANGLE = 0.0;               // Current angle read from the feedback pin
double SERVO_VAL = 93;            // Initial setpoint is the servo value that maintains zero motion
double OUTPUT_VAL = 0;            // Output value of the PID loop when it executes the Compute() function
int RESULT = 0;
/***************************************
* PID Gains
***************************************/
double Kp = 0.2;
double Ki = 0.05;
double Kd = 0.01;
/**************************************/
/**************************************/

// Parallax 360 Servo variables
int PIN_FEEDBACK = 5;         // Connect the feedback pin from the Parallax 360 servo to the #5 PWM pin
unsigned long T_HIGH = 0;
unsigned long T_LOW = 0;
unsigned long T_CYCLE = 0;
float DC = 0;
float DC_MIN = 2.9;           // From Parallax spec sheet
float DC_MAX = 97.1;          // From Parallax spec sheet

// Instantiate the PID and Servo instances
Servo CAM_CONTROL;
PID PID_LOOP(&ANGLE,&OUTPUT_VAL,&TARGET_ANGLE,Kp,Ki,Kd,DIRECT);

void setup() 
{ 
  Serial.begin (9600);

  // Servo initialization
  CAM_CONTROL.attach(9);                    // Attach the signal pin of servo to pin 9 of arduino
  pinMode(5, INPUT);                        // Sets PWM pin 5 as the Feedback input pin

  // PID initialization
  PID_LOOP.SetMode(AUTOMATIC);              // Turns the PID loop on
  PID_LOOP.SetOutputLimits(-30,30);         // Sets the PID output to a range usable by the Parallax 360 Servo
  PID_LOOP.SetSampleTime(100);              // Set the PID to actually compute every 100 ms.
} 
  
void loop() 
{
/*********************************************************************************
* From the Parallax 360 spec sheet
*********************************************************************************/
  while(1)
  {
    T_HIGH = pulseIn(PIN_FEEDBACK, HIGH);
    T_LOW = pulseIn(PIN_FEEDBACK, LOW);
    T_CYCLE = T_HIGH + T_LOW;
    if ( T_CYCLE > 1000 && T_CYCLE < 1200)
    {
      break;              //valid T_CYCLE;
    }
  }
  
  DC = (100 * T_HIGH) / T_CYCLE; 
  ANGLE = ((DC - DC_MIN) * 360) / (DC_MAX - DC_MIN + 1); 
/********************************************************************************/
/********************************************************************************/

  RESULT = PID_LOOP.Compute();
  Serial.print("RESULT: ");
  Serial.println(RESULT);
  if (RESULT == 1)
  {
    SERVO_VAL = 93 - OUTPUT_VAL;
    if (SERVO_VAL > 180) SERVO_VAL = 180;
    else if (SERVO_VAL < 93) SERVO_VAL = SERVO_VAL - 3;
    CAM_CONTROL.write(SERVO_VAL); //Move the servo
  }
}

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;
  
}

I also wrapped the servo command in a wrapper. I’m using a 16 servo driver board from Adafruit so my servo command is a bit different. This code uses a single proportional constant Kp with no Integral or derivative.

The code behind this function is from the Parallax sample code block.

#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40);

#define Kp 1
//This is called with the servo number and the angular error between the current angle
// from the previous function and the target angle.
void ServoCommand(int ServoNum, float errorAngle)
{
  int output, offset;             // Control system variables
  
  output = errorAngle * Kp;                 // Calculate proportional (
  if(output > 200) output = 200;            // Clamp output
  if(output < -200) output = -200;
  if(errorAngle > 0.5)                        // Add offset
    offset = 20;
  else if(errorAngle < -0.5)
    offset = -20;
  else
    offset = 0;     
  pwm1.setPWM(ServoNum, 0, getServoPulse(1500 + output + offset));
  
}

This is a helper function to take the microsecond pulse lengths provided by parallax and convert them to a 12 bit duty cycle as required by the PWM Servo Driver board. Source inspiration from Adafruit.

// Convert the pulse length in Micro seconds (us) to duty cycle
// e.g. getServoPulse(0, 1500) is a ~1.5 millisecond pulse width. 
float getServoPulse(unsigned long pulse) {
  //Function collapsed into a constant for simplicity
  //pulselength = 1000000;   // 1,000,000 us per second
  //pulselength /= 60;   // 60 Hz
  //pulselength /= 4096;  // 12 bits of resolution
  return pulse / 4.0690104167;

JavelinPoint: angle = ((dc - dcMin) * 360) / (dcMax - dcMin + 1); //From Parallax spec sheet

After reading the sheet i could not figured out why there is "+1" in denominator of equation?? if it is just linear relation then it should not come i guess