# New I2C controlled Dual Motor Positioning Controller

The dual motor controller is based around the Arduino Nano using PWM output to control up to two DC motors and one standard type servo.
Pictured below is a prototype (bottom right) with and Arduino Nano (top right) and an AQMH2407ND H-Bridge 7 Amp 12 Volt - 24 Volt motor driver.
The fitted ribbon cable is connected for a single PWM per motor with two wires to control direction.

NOTE: the servo will derive its power from the I2C bus, so make sure there is sufficient power available on the bus to power all the servos connected, in this case a total of 8 servos assuming you have 8 dual motor controller units installed.

Proportional Integral Derivative (PID) loops is the term given to a type of control loop normally associated with power control.  PID is used extensively throughout industry for the control of pressure, temperature, flow rate and level control.
PID loops typically have a set-point, an input and the output.  The loop comprises 3 separate formula to control an output that adjust the input to be closer to the set-point based on the error between the set-point and the input.
The three formula can be expressed in simple mathematical terms as the following:
• Proportional. Output = Error * Kp
• Integral. Output = Integral * Ki, Integral = Integral + Error * DeltaTime
• Derivative. Output =Derivative * Kd, Derivative = (PrevousError - Error) / DeltaTime
The three outputs are summed together to give a Control Output.
Kp, Ki and Kd are the PID tuning constants.
DeltaTime is the change in time since the last time the loop ran.

For most applications the PID loop on its own works reasonable well, however in motor positioning systems as we are implementing, the integral component winds up over longer travel distances and results in overshoot.  A proportional only system is also not ideal, particularly for smaller changes.  If the Kp value is set high enough for the smaller changes to work, the loop overshoots on the larger changes.  In this situation the Integral component would normally be used, however when set high enough to have an effect at smaller distances, the wind-up becomes problematic for the larger distances.

Enter the the Dual PID Motor controller.

The system works by using a PID to set a speed request based on the error derived from the set-point position and the current position, and a second PID to control the power sent to the motor to achieve the required speed.
The first PID control is normally configured as a proportional only loop with a maximum speed setting based on something the motor can achieve without any trouble.
The second PID loop is used to set the power output to the motor via Pulse Width Modulation (PWM) based on the error of current speed vs request speed.
As the max speed that can be set by the first PID loop should be well within the motors capability, the motor should achieve the speed without too much Integral error, and also produce a breaking action if required when the requested speed falls below the current speed.
This dual PID operation gives us both fast motor speed even at small distance changes, and very little, if any, over shoot that can result in damage.

Even with the dual PID operation, when errors are very small, the speed request from the first loop can be very small, resulting in small power outputs to the motor.  In these cases static friction which is normally greater than non-static friction in the mechanical system will resist in allowing the motor to move until the integral component builds to a level that is great enough for the motor to overcome the static friction.  This can result in the motor overshooting the set-point by a small amount and the whole process repeating its self again.   For this reason a feature known as the Kick has been added.
When set to a value greater then 0 (the default setting) and while the motor speed is at 0 and when the speed request is greater than 0, the program will over ride the PID power output with the Kick value.  As soon as speed is detected, the output power is set back to the PID output level.  This gives the motor a little kick as it were to get the motor moving, breaking the static friction.  In my setup for the knee joint, I know the motor does not start to turn until the power reaches at least 12 to 13 in a range of 0 - 255.  This range is the PWM output range as dictated by the micro-controller.

In most systems the PID loop is run on a reasonably low rate, some heating applications can have the loop cycle as low as once per second, most however run at the more reasonable rate of once every 100 mS.  In motor positioning systems this can be too slow.  In the Dual PID motor controller, we run both the PID loops at 10 mS intervals so as to get the fastest possible response times.
As can be seen from the above video, the speed and accuracy of the control system is quite impressive.  The long delays between operations was for two reasons:
1. To allow you to see the position was stable.
2. It is hard to record video with a tablet and type on the keyboard of the computer

It should be noted that apart for the position set-point limits, the motor controller was operating on default setting with the Kick function turned off.  There was also a second controller connected to the I2C bus that was also being scanned.

On power up the motor controller sets the set-point for each of the motors to match that of the current position.  This is to prevent unexpected motor movement during system startup before the bus-master can give the motor controller instructions.  It is strongly advised that the bus-master not issue and commands until after it has obtained a full system status from the motor controller.  Also during startup, the PID and set-point limits are also loaded from EEPROM and applied.  If you make any changes to the set-point limits or the PID loop settings, then it is strongly advised to execute the save command to save the settings to EEPROM.

Note: PID setting will not be applied until the save command is issued.

The address of the motor controller is set by connecting a combination of D2, D4 and D12 to ground.  This forms a bit field to select the address with D2 being bit 0, D4 as bit 1 and D12 as bit 2.  With this combination a possible of 8 different address can be selected. The base address is 50 dec.
A2 is used to select the output type, if you require two PWM signals per motor, one for forward the other reverse, then connect A2 to ground.

Connection details
 Pin Output Mode 0 Output Mode 1 D0 USB RX USB RX D1 USB TX USB TX D2 I2C Address set bit 0 I2C Address set bit 0 D3 PWM Motor 2 PWM Motor 2 FWD D4 I2C Address set bit 1 I2C Address set bit 1 D5 PWM Motor 1 PWM Motor 1 FWD D6 PWM Motor 1 PWM Motor 1 REV D7 Motor 1 Direction FWD Motor 1 Direction FWD D8 Motor 1 Direction REV Motor 1 Direction REV D9 Motor 2 Direction FWD Motor 2 Direction FWD D10 Motor 2 Direction REV Motor 2 Direction REV D11 PWM Motor 2 PWM Motor 2 REV D12 I2C Address Set bit 2 I2C Address Set bit 2 D13 Servo Output Servo Output A0 Motor 1 Position Feed Back Motor 1 Position Feed Back A1 Motor 2 Position Feed Back Motor 2 Position Feed Back A2 Output Mode 0 select open circuit Output Mode 1 select Connected to ground A3 Test Input, Connect to ground to use A6 A7 as setpoints Test Input, Connect to ground to use A6 A7 as setpoints A4 I2C SDA I2C SDA A5 I2C CLK I2C CLK A6 Motor 1 Analog test set point Motor 1 Analog test set point A7 Motor 2 Analog test set point Motor 2 Analog test set point

This controller uses an I2C command 6 bytes long, and returns 28 bytes of data when polled.
The 6 byte command is actually two values, a word or 2 bytes which forms the command WORD and 4 bytes that is the DWORD  data.
The Command word tells the motor controller what to do with the data, most of the time the data will be a float cast to a DWORD.

List of Commands
 CMD Name Data Type Default Value Description 0 Not Used Not used and is ignored 1 Set Point 1 Float Motor 1 CurrentPos The data is used to update the first motor requested position and has a range of Motor1MinPos - Motor1MaxPos 2 Set Point 2 Float Motor 2 CurrentPos The data is used to update the Second motor requested position and has a range of Motor2MinPos - Motor2MaxPos 3 Servo SetPos Float Not Defined. Remains off until set The data is used to set the servo position. Note: this is rounded to an integer and has a range of 0 - 180 4 Servo Timeout Dword 2000 mS The data is used to set how long after the last Servo SetPos command before the servo is turned off.  This allows for power saving and helps to prevent servo burnout. 5 Set Page Returned Dword 0 The data has a range of 0 - 7. There are up to 8 pages of data that can be returned, this selects which page will be returned on the next data request.  See returned data for more detail. 6 Reset Statistics The data is ignored.  The Motor controller records the maximum power sent to each motor and the maximum speed forward and revers for each motor. This command set the statistics back to zero.  Useful when setting the PID loops. 7 Update & Save Config The data is ignored.  While set-point limits are applied immediately, PID values are not updated until this command is executed. Also data for the set-points is not saved until after this command. Once saved, the current config will be reloaded on power up. 8 Reserved 9 Reserved 10 Set Motor 1 Min Position Float 150 This sets the minimum position you can set the set point to. If you try and set the set-point to a value less than this, it will be set to this value.   Range 0 - 1022 11 Set Motor 1 Max Position Float 650 This sets the maximum position you can set the set point to. If you try and set the set-point to a value greater than this, it will be set to this value.  Range 1 - 1023 12 Set Motor 1 Max Reverse Speed Float -200 The controller will not try and run the motor any faster than this in the reverse direction. 13 Set Motor 1 Max Forward Speed Float 200 The controller will not try and run the motor any faster than this in the forward direction. 14 Set Motor 1 Max Reverse Power Float -255 The controller will not exceed this level of power in the reverse direction. Range: -255 - 0 15 Set Motor 1 Max Forward Power Float 255 The controller will not exceed this level of power in the forward direction. Range: 0 - 255 16 Set Motor 1 Timeout Dword 1000 mS Once the motor falls within the acceptable range the time out timer starts. once the timer is exceeded, the power to the motor will be shut down to save energy and to prevent the motor or motor driver from over heating.  The value is in mili seconds 17 Set Motor 1 Acceptable Error Float 3.0 This is the allowable error in positioning the motor, once within this range, the motor will continue to try for the exact position until the timeout has occurred as set above. 18 Motor 1 Kick Float 0 The default value is 0.  When Kick is set to a value greater than 0 and the current speed = 0 and  the requested power if greater than 0 and less than Kick, then the Kick value is applied to the output power, over riding the PID power request.  On the next loop, if the speed is greater than 0 then Kick is not applied and the normal PID output is.  This can be useful when high static friction loads are present. 19 Motor 1 Input Filter Dword 1 A setting of 0 turns it off.  This function can smooth out a noisy analog input allowing the motor to go to sleep. 20 Motor 1 Speed PID Float 1.5 This sets the Kp (proportional) value of the Speed PID for Motor 1. Note: this value is not used until the data s saved with command 07. 21 Motor 1 Speed PID Float 0.0 This sets the Ki (integral) value of the Speed PID for Motor 1. Note: this value is not used until the data s saved with command 07. 22 Motor 1 Speed PID Float 0.0 This sets the Kd (derivative) value of the Speed PID for Motor 1. Note: this value is not used until the data s saved with command 07. 23 Motor 1 Power PID Float 1.0 This sets the Kp (proportional) value of the Power PID for Motor 1. Note: this value is not used until the data s saved with command 07. 24 Motor 1 Power PID Float 0.1 This sets the Ki (integral) value of the Power PID for Motor 1. Note: this value is not used until the data s saved with command 07. 25 Motor 1 Power PID Float 0.0 This sets the Kd (derivative) value of the Power PID for Motor 1. Note: this value is not used until the data s saved with command 07. 30 Set Motor 2 Min Position Float 150 This set the minimum position you can set the set point to. If you try and set the set-point to a value less than this, it will be set to this value. 31 Set Motor 2 Max Position Float 650 This set the maximum position you can set the set point to. If you try and set the set-point to a value greater than this, it will be set to this value. 32 Set Motor 2 Max Reverse Speed Float -200 The controller will not try and run the motor any faster than this in the reverse direction. 33 Set Motor 2 Max Forward Speed Float 200 The controller will not try and run the motor any faster than this in the forward direction. 34 Set Motor 2 Max Reverse Power Float -255 The controller will not exceed this level of power in the reverse direction. 35 Set Motor 2 Max Forward Power Float 255 The controller will not exceed this level of power in the forward direction. 36 Set Motor 2 Timeout Dword 1000 mS Once the motor falls within the acceptable range the time out timer starts. once the timer is exceeded, the power to the motor will be shut down to save energy and to prevent the motor or motor driver from over heating.  The value is in mili seconds 37 Set Motor 2 Acceptable Error Float 3.0 This is the allowable error in positioning the motor, once within this range, the motor will continue to try for the exact position until the timeout has occurred as set above. 38 Motor 2 Kick Float 0 The default value is 0.  When Kick is set to a value greater than 0 and the current speed = 0 and  the requested power if greater than 0 and less than Kick, then the Kick value is applied to the output power, over riding the PID power request.  On the next loop, if the speed is greater than 0 then Kick is not applied and the normal PID output is.  This can be useful when high static friction loads are present. 39 Motor 2 Input Filter Dword 1 The default value for the filter is 1, a setting of 0 turns it off.  This function can smooth out noisy analog input allowing the motor to go to sleep. 40 Motor 2 Speed PID Float 1.5 This sets the Kp (proportional) value of the Speed PID for Motor 2. Note: this value is not used until the data s saved with command 07. 41 Motor 2 Speed PID Float 0.0 This sets the Ki (integral) value of the Speed PID for Motor 2. Note: this value is not used until the data s saved with command 07. 42 Motor 2 Speed PID Float 0.0 This sets the Kd (derivative) value of the Speed PID for Motor 2. Note: this value is not used until the data s saved with command 07. 43 Motor 2 Power PID Float 1.0 This sets the Kp (proportional) value of the Power PID for Motor 2. Note: this value is not used until the data s saved with command 07. 44 Motor 2 Power PID Float 0.1 This sets the Ki (integral) value of the Power PID for Motor 2. Note: this value is not used until the data s saved with command 07. 45 Motor 2 Power PID Float 0.0 This sets the Kd (derivative) value of the Power PID for Motor 2. Note: this value is not used until the data s saved with command 07.

Returned Data
The data is returned as a 28 byte packet divided into 7 dwords.
The dwords are either used as dwords, a bit field or cast into a floating point number.
As the packet or page can be selected by the controller, it is suggested the only the first two be aggressively scanned with the remainder being scanned at intervals as great as 5 seconds apart.
 Page DWORD Data type Description 0 0 dword Device Status as a bit field 1 float Current position of Motor 1 2 float Current position of Motor 2 3 float Current speed of Motor 1 4 float Current speed of Motor 2 5 float Current Power being sent to Motor 1 6 float Current Power being sent to Motor 2 1 0 dword Device Status as a bit field 1 dword ????? 2 float Servo Set point 3 float Motor 1 Set point 4 float Motor 2 Set point 5 Reserved returns 0 6 Reserved returns 0 2 0 dword Device Status as a bit field 1 float Motor 1 Min Position setting 2 float Motor 1 Max Position setting 3 float Motor 1 Reverse Max Speed 4 float Motor 1 Forward Max Speed 5 float Motor 1 Reverse Max Power 6 float Motor 1 Forward Max Power 3 0 dword Device Status as a bit field 1 float Motor 2 Min Position setting 2 float Motor 2 Max Position setting 3 float Motor 2 Reverse Max Speed 4 float Motor 2 Forward Max Speed 5 float Motor 2 Reverse Max Power 6 float Motor 2 Forward Max Power 4 0 dword Device Status as a bit field 1 float Motor 1 Speed PID Kp setting 2 float Motor 1 Speed PID Ki setting 3 float Motor 1 Speed PID Kd setting 4 float Motor 1 Power PID Kp setting 5 float Motor 1 Power PID Ki setting 6 float Motor 1 Power PID Kd setting 5 0 dword Device Status as a bit field 1 float Motor 2 Speed PID Kp setting 2 float Motor 2 Speed PID Ki setting 3 float Motor 2 Speed PID Kd setting 4 float Motor 2 Power PID Kp setting 5 float Motor 2 Power PID Ki setting 6 float Motor 2 Power PID Kd setting 6 0 dword Device Status as a bit field 1 float Motor 1 Max power used 2 float Motor 2 Max power used 3 float Motor 1 Max reverse speed achieved 4 float Motor 2 Max reverse speed achieved 5 float Motor 1 Max forward speed achieved 6 float Motor 2 Max forward speed achieved 7 0 dword Device Status as a bit field 1 Float Motor 1 Kick setting 2 dword Motor 1 Filter for the analog input 3 Float Motor 2 Kick setting 4 dword Motor 2 Filter for the analog input 5 Reserved for future use 6 Reserved for future use

Device Status Bit Field
This is a 32 bit bit field, however not all have been defined at this time.
 Bit No. Description 0 Returned packet type bit 0 1 Returned packet type bit 1 2 Returned packet type bit 2 3 Controller ID bit 0 4 Controller ID bit 1 5 Controller ID bit 2 6 Servo Enabled 7 Servo Disable Sleep (Proposed) 8 Motor 1 Stopped 9 Motor 1 Running Forward 10 Motor 1 Running Reverse 11 Motor 2 Stopped 12 Motor 2 Running Forward 13 Motor 2 Running Reverse 14 Motor 1 Paused.  (Position is deemed close enough to target after timeout period) 15 Motor 2 Paused.  (Position is deemed close enough to target after timeout period)

The Libraries used by the Arduino code are as follows:
#include <EEPROM.h>  // used for the saving and loading of the configuration
#include <PID_v1.h>    // the primary PID code used 4 times in the program
#include <Wire.h>      // the I2C library used for control and communication of status
#include <Servo.h>    // used for the servo output.

## Comment viewing options

### Great!

Ray this is great! I can't wait to see how it runs with mrl.. Good Job!

### wow.. lot's going on here

Hey Ray,

First off , the videos look great, seems like you've got a nice tight control loop implemented.  It's going to take me a little while to digest everything that you've done, but I think the idea is/should be that we borg it!  Worky is king.

I'm sure I'll have some more questions as I work my way through the source code.  Basically, what I think we should do is encapsulate this into a sub-class of MrlDevice in MrlComm, then we can implement the cooresponding java services to be able to configure and control it from java-land MRL.

I will probably attempt to carve out the i2c stuff from the code to start, and try to do a test with my DIY 3D printed servo.  That way I've got some hardware to test with...  I'd like to use this to build an open source version of bigdog that is constructed out of windshield wiper motors and pvc pipes...  (I figure the InMoov needs a robot doggie friend.)

Anyway, great stuff..  I'm curious, where did the inspiration for this come from?  Are there some academic papers or something? or was this just a ... "hmm.. seems like this would be a good way to use these alrogirthms"? sort of moment ?

Keep it up!

### Control systems

Hello Kwatters,

edit, I really shouldn't reply to these from work on my tablet...

I have been working in industry for years, using PID control systems for the control of Flower blow lines and the addition of water to wheat.

I was looking at the problem of windup, then it occured to me that there really it two problems.

First is speed control, the other is positioning.

In the past we had been trying to control the power based on position, and that was causing us problems with the windup when the distance traveled was large.

My idea was to split the problem.

We set a speed cap and have the position control the speed using the proportional only approach with a speed cap.

We  then control the power to the motor to achive the required speed as request by the positioning system.
As the speed is capped at something that can be achived, there is no real windup on the PID loop.

That seams to work.

Keep in mind that I also run the PID loops at a much faster than normal rate to keep the speed control fast and accurate.
The loop is running at 10 mS periods.

So in short, this is an original open source idea :-)