Parallax Feedback 360 and Arduino

You know what I hate?

The fact that I listened to my guidance counselor in high school. I should have stopped thinking for a moment, closed my eyes, and just said, “I want to be a fire truck” immediately following the question of “what do you want to do with your life?” Instead, I said that I liked computers. And now look at me. I’m using computers everyday. Like an idiot.

I think that kids should pick careers based on HOW they want to live. If I knew back then that working in tech would prevent me from being able to enjoy a cup of coffee on a Tuesday afternoon and that this simple action would give me stress induced panic attacks, I would have chosen a very different career.

Anyway, with that said, once you know how to write code and design circuits there really isn’t any going back. I use Linux everyday, so now what am I going to do? Go back to Windows? WINDOWS 10!?! WITH ADS!? Pffff… There just ain’t no going back, baby. There’s no un-ringing the bell.

Speaking of working with computers, and micros, and knowing how to do stuff… I really wanted to get into working with servos. There’s just something magical about turning code into physical movement. I think that when people see something move, your work just becomes 10x more impressive. I think at its core, most people don’t understand systems engineering, and that until something moves, people view it as a desk job.

So I instantly went into looking for hobby servos. A problem that I have with hobby servos is that they typically don’t have 360 degree rotation and positional feedback. That was until the Parallax Feedback 360 was released. In the adafruit product video (3:41) they actually crack the little guy open so you can see its guts. Basically it’s a normal continuous servo that uses a AS5600 contactless hall effect sensor that gives positional feedback as a pulse train.

At this point I think I would be remiss if I didn’t at least mention Dynamixel. If you’re reading this and you simply just want to control something, Dynamixel makes very sleek hobby servos that have full 360 degree control and feedback. A rather inexpensive offering is the AX-12A. I highly recommend it.

For this project, however, I wanted to be as close to metal as possible. By using this servo, I would be making my Arduino Uno / ATMega328P the servo controller itself. Rather than just issuing RS485 commands to a blackbox, like you would do with the AX-12A.

The first thing I noticed is that there wasn’t very much software out for this thing. The software that I was able to find was from Parallax themselves and it is written for a Parallax board which ostensibly have multiple cores. The Arduino Uno / ATMega328P has only a single core, so I already know that I will have to rewrite part of this. The documentation explains a lot of what I was seeing in the code.

Over the past few days I have run into a bunch of problems, but I think I’ve created something… stable… ish. Hahah, I don’t really know how else to put it. It’s probably the closest approximation I could get to the original Parallax code, but targeted for the Arduino, ATMega328P platform. I still haven’t made my repo public for this code yet as I’m still actively making changes, but if you’re reading my blog, chances are you ran out of options and maybe this might help. This post will be updated as I progress.

#include <Servo.h>

Servo myservo;

#define DIR_CW 0
#define DIR_ACW 1

//Typically servos will accept microsecond pulses from 1000 to 2000,
// however, not all servos are the same, and these values may need to 
// be changed to 700 and 2300 for example
uint16_t fullyCW = 1000; //ACW meaning anti-clockwise (counter clockwise)
uint16_t fullyACW = 2000;

uint8_t servoControlPin = 9;
uint8_t servoFeedbackPin = 7;

//Originally I didn't know how these values were obtained in the Parallax documentation
//https://www.parallax.com/downloads/feedback-360%C2%B0-high-speed-servo-propeller-c-example-code

//After experimentation I discovered that those values were indeed correct as the PWM
// sent back from the device do show this range
int feedbackLow = 33;
int feedbackHigh = 1050;
float commandPos = 180.f;
float errorMargin = .6f;
int microsecondRange = 1000;
int microsecondMin = 1000;
int microsecondCenter = 1500;
int microsecondMax = 2000;
int minpulse = 45;
float currentPos = 0.f;
int pulseinVal = 0;

//Meta Values
int totalRange = feedbackHigh - feedbackLow;
float pulsePerDeg = totalRange/360.f;


//String used for obtaining serial input
String serialString = "";

int servoMoveHoming(float _commandPos);
float feedback2Deg(float _inputPulse);
int servoMoveCW(float _commandPos);
int servoMoveACW(float _commandPos);

int promptInput();

float feedback2Deg(float _inputPulse){
  float outDeg =  (_inputPulse-feedbackLow)/pulsePerDeg;
  return outDeg;
}

int promptInput(){
  uint8_t sb;

  //Clear input
  serialString = "";
    
  //Print Question
  Serial.print("Input Position (Deg):");
      
  //Block, wait for input
  while (Serial.available() == 0);
    
  //Read Serial
  if(Serial.available()) { 
        
    while (Serial.available()){
      delay(10); //delay needed for each byte
      sb = Serial.read();             
      serialString += (char)sb;
    }
   }

   serialString.trim();

   if(strcmp("getpos", serialString.c_str())==0){
     //float pos = servoGetPos();

     pulseinVal = pulseIn(servoFeedbackPin, HIGH);
     currentPos = feedback2Deg(pulseinVal);
     
     Serial.println();
     Serial.print("Current Servo Position: ");
     Serial.println(currentPos);
     
     return 0;
   } else {
    
    //Display User's input and go to position
    Serial.println(serialString);
    return -1;
   }
   return 1;
}

void setup() {
  Serial.begin(115200);
  pinMode(servoFeedbackPin, INPUT);
  myservo.attach(servoControlPin);  // attaches the servo on pin 9 to the servo object

  pulseinVal = pulseIn(servoFeedbackPin, HIGH);
  currentPos = feedback2Deg(pulseinVal);
}

int servoMoveHoming(float _commandPos){

  //Current Position
  pulseinVal = pulseIn(servoFeedbackPin, HIGH);
  currentPos = feedback2Deg(pulseinVal);

  while(abs(_commandPos - currentPos) > errorMargin){
    //Determine which direction to rotate

    //Create copies of our actual values
    float limitedCommandPos = _commandPos;
    float limitedCurrentPos = currentPos;

    //Give them floors and cielings for the sake of repeatibility
    if(limitedCommandPos < 0.f){
      limitedCommandPos = 0.f;
    } else if(limitedCommandPos > 360.f) {
      limitedCommandPos = 360.f;
    }

    if(limitedCurrentPos < 0.f){
      limitedCurrentPos = 0.f;
    } else if(limitedCurrentPos > 360.f) {
      limitedCurrentPos = 360.f;
    }

    //Determine if the shortest path is going to be clockwise or counter clockwise
    if(limitedCommandPos > limitedCurrentPos){
      if((limitedCommandPos - limitedCurrentPos) > 180.f){
        servoMove(_commandPos, DIR_ACW);
      } else {
        servoMove(_commandPos, DIR_CW);
      }
    } else {
      if((limitedCurrentPos - limitedCommandPos) < 180.f){
        servoMove(_commandPos, DIR_ACW);
      } else {
        servoMove(_commandPos, DIR_CW);
      }
    }

    pulseinVal = pulseIn(servoFeedbackPin, HIGH);
    currentPos = feedback2Deg(pulseinVal);
  }
  
  
  return 0;
}

int servoMove(float _commandPos, int _dir){
  do{

    //Get the current position of the servo horn
    pulseinVal = pulseIn(servoFeedbackPin, HIGH);
    currentPos = feedback2Deg(pulseinVal);

    //Formulate a magnitude based upon how farway the horn is away from its desired position
    float absMag = (abs(currentPos - _commandPos) / 360.f) * (microsecondRange/2.f);

    //Let's set the speed based on how big our magnitude is. The resulting effect should be that if there is
    //a lot of distance the servo should move fast, but smaller distances it will go slower. This code puts
    //a floor and a cieling on the servo speed.
    int force = 0;
    if(DIR_CW == _dir){

      force = microsecondCenter - absMag;
      if(force < microsecondMin){
        force = microsecondMin;
      } else if(force > microsecondCenter - minpulse){
        force = microsecondCenter - minpulse;
      }

    } else {

      force = microsecondCenter + absMag;
      if(force > microsecondMax){
        force = microsecondMax;
      } else if(force < microsecondCenter + minpulse){
        force = microsecondCenter + minpulse;
      }
    }

    //We move the servo
    myservo.writeMicroseconds(force);
    
  }while(abs(currentPos - _commandPos)>errorMargin);

  myservo.writeMicroseconds(microsecondCenter);
  
  return 0;
}

void loop(){

  int retVal = promptInput();

  if(retVal < 0){
    int commandPos = serialString.toInt();
    servoMoveHoming(commandPos);
  }

}

Leave a Reply

Your email address will not be published. Required fields are marked *