Expanding into a motion contoller
As a bit of background, during the prototyping period of the robotic platform the vast majority of “Motion Control” has been done in an Open-Loop Setting where the ESP uses PWM to command an arbitrary speed setpoint via the motor driver.
This design choice required quite a bit of “tuning” on the front end to get a decent, negotiable, speed out of the robot.
Once tuned there were still instances of the robot jittering, veering off-course, or even spinning when gentle forward motion was commanded.
While working to overcome these issues it became more and more obvious that I needed to close the loop, and that I would likely need a second processor to handle this load.
The version of the code running on the open-loop system is heavy on the processor, reading a UDP packet, checking the battery voltage, updating a status light using RMT, and controlling two motors over PWM control using 2 pins each allowed me to find the actual practical limit of the ESP32.
It was to the point that I can only run the battery and light code once per 30 cycles of the processor to keep the input delay low from the HMI.
But there I was, with a hardware and software platform that was qualifiedly ready to compete in the spring combine.
I had just spent 2+ years worth of free time endeavoring to completely rebuild a system so that people, whom graduated before we could compete, would feel good about the design.
I had just effectively worked for free, and found myself going HMMM…what next?
What next needed to be a new Closed-loop Controller, with the controller actually mounted onto the PCB (preferably TIP style so that students can easily program and assemble the boards).
It was also going to be a deep review on the topics of non-blocking code, port calls, encoding options, and calculus.
So it was off to the proverbial races, or it felt that way until I realized that I was actually more on a Journey.
This journey was going to require me to educate myself on the foundation of a variety of topics, a majority of which I would cease caring about once I knew what I was doing.
These are honestly some of my favorite problems to have, the type that requires and allows for the time to familiarize yourself with all the options and then lobby for the efficiencies you actually need.
One of the first design constraints to force itself on me was the actual micro-controller needing to be a TIP package.
Its not that I don’t think that the club is incapable of SMD soldering, but I think this route offers a lower risk while allowing for more students to participate.
While I recently began learning about the PIC series of MCUs, I was hesitant to step into so many new and deep waters at once and opted to look at my options in the ATMEGA realm.
This allows me to keep using the Arduino IDE, it’s a platform the school is already teaching on, and the TIP package would allow us to flash the chip on a prebuilt unit like the UNO.
The rub to this solution is that the UNO seems to be the only TIP package still produced, and it only has 2 interrupt pins available.
This shouldn’t be to big of a deal as long as we don’t do something like use quadrature encoders (foreshadowing).
“This shouldn’t be to big of a deal as long as we don’t do something like use quadrature encoders”
The next issue to tackle was, how we encode. Initially I, along with a fellow student, looked at using a Accelerometer to close the loop
I wasn’t happy with the overall performance, especially given how complex the code became.
I also looked at some other novel approaches like using the sensor from an optical mouse.
Ultimately none of these solutions were great and had huge flaws mostly around the absurd cost of the sensor, or how precise the mounting needed to be.
So this is where I capitulated and started looking at different motor mounted encoders.
I say capitulated because I was avoiding this route due to the currently owned motors not being through shaft, so any traditional encoder was about to be a bear to install.
Ironically though, it was while fussing over the “currently owned motors” that I found our route and solution.
You see, the motors we bought were part of a dev kit from a website “AndyMark” which offers robotics kits.
When I found the kit we bought, it turned out that the motor itself was discontinued, and we did not have enough motors for 8 robots let alone 10-14 like we really wanted.
The discontinued motor had a suggested replacement that was also nearly EOL (typical lol). BUT this new model was onsale WITH an encoder for 10$ a piece.
This is effectively stealing them, the motor alone was supposed to be 20-30$ and the current gen encoder is 45$ there was clearly a catch here but at these prices I was willing to give it a test and bought 2 for myself.
The motors arrived quickly and it was clear they would pretty decently fit the application.
Without physically testing the units, I brought them before the club and gave the run-down.
During this time we reviewed the datasheet differences between the models and looked how the model we tested on compared to the previous years combine times.
After adjusting some numbers we found that the new 10$ motors with encoders would be powerful enough if we used a 4:1 gearbox to reduce the top speed into torque.
Without giving it much more thought the club said yes and we bought 20. Great, now add gearbox design to the task list.
“Great, now add gearbox design to the task list.”
Feeling good about my sales pitch to the club I dug deeper into the encoder I just bought myself into.
It turned out to be a quadrature encoder, and a pretty spartan one at that.
7 pulses per revolution, and no z-tracks this thing could pass as a oven control knob if it wasn’t also rate for an absurd speed of 10k rpm.
Which oven knobs definitely are not, but please don’t ask me how I know this fact.
So here I was, with all the parts picked out but only half a clue as to how they go together.
It was at this point that I went to one of my most beloved professors, Youtube.com
Pretty quickly into my searches I came across a content creator by the tag “Curiores” who has a blog, youtube videos, and github covering the topic to a great extent.
This was the perfect platform to take off from and only required me to learn one new topic to understand the core operation, atomic blocks.
I worked to quickly rebuild their code so that the variables were all full descriptive names and then began looking to expand upon their work.
The first expansion of code was modifying their ISR sub-routines so that the digital read value was a port call.
You see, in arduino Ide a digital read takes the processor roughly 4 microseconds to complete, but a port call is approx .5 microseconds to complete.
This means that by switching to a port call I was now taking roughly an eight the time to complete the same code, which allowed me to expand the logic quite heavily.
Curiores was using a rising edge trigger to identify the direction of the encoder using a simple if statement.
I expanded this statement to check both directions, and also trigger on a changing edge.
This meant that now the code would see two channels worth of encoder data (a and b vs a or b) using 1 interrupt pin.
This meant an increase in precision while also reducing the blocking time of the code down to approximately a sixth of the original code.
void encoder_read() //The Motor 1 Channel A sub-routine { //position buffer if(bitRead(PIND,plant_encoder_ch_a[index]))//Channel A has risen, is B already High? { if(bitRead(PIND,plant_encoder_ch_b[index])) { encoder_count[index]--; //if yes, we went "backwards" by 1 }else { encoder_count[index]++; //otherwise we went forwards by 1 } } else//channel A fell instead of rising { if(bitRead(PIND,plant_encoder_ch_b[index]))//is channel b high? { encoder_count[index]++; //we went "forwards" by 1 }else { encoder_count[index]--; //otherwise we went backwards by 1 } } }
The next big change was actually more of a choice. Curiores lays out two great ways for closing the loop in quadrature.
1 is checking the time and number of encoder pulses at the top of each processor loop, this is efficient code but causes motor rpm to appear as discreet steps of output.
2 is by checking the time difference each time the encoder pulses. This code is also efficient but has issues with jitter requiring filters and passes to be applied to the output, additionally this method is more prone to processor locking at higher speeds as the processor is regularly in is ISR sub-routine calculating the time.
Thus it has a lower theoretical rpm limit, though this was never tested.
I opted to take the first method, and built a function that spells out each of the translations as we move from desired RPM to pulses per clock cycle.
Once we were here, I built a simple serial algorithm with datablocks that allowed me to apply a desired set-point without needing to hardcode and flash the processor.
It took some pretty major bug fixing but I was eventually able to produce a code that output 1 rpm which was actually way harder than expected on a 7 pulse encoder.
Then I asked for 20 rpm and realized I was going to quickly run into issues validating if I was actually spinning 2000 times a minute or not, so I went on amazon and bought a digital tachometer.
Once it arrived I was able to tune both the PID and code in so that each motor could independently spin anywhere from -100 to 100 rpm.
I wrote the description of a datablock that will move between the processors via non-blocking serial calls and implemented it into the code.
Using the serial monitor I was able to command basic motion of both wheels using this method.
I then wired the esp32 to the test uno using two logic level adapters and could not get the data to cross between controllers.
That has been basically the story until now. I ran out of time at the hotel while working on a project out of state, I brought the kit home but will likely not touch it until August.
Once I finish troubleshooting the data bridge I plan on writing my next post, but over the next month I hope to finish building out prior articles adding content to my new website.
Feel free to review my code below, coming in at just over 300 lines of code.
/*
Penn State York Robotic Football
Motion Controller
Written By: Chris G. Soukas
Last Edit: 6/2/2024
Last Edit Note: Serial comms fixed, no int for buad speed.
add comments on all lines is the next offical step, we are heading into a full build enviornment.
I would like to make this code publicly available via a blog one day, having it look clean is important.
*/
#include <util/atomic.h> // For the ATOMIC_BLOCK macro
#include "BTS7960.h" //included to make driving the machine easier
class motionController
{
private:
float constant_proportional, constant_derivative, constant_integral, drive_signal_max; //parameters of the class
float error_previous, error_integral, velocity_clock; //Storage variables of the class
int position_previous,target_position; //Storage variable of the class
public:
//Constructor call
motionController() : constant_proportional(13.85), constant_derivative(0.35), constant_integral(0), drive_signal_max(255), error_previous(0.0), error_integral(0.0){}
//Parameterization Call
void modifyController(float constant_proportional_in, float constant_derivative_in, float constant_integral_in, float drive_signal_max_in)
{
//lack of sanitization makes me worried but here we go
constant_proportional = constant_proportional_in;
constant_derivative = constant_derivative_in;
constant_integral = constant_integral_in;
drive_signal_max = drive_signal_max_in;
}
//Evalutate motion control signals call
void motion_evaluation(int plant_position, int target, float clock_change, int &plant_power, bool &plant_direction)
{
//Convert rpm request to counts per second
float target_counts_per_second = target*(1.0/60.0)*14.0*(100.0/26.0);
float seconds_per_count = 1.0/target_counts_per_second;
//Check if we need to interpolate
if(fabs(seconds_per_count) > clock_change)
{
velocity_clock = velocity_clock + clock_change;
if(fabs(seconds_per_count) <= velocity_clock)
{
if(target_counts_per_second > 0)
{
target_position++;
velocity_clock = 0;
}else if(target_counts_per_second < 0)
{
target_position--;
velocity_clock = 0;
}
}
}else
{
if(target_counts_per_second > 0)
{
target_position = target_position + target_counts_per_second/clock_change;
}else
{
target_position = target_position - target_counts_per_second/clock_change;
}
}
//calculate the error
int error = target_position - plant_position;
//calculate the derivative
float derivative_error_over_time = (error - error_previous)/(clock_change);
//calculate the integral
error_integral = error_integral + error * clock_change;
//calculate output signal
float output_signal = constant_proportional * error + constant_derivative * derivative_error_over_time + constant_integral * error_integral;
//derive motor output
plant_power = (int) fabs(output_signal);
//clamp motor output
if(plant_power > drive_signal_max){plant_power = drive_signal_max;}
//Find direction
if(output_signal >= 0){plant_direction = 1;}
else {plant_direction = 0;}
//Store Previous error for next loop
error_previous = error;
position_previous = plant_position;
}
};
/*
We are building an array of plants, or motor driver pairs, for the motion controller
First define the number of plants
then add the pin details into the following arrays, per plant
the order of the pins in the arrays in critical, place new plant details at the same position in all arrays
*/
//Plant count
#define number_plants 2
//Plant Pin definitions
const int plant_encoder_ch_a[] = {2,3}; //Pins coming from encoders channel a
const int plant_encoder_ch_b[] = {4,5}; //Pins coming from encoders channel b
const int drive_signal_fwd_ch[] = {10,6}; //Pins going to the FWD/"R" pwm channel
const int drive_signal_rev_ch[] = {11,9}; //Pins going to the REV/"L" pwm channel
const int drive_enable_fwd_ch[] = {12,7}; //Pins going to the FWD/"R" Enable Pins
const int drive_enable_rev_ch[] = {13,8}; //Pins going to the REV/"L" Enable Pins
//setup counter array and clock holder
volatile long encoder_count[] = {0,0};
long clock_previous = 0;
//End of plant setup
//Call PID Class
motionController motor[number_plants];
//Other Variables
int s_baud = 38400; //lower baud rates burden the processor with blocking calls. PC can only offer 115200, keep it high. When moving off PC to ESP, try higher.
int motor_target[number_plants];
//Setup motors
BTS7960 motor_1(drive_enable_rev_ch[0],drive_enable_fwd_ch[0],drive_signal_rev_ch[0],drive_signal_fwd_ch[0]);
BTS7960 motor_2(drive_enable_rev_ch[1],drive_enable_fwd_ch[1],drive_signal_rev_ch[1],drive_signal_fwd_ch[1]);
bool skipByte = false;
bool dataReady = false;
bool estop = true;
uint16_t dataPacket = 0;
#define buffer_ready 14
void setup() //run once code
{
pinMode(13,OUTPUT);
Serial.begin(115200); //start channel one serial output at s_baud rate on phsyical pins 1&2
Serial.println("Boot complete, Starting Counter!"); //Put something in the channel.
//setup the plant pins and default parameters
for(int index = 0; index < number_plants; index++)
{
pinMode(plant_encoder_ch_a[index],INPUT_PULLUP);
pinMode(plant_encoder_ch_b[index],INPUT_PULLUP);
pinMode(drive_signal_fwd_ch[index],OUTPUT);
pinMode(drive_signal_rev_ch[index],OUTPUT);
pinMode(drive_enable_fwd_ch[index],OUTPUT);
pinMode(drive_enable_rev_ch[index],OUTPUT);
motor[index].modifyController(28.05,0.003,0.0,255);
digitalWrite(buffer_ready,HIGH);
}
//Setup Interrupts
attachInterrupt(digitalPinToInterrupt(plant_encoder_ch_a[0]), encoder_read<0>, CHANGE); //create an interrupt call on Motor 1 channel a pin calling the encoder read sub-routine passing option 0
attachInterrupt(digitalPinToInterrupt(plant_encoder_ch_a[1]), encoder_read<1>, CHANGE); //create an interrupt call on Motor 2 channel a pin calling the encoder read sub-routine passing option 1
//Disable both drivers
motor_1.Disable();
motor_2.Disable();
motor_target[0] = 0;
motor_target[1] = 0;
}
void loop() //run continuously
{
readData(dataPacket, dataReady, skipByte); //at the top of each loop, call the readData sub-routine to check for inbound data and handle if appropriate. Non-blocking optimized call
if(dataReady)//If the dataReady bit was set high by the readData sub-routine, apply the following code.
{
processData(dataPacket); //call the process data sub-routine to evaluate the new data
dataPacket = 0;
}
//Calculate the time difference since the last loop
long clock_now = micros();
float clock_change = ((float) (clock_now - clock_previous))/( 1000000 );
clock_previous = clock_now;
int motor_position[number_plants];
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
for(int index = 0; index < number_plants; index++)
{
motor_position[index] = encoder_count[index];
}
}
for(int index = 0; index < number_plants; index++)
{
int plant_power;
bool plant_direction;
motor[index].motion_evaluation(motor_position[index],motor_target[index],clock_change,plant_power,plant_direction);
set_motors(index,plant_power,plant_direction);
}
delay(3);
}
void readData(uint16_t &dataPacket, bool &dataReady, bool &skipByte)
{
//please see datagram.txt in this same folder for a description of the data moving through the motion controller
//general concept. if data is available bring in the next byte. read the bytes left to right or MSB first
//once two bytes have come in, write the data out and flip the new data bit
if(Serial.available())
{
byte inBoundData = (byte)((int)Serial.read());
Serial.println(inBoundData);
if(dataReady)
{
skipByte = true; //tell the processor to skip the next byte
//drop data for this cycle
}else if(!dataReady && skipByte)
{
skipByte = false; //tell the processor to read the next byte
//drop data for this cycle
}
else if(dataPacket == 0 && (!dataReady|| !skipByte))
{
dataPacket = (uint16_t)inBoundData << 8; //read in the 8 bit byte as an unsigned 16 bit word, and then shift the bits by a byte making it the Most significant byte
}else if(dataPacket != 0 && (!dataReady|| !skipByte))
{
dataPacket = dataPacket ^ (uint16_t)inBoundData; //XOR the byte cast as a 2 bytes with leading zeros into the data packet
dataReady = true; //Tell the processor to read the data, clearing the buffer
}
}
}
void processData(uint16_t dataPacket)
{
//please see datagram.txt in this same folder for a description of the data moving through the motion controller
//general concept, only called once the new data bit is high.
dataReady = false;
Serial.println(dataPacket);
uint16_t inBoundSetPoint = 0;
bool setPoint_enable = false;
bool dualOutput_enable = false;
bool plant2 = false;
for(int index = 0; index <= 15; index++)
{
if(index <= 8)
{
inBoundSetPoint = inBoundSetPoint | (uint16_t)bitRead(dataPacket, index)<<index; //make the inBoundSetPoint equal the inBoundSetPoint plus the value of the bit in the dataPacket at the index shifted left by the index
}
else if(index == 9)
{
plant2 = bitRead(dataPacket,index); //set the value of the plant bit to the 10th bit in the datapacket
}
else if(index == 10)
{
dualOutput_enable = bitRead(dataPacket,index); //set the value of the dual output enable to the 11th bit in the datapacket
}
else if(index == 11)
{
setPoint_enable = bitRead(dataPacket,index); //set the value of the set point enable to the 12th bit in the datapacket|
}
else if(index == 12) //functionality testing code
{
if(bitRead(dataPacket,index)) PORTB = PORTB ^ B00100000; // invert PB5, leave others untouched
}
else if(index == 15)
{
estop = bitRead(dataPacket,index); //set the value of the estop boolean to the 16th bit in the datapacket
}
else
{}
}
if(setPoint_enable)
{
if(dualOutput_enable)
{
motor_target[0] = int(inBoundSetPoint);
motor_target[1] = int(inBoundSetPoint);
}else
{
motor_target[int(plant2)] = int(inBoundSetPoint);
}
}
}
void set_motors(int target_index, int target_power, bool target_direction)
{
if(!estop) //if the system is not in estop
{
if(!target_direction)
{
if(target_index == 0)
{
motor_1.Enable();
motor_1.TurnRight(target_power);
}else if(target_index == 1)
{
motor_2.Enable();
motor_2.TurnRight(target_power);
}
}else if(target_power == 0)
{
if(target_index == 0)
{
motor_1.Stop();
motor_1.Disable();
}else if(target_index == 1)
{
motor_2.Stop();
motor_2.Disable();
}
}else if(target_direction)
{
if(target_index == 0)
{
motor_1.Enable();
motor_1.TurnLeft(target_power);
}else if(target_index == 1)
{
motor_2.Enable();
motor_2.TurnLeft(target_power);
}
}else
{
if(target_index == 0)
{
motor_1.Stop();
motor_1.Disable();
}else if(target_index == 1)
{
motor_2.Stop();
motor_2.Disable();
}
}
}else //we are in estop state, so stop and disable both motors
{
motor_2.Stop();
motor_1.Stop();
motor_2.Disable();
motor_1.Disable();
}
}
template <int index>
void encoder_read() //The Motor 1 Channel A sub-routine
{
//position buffer
if(bitRead(PIND,plant_encoder_ch_a[index]))//Channel A has risen, is B already High?
{
if(bitRead(PIND,plant_encoder_ch_b[index]))
{
encoder_count[index]--; //if yes, we went "backwards" by 1
}else
{
encoder_count[index]++; //otherwise we went forwards by 1
}
}
else//channel A fell instead of rising
{
if(bitRead(PIND,plant_encoder_ch_b[index]))//is channel b high?
{
encoder_count[index]++; //we went "forwards" by 1
}else
{
encoder_count[index]--; //otherwise we went backwards by 1
}
}
}
