BLDC + gear position and speed control

I'm building a lower limb exoskeleton with active hip, knee and ankle joints. Each joint has a brushless dc motor coupled to a 120:1 gear and the output shaft of each gear is coupled to a single turn (~320 degrees) potentiometer via a toothed belt and pulley for position data. The output shaft of the gear is also connected to the exoskeleton frame so that it can rotate each joint. An Arduino Mega Outputs a pwm signal to a motor controller to control speed and direction. [image attached]

I can drive each motor separately and both of the motors together but I want to do everything I can to make sure the two joints stay in sync and move accurately to the correct positions. I have a list of 50 data points for each joint. Each data point is an angle at which a joint should be for every 2% of a stride. The first data point is the angle a joint should be at the start, the next data point is the angle the joint should be at 2% of the stride and the next data point at 4% of the stride and so on.

Currently I'm moving each motor to the start position and then moving to each subsequent position in order. I'm calculating the speed by taking the difference in the data points and dividing it by 1/50th of the walking speed and generating a pwm value. This is pretty stuttery. Sometimes the motor overshoots the position, sometimes one motor reaches its target position before the other and has to wait.

I've started to look into PID control and played with it a bit for a single joint. I could get fairly decent position accuracy with little to no overshoot but I'm not sure how to incorporate speed and, again, most importantly, keep the joints in sync.

Here is the code so far. I had to attach it because it's too long. There's some extra stuff in here to communicate with a processing sketch, some notes to myself and some things I used to mess around with PID control. Comments on how to tidy things up or make them more efficient are appreciated but I'd rather not get too off topic if possible.

megaCode.ino (18.3 KB)

Standard sensorless ESC's are not very good for this. No holding torque, twitchy when starting up, and no way to stop at a precise position. If you have $350 or so, ODrive is exactly what you need. If not, then you can add hall sensors to your motors and use custom firmware on your current ESC's to do more complex control. I have an in-progress firmware for servo-style control that you can use as a starting point. Read post #40 here for details Attempt(s) at a servo-flap ornithopter - Page 3 - RC Groups or go straight to the source code here #include "ATmega8.h"#include <avr/interrupt.h>// Various ESC transistor ma - Pastebin.com

I have proper motor controllers for each motor and the motors have built-in hall sensors. I put a link to the controller below but the website was being weird and I couldn't get to the product page for the motor. It's a maxon EC60 flat motor with part number 411678. I also attached the datasheet for the motor so you can see what I'm working with.

The motor controller gives me speed and current feedback and it uses the hall sensors on the motor to control the motor but that controller doesn't give me any position information that I could use to turn the motor a specific number of degrees or what have you.

ec60Specsheet.pdf (99.4 KB)

Oh, I just assumed from the description of velocity control and twitchiness. Those controllers should be good.

Maybe you could start heading toward the next data point before you actually reach the current target, so they should hopefully never have to stop and start. Like when all joints are within 5 or 10% of the distance between their previous data point and target point, then have all of them start heading toward the next point.

And PID control of the speed should indeed be good. Maybe when you start all the joints heading toward the next data point, calculate target time of arrival, and then all the joints can control their speed up and down to try and arrive at that time.

Well I wasn't too clear on that bit and I know a lot of people use ESCs for this kind of thing but reading about all the problems they have I decided to just go with a proper controller.

That's a decent idea. When checking to see if a joint has reached the target position I could include about a 5% error and if it's inside that error then I could calculate the next speed and set the next target position. I'll play with that a little bit. I want to see how that will affect the synchronicity of the two joints.

The following is the related code I have currently for moving the ankle joint. I tried to include all of the necessary variables and methods. I think my comments should be pretty helpful but if I miss something or you have questions let me know. I'll post some new code when I get it tested.

int  rightAnkleOffset = 495;  //potVal when ankle = 0deg
double rightAnklePotVal;  //read ankle position
float rightAnkleTargetPos = rightAnkleOffset;  //target ankle potVal reading
float rightAnkleAngle = 0;  //angle value used to calc angular velocity
float angVelSpeedConst = 0.0628;  //stride segment speed constant
int rightLegTarget = 0;  //target segment in kneeAngles[], ankleAngles[]
double rightAnkleOutput;  //PWM value to match input to setpoint
bool rightAnkleReady = false;  //has ankle reached target pos?

float ankleAngles[] = {0.02, -2.06, -3.88, -4.6, -3.98, -2.4, -0.45, 1.45, 3.04, 4.27, 5.13, 5.71, 6.1, 6.43, 6.76, 7.12,
                       7.54, 7.99, 8.44, 8.86, 9.23, 9.51, 9.62, 9.43, 8.7, 7.2, 4.69, 1.15, -3.26, -8.17, -13.05, -17.13,
                       -19.52, -19.77, -18.12, -15.29, -12.04, -8.85, -5.96, -3.51, -1.64, -0.5, -0.07, -0.16, -0.42, -0.52,
                       -0.26, 0.36, 1.0, 1.2, 0.58
                      };


void loop {
/*some code here to check if joints are out of bounds, set joints to a start position and some other options before everything is ready to start moving*/

    moveRightKnee();  //same as ankle
    moveRightAnkle();

    if(rightKneeReady && rightAnkleReady) {
      rightLegReady = true;
    }

    if(rightLegReady) {
      if(modeAuto) {  //auto mode moves the joints by itself, manual mode stops after each target position and asks to continue or quit
        rightLegTarget++;
        rightLegReady = false;
        rightKneeReady = false;
        rightAnkleReady = false;
      } else {
        //code for manual mode
      }
    } else {
      if(!rightKneeReady) {
        digitalWrite(rightEnablePinAnkle, LOW);  //LOW is disabled
      } else if(!rightAnkleReady) {
        digitalWrite(rightEnablePinKnee, LOW);  //LOW is disabled
      }
    }
}

int calculateSpeed(float angle) {
  float rpm = angle / (angVelSpeedConst * 1.2) * 0.16666667 * 120.0 / 5.0;  //0.1667 converts deg/sec to rpm, 5rpm per 1pwm
  return int(rpm);
}

void moveRightAnkle() {
    rightAnkleTargetPos = round(ankleAngles[rightLegTarget] * 6.7 + rightAnkleOffset);  //convert target ankle segment val to pot val
    
    if (rightLegTarget == 50) { //if target is last segment, set to first
      rightLegTarget = 0;
    }
    if (rightLegTarget == 0) { //if target is first segment, subtract last
      rightAnkleAngle = ankleAngles[rightLegTarget] - ankleAngles[49];
    } else {  //angle difference
      rightAnkleAngle = abs(ankleAngles[rightLegTarget] - ankleAngles[rightLegTarget - 1]);
    }
    rightAnkleOutput = calculateSpeed(rightAnkleAngle);
  if(!rightAnkleReady) {
    digitalWrite(rightEnablePinAnkle, HIGH);  //LOW is disabled
    if (rightAnklePotVal > rightAnkleTargetPos) { //extend knee
      digitalWrite(rightDirPinAnkle, LOW); //CW
      analogWrite(rightSpeedPinAnkle, int(rightAnkleOutput));
    } else if (rightAnklePotVal < rightAnkleTargetPos) { //flex knee
      digitalWrite(rightDirPinAnkle, HIGH);  //CCW
      analogWrite(rightSpeedPinAnkle, int(rightAnkleOutput));
    } else if (abs(int(rightAnklePotVal) - int(rightAnkleTargetPos)) < 2) { //stop when reach target position
      rightAnkleReady = true;
    }
  }
}

I tried a few things here. First, I tried adding a larger range around the target position so that I could change the target a bit sooner but that was ineffective. I was essentially just changing the target position itself and not doing anything to the speed, which probably should have been obvious before I tried to code it. Oh well.

Instead of messing with the target position I focused on just the speed. When the joint position was within 5% of the target position I looked to the next speed and changed it. This was still an abrupt change though. There wasn't a smooth transition between the two speeds which is basically the same problem I had before. So what I really need is some sort of easing function to change the acceleration smoothly between the current speed and the next speed. That might have been what dekutree64 was trying to say in the first place but it took me a bit to get on the same page I guess.

As I started writing the function to ease between the two speeds I thought maybe this sounds like what a PID controller is used for so I'm just going to dive into that instead. I set up a basic PID controller for just the speed and left the positioning code as I had it. I'm now having some issues tuning the controller. I'm not sure what sort of values to start with. I glanced at the PID auto-tune library and the starting values and there are kp=2,ki=0.5,kd=2. Thats what I've started with but it doesn't seem very accurate. It'd be helpful if someone knew another similar project that used the PID library so I can check my values against theirs. It might be a better starting point for me.

Yeah, you'll want to limit how much the PID controller is allowed to change the speed on each update cycle so there are no sudden jerky accelerations. I think there's a function called SetOutputLimits for that.

And to clarify what I had in mind, the PID setpoint is target time of arrival, input is predicted time of arrival (curTime + (targetPos - curPos) / speed), and output is how much to change the speed.

But upon further consideration, it may still not be quite good enough. I'm pretty sure the resulting speed will lag behind the ideal speed by half a data point's worth of time, which might cause some overshoot or subtle lack of smoothness to the motion. With 50 data points per step, it may not be enough to matter. But I'll keep pondering on a better approach.

I've never actually used PID before, so hopefully someone more experienced can chime in on the tuning the constants.

Oh, I see. That changes things a little. I was using the speed I got from the motor controller as an input and the target speed as a set point instead of the time variables you suggest. I was having some issues with the speed sensor being a bit temperamental as well so using time variables would probably help clean that up.

I did constrain output between -255 and 255, then checked if it was a negative value and made it positive. The output variable became negative if the motor had to change direction, but with the time variables that wouldn't be necessary. My motor controller also sets 0rpm at 10% pwm and max speed at 90% pwm so I used the map function to bring the rpm from between 0 and 255 to between 25 and 230.

I'll update the code using new variables and see if that helps any or if I find any new issues and post back later today.

Oh, and just to be sure there's no confusion: the speed in the time prediction (curTime + (targetPos - curPos) / speed) is the actual RPM of the motor, read from the controller. Not the most recent target speed you sent to it.

And one more thought: you'll probably need to do something to prevent the motor controller from decelerating the motor as it nears each data point. Two options:

  1. Switch the controller to open loop mode while walking and closed loop mode when you want to stop and hold a position.
  2. Use closed loop mode all the time, but while walking, send it target positions that are past the actual data points. That way it should let you control the speed without interference. When you want to stop, then send the actual data point and let the controller do its thing.

I have two problems I think. One is with getting accurate speed sensor data and the other with how I set the setpoint.

My motor controller can output an analog value between -4V and 4V. I constrained this to -2.5V to 2.5V in the controllers software. To shift this voltage between 0 and 5 I used a couple 10k resistors and a 1uf capacitor. The two resistors are in series between 5V and ground, making the junction between them sit at 2.5 volts. There's also a capacitor between the signal from the controller and the junction and the analogRead() function is getting a value from that junction. When the motors aren't moving I should get a value of about 512. That's what I get most of the time but not always.

I've limited the maximum speed for the motor right now to 1000rpm and remember I have a 120:1 gear reduction on the motor.
Here's the relevant code:

#define rightKneeSpeed A2  //measure speed from motor
double inputSpeedScale = 1000.0 / 2.5 / 120.0;  //max motor speed / voltage / gear ratio

void setup() {
  rightAnkleSpeedOffset = analogRead(rightAnkleSpeed);  //read value at 0rpm
}

void getSensorValues() {
  rightKneeMeasuredSpeed = (analogRead(rightKneeSpeed) - rightKneeSpeedOffset) * inputSpeedScale;
}

As for the PID output, I've printed the input, setpoint and output to the serial monitor before and after each call to PID.Compute(). The input is always higher than my setpoint and my output is always zero so I'm probably putting my input or setpoint calculation in the wrong spot.

unsigned long segStartTime = millis();
float angVelSpeedConst = 62.8 * 1.2;  //stride segment speed constant in millis. *1.2 to slow down for test
bool segTimeSet = false;

void loop() {
    if(!segTimeSet) {
      segStartTime = millis();
      segTimeSet= true;
    }
    moveRightKnee();
    moveRightAnkle();

    if(rightKneeReady && rightAnkleReady) {
      rightLegReady = true;
    }

    if(rightLegReady) {
      if(modeAuto) {
        rightLegTarget++;
        rightLegReady = false;
        rightKneeReady = false;
        rightAnkleReady = false;
        segTimeSet = false;
      }
      ...
}

void moveRightKnee() {
...
  kneeInput = millis() + abs(rightKneeTargetPos - rightKneePotVal) / rightKneeMeasuredSpeed;
  kneeSetpoint = segStartTime + angVelSpeedConst;
...

I've got it so that my input starts lower than my setpoint but the output is still always zero so the input eventually is higher than my setpoint

I was researching weird analogRead() readings and fluctuations and came across some information about high impedance sensors and how they screw with the multiplexed ADC on the atmega. Apparently the chip on the arduino mega only has one ADC which switches to each pin as you call the analogRead() function. Sensors with some capacitance, like mine which literally has a capacitor attached to it, take a little bit to change the voltage on the ADC. You can read more about it at the two links on this page.

Adding this updated code helps to stabilize some of the readings but I still sometimes get inf or nan as a value for my input, probably because the output is 0 and the motor isn't moving which causes a divided by 0 scenario.

void getSensorValues() {
  analogRead(rightPosPinAnkle);
  delay(10);
  rightAnklePotVal = analogRead(rightPosPinAnkle);  //initial ankle position
  analogRead(rightAnkleSpeed);
  delay(10);
  rightAnkleMeasuredSpeed = (analogRead(rightAnkleSpeed) - rightAnkleSpeedOffset) * inputSpeedScale;  //read speed and covert to rpm
}

Also, the default sample time is 200 milliseconds which is way too slow. I changed that to 10ms and then to 1ms sample time.

Anyone wanna jump in here with some suggestions?

A couple updates here. I haven't fixed the problem with my PID loop but I have changed a couple things. I got rid of the resistors and capacitors from my speed analog reading and changed my motor controller to just output a voltage between 0 and 4 volts, with 2 volts sitting at 0rpm, 4V at 1000rpm and 0 Volts for -1000rpm. I get worse resolution but simplify the circuit.

Once in a while I would start getting some NaN errors for the output and some inf (infinity) errors for the input which I think was caused by dividing by zero when calculating the input because at some point the measured speed could be 0rpm. The fix? If its zero, say its 1 instead.

I added it to the end of my getSensorValues function:

if(rightAnkleMeasuredSpeed == 0) {
  rightAnkleMeasuredSpeed = 1;
}

Next, I was mixing units when calculating the input. Distance to travel was in degrees while speed was in RPM so I converted it to degrees and degrees per millisecond to match with the millis() call I was adding it to:

ankleInput = millis() + 
                          abs(ankleAngles[rightLegTarget] - (rightAnklePotVal - rightAnkleOffset) / 6.7) //target angle in deg - potVal converted to angle in deg
                        / ((6 * (rightAnkleMeasuredSpeed / 120.0)) / 1000.0);  //motor speed (rpm)/gear ratio * 6 to convert rpm to deg/s, / 1000 for deg/ms

I still get very sporadic and inaccurate motor movement speeds. My Kp value is set to 1 and my Ki and Kd values are 0.