Here's your code above integrated with my safety "compliance" code. How should it be modified to add the newer features? Thanks.
{{
}}
obj
pos : "AS5055Demo120526a" ' magnetic position encoder
pwmA : "jm_lfpwm_8x" ' A pwm output
pwmB : "jm_lfpwm_8x" ' B pwm output
dbug : "Serial Terminal" ' pc serial IO
var
long speed, time, distance, activeAcceleration
long rampDownTarget
byte driverState
byte dir
con
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000 ' use 5MHz crystal
BAUD = 115200
ACCELERATION = 2
MAX_SPEED = 200
MIN_SPEED = 1
DISTANCE_TO_MOVE = 40_000
' driverState enumeration
#0, RAMP_UP, FULL_SPEED, RAMP_DOWN, STOP
pub main
' dbug.start(rxpin, txpin, mode, baudrate)
dbug.startRxTx(31, 30, 0, 115200) ' start debug object
waitcnt(clkfreq*3+cnt) 'Wait for Debug Startup
dbug.Str(string(13,"Debug Start "))
pos.Setup ' start position sensor object
' pwmX.start(n, pin, freq) ' start PWM object
pwmA.start(1, 4, 100) ' start a PWM to drive A side of valve
pwmB.start(1, 5, 100) ' start a PWM to drive B side of valve
repeat ' swing until power off
dir := 1
move ' move up to measured top of swing
dir := -1
move ' move dn to measured bottom of swing
PUB Move
dbug.Str(string(13,"Move Start "))
case driverState
RAMP_UP:
RampUp
FULL_SPEED:
FullSpeed
RAMP_DOWN:
RampDown
distance += speed '??? needed might overflow
dbug.Dec(distance)
dbug.Char(13)
PUB RampUp
result := speed + ACCELERATION
if result => MAX_SPEED
speed := MAX_SPEED
rampDownTarget := DISTANCE_TO_MOVE - distance
if rampDownTarget =< 0
driverState := RAMP_DOWN
else
driverState := FULL_SPEED
else
speed := result
ptmove(speed / 200)
PUB FullSpeed
ptmove(1)
if distance + speed => rampDownTarget
driverState := RAMP_DOWN
PUB RampDown
result := speed - ACCELERATION
result #>= 1
if distance + result => DISTANCE_TO_MOVE
speed := DISTANCE_TO_MOVE - distance
driverState := STOP
else
speed := result
ptmove(speed / 200)
pub ptmove(level) | oldPos, apos
' Move one point up or down with position sense and
' Safety Stop or delay if actual move <> one point (from external forces).
' dir - plus = up, minus = down
' speed = duty = level = % of valve open or width of pulse
' PID would adjust level to meet a time and keep position until control change
' PID will be useful when motion is not continuous, to hold position
oldPos := pos.avgpos
repeat until pos.avgpos == (oldPos + 1) 'loop move to next position
if dir == 1 'open correct A B valve coil
pwmA.set(1, level) 'emit pwm to coil A up
else
pwmB.set(1, level) 'emit pwm to coil B dn
' Safety stop if motion faster or slower than expected
apos := pos.avgpos '
if apos > oldPos + 1 'close valves
pwmA.set(1, 0) '
pwmB.set(1, 0)
abort
if apos < oldPos - 1 'close valves
pwmA.set(1, 0) '
pwmB.set(1, 0)
abort
{{
}}
The safety algorithm hasn't been tested but a lot of the literature on professional robotics demands safety compliance because people have been killed by robots that don't stop when a human is in range of motion. It looks like detecting unexpected motion this way should work without adding special springy limbs or touch sensors but it may need tweaking.
How should it be modified to add the newer features?
Kirk,
As you know I've been working on a CNC machine which uses stepper motors. I'm still trying to figure out a good substitute for your hydraulic actuators. I wonder if using my own h-bridge to control a modified hobby servo motor would allow me to test a control algorithm using PWM as output and the magnetic encoder as input. I'm afraid the CNC stepper motors wouldn't be a good substitute. (I'm also not sure I want/need to use magnetic encoders on my CNC machine.)
If a hobby servo could be used as a substitute, then I could use one of the servos I'm modifying in this thread to test an algorithm with "jerk" (variable acceleration).
What do you think about my using a hobby servo to test the control algorithm?
I'm still trying to figure out a good substitute for your hydraulic actuators. I wonder if using my own h-bridge to control a modified hobby servo motor would allow me to test a control algorithm using PWM as output and the magnetic encoder as input. I'm afraid the CNC stepper motors wouldn't be a good substitute. (I'm also not sure I want/need to use magnetic encoders on my CNC machine.)
If a hobby servo could be used as a substitute, then I could use one of the servos I'm modifying in this thread to test an algorithm with "jerk" (variable acceleration).
What do you think about my using a hobby servo to test the control algorithm?
I don't know how to control a CNC stepper motor or remote control hobby servo with PWM but how about just do what you think would work best in theory without physical hardware testing, except of course that it compiles and produces mathematically correct output?
I doubt an ordinary hobby servo would show much acceleration control although the larger quarter scale size with more weight attached might - it's just a guess.
Of course, it might save me time to see the code first, before testing, in case testing takes a while. I want to try it as soon as possible on my hardware. Thanks.
Now that I have a good feedback system for position and speed (with the magnetic encoder), I've been trying to get this sort of s-curve control algorithm to work.
Not surprisingly, getting a physical object to move in a way described by an equation isn't nearly as easy as plotting the positions in a spreadsheet.
There are several challenges to writing code which uses jerk. One of the challenges is to figure out when to apply the jerk and for how long in order to achieve a predetermined outcome. In the spreadsheets I just used arbitrary values for time and distance in order to have the curves the general shape I was after. Determining when to apply jerk to influencement precise movement is much more of a challenge.
Wikipedia has a good article about jerk. The bottom three equations on the page provide ways of calculating the required jerk needed in order to produce movement of a desired acceleration, speed or displacement. Since calculating the required jerk will likely involve cube roots, I'll probably shift to using floating point numbers in the calculations.
Rather than using the equations to calculate when to apply jerk to achieve a desired outcome, it's also possible to just monitor the system and observe what the effects of applied jerk are. These effects could then be used to determine when to apply jerk to achieve the opposite effect.
For example, the initial change in speed while jerk is applied to a system may be used to calculate when to apply negative jerk in order to achieve a desire maximum speed. The distance traveled while reaching maximum speed should be the same distance required to stop if the same magnitude of jerk (and max acceleration) is used to stop the system as was originally used to get the system moving.
The graphs in post #23 assume there was time to both reach the maximum acceleration and the maximum speed of a system. There will likely be times when the maximum speed will be reached prior to reaching the maximum acceleration. Here are a couple of graphs showing the displacement, speed and acceleration curves for such a system.
You can see the ramp up of the speed graph doesn't have a flat area representing constant acceleration.
There will likely be times when the distance to be moved will not be enough to allow the system to achieve its maximum speed. Here are a few graphs of this scenario.
You can see the speed curve not only doesn't have flat areas of constant acceleration but there also isn't a flat top area of constant speed.
I think the movement of both these cases could be computed by monitoring the position of and speed of a system and by using the information from initial movements to calculate appropriate actions to achieve a desired outcome.
While complex calculations may be avoided by monitoring initial speed and displacement, I think any sort of control system using these complex motions should have the ability to calculate actions required without the need of first monitoring initial movements.
There are likely times when the motion of a system will not be symmetrical with its initial and final movements. Particularly in a case of an emergency. In an emergency situation it may be required to use a larger jerk than normal. The system should be able to accommodate these situations.
I may use the simpler method of monitoring speed and displacement and assume symmetrical movements in order to come up with some initial test code to use with the encoder, however any sort of code used in a system with valuable equipment should have a more robust algorithm. The requirements of a robust control algorithm would also apply to any system large or strong enough to injure humans.
I don't think I have a question in all this. I'm just posting some thoughts on the subject.
As an experiment in using ramped acceleration, I wrote some code to generate an rounded trapezoidal speed profile. The only parameter changed in the code was the jerk amount. The jerk was either positive, negative or zero. Both the positive and negative jerk amounts had the same magnitude.
The acceleration and speed were then computed based on this jerk amount with the following equations.
I use the prefix "pseudo" to remind myself I'm dealing with a scaled integer. The scaled value will need to be divided by the scale factor before being used in the real world. In this case, since I'm using the speed to control a continuous rotation servo, I use the following code in order to conver the "pseudoServoTargetSpeed" into a pulse used by the servo driver.
"direction" is either one or negative one depending on which way the servo is to rotate. "STOP" is 1500 which is the pulse length used to stop a CR servo. "PSEUDO_MULTIPLIER" is the scaling factor (1000 in this case).
Since I wasn't using any sort of feedback system to monitor the speed (at least not in a useful way), the ramped acceleration is kind of hard to see when using a motor with such limited speed range. I thought the whole concept of ramped acceleration might be easier to see if I had some sort of gauge to visually show the acceleration and speed amounts.
Having recently purchased some NeoPixel rings from AdaFruit, I decided to try using them as gauges to show the various motion parameters.
Here's the first of several video attempting to use the NeoPixel rings as demonstration tools.
Hopefully seeing the LEDs light up will make the concept of ramped acceleration more intuitive to some viewers. (If any views feel like the LEDs aided in their understanding off ramped acceleration, I hope you let me know.)
I'm been working with Kirk Fraser on this project. Kirk, do you think there would be any benefit in modifying this ramped acceleration algorithm to use a PWM object rather than the servo object? Or do you want to wait until I have some sort of sensor feedback with a PID control algorithm before switching to a PWM object?
I have a couple more videos I'm uploading to YouTube exploring the use of these LEDs as visualization aids. The one currently uploading shows when jerk is applied.
Here's another video of my attempt to use LEDs as an aid in the explanation of ramped acceleration.
While I thought the combined gauge of acceleration, speed and distance wasn't intuitive, I like the combined gauge of jerk and acceleration.
In case I didn't make it clear in my earlier post, I do not think a CR servo is the best application for ramped acceleration. Ramped acceleration has its applications as explained in the Wikipedia article. Servos will generally be fine with ramping just the speed (with constant acceleration). I used a servo since it was an easy way to test the ramped acceleration algorithm on a real world device.
I have another video in the process of uploading to YouTube. I'll be posting it soon. It's every bit as exciting as these last two videos.
Coming soon, the final installment of the ring trilogy, Lord of the Jerk.
As mentioned in the Wikipedia article about jerk, there are up to seven segments in the motion profile.
These segments are:
1. Acceleration build-up, with maximum positive jerk. {Labelled "a+, s+" in the video.}
2. Constant maximum acceleration (zero jerk) {Labelled "max a, s+" in the video.}
3. Acceleration ramp-down. approaching the desired maximum velocity, with maximum negative jerk. {Labelled "a-, s+" in the video.}
4. Constant maximum speed (zero jerk, zero acceleration) {Labelled "constant speed" in the video.}
5. Deceleration build-up, approaching the desired maximum deceleration, with maximum negative jerk. {Labelled "a+, s-" in the video.}
6. Constant maximum deceleration (zero jerk) {Labelled "max a, s-" in the video.}
7. Deceleration ramp-down. approaching the desired position at zero velocity, with maximum positive jerk. {Labelled "a-, s-" in the video.}
Since I had a couple extra LEDs not used in the "profile gauge" I added an eighth segment.
8. {{Labelled "stopped" in the video.}
I imagine most systems which would benefit from a ramped acceleration would also benefit (if not require) from some form of position and speed feedback.
I had hoped to use a magnetic encoder for position and speed feedback. I've modified my original AS5055 driver to include a speed measurement. Unfortunately wasn't successful in my attempt at aligning the magnet and sensor chip.
I've attached a couple videos demonstrating the erratic speed readings in the encoder hardware thread.
I'm been working with Kirk Fraser on this project. Kirk, do you think there would be any benefit in modifying this ramped acceleration algorithm to use a PWM object rather than the servo object? Or do you want to wait until I have some sort of sensor feedback with a PID control algorithm before switching to a PWM object?
I haven't seen the full code but from the ring display videos it looks like the one way commanded motion system with constant jerk will serve my immediate testing need to operate a hydraulic valve using the PWM driver object. Your videos enlightened me to understand the position feedback is not actually necessary for a mechanical test safe from public contact. I can put that to use.
However, a larger hydraulic device which can damage human body parts that get in its way would be made more safe utilizing a position feedback system which shows error between commanded position and measured position to stop motion since it could indicate human contact, I can either use what you have with the PWM object or wait for this safety feature.
The Proportional Integral Derivative (PID) program apparently presents control features I do not need for my first round of testing. However, I do believe it will be useful in future testing and motion control applications. Thank you.
The Proportional Integral Derivative (PID) program apparently presents control features I do not need for my first round of testing. However, I do believe it will be useful in future testing and motion control applications. Thank you.
Kirk,
I uploaded the final video to the encoder PCB thread. I know you've mentioned a desire for a small PCB to use with the encoders. Are you planning on using the encoders to monitor the position of the equipment being moved by your hydraulic system?
I'll get to work on switching this demo from output intended for a servo to PWM output if you think it would be useful without position feedback.
Comments
Here's your code above integrated with my safety "compliance" code. How should it be modified to add the newer features? Thanks.
{{ }} obj pos : "AS5055Demo120526a" ' magnetic position encoder pwmA : "jm_lfpwm_8x" ' A pwm output pwmB : "jm_lfpwm_8x" ' B pwm output dbug : "Serial Terminal" ' pc serial IO var long speed, time, distance, activeAcceleration long rampDownTarget byte driverState byte dir con _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 ' use 5MHz crystal BAUD = 115200 ACCELERATION = 2 MAX_SPEED = 200 MIN_SPEED = 1 DISTANCE_TO_MOVE = 40_000 ' driverState enumeration #0, RAMP_UP, FULL_SPEED, RAMP_DOWN, STOP pub main ' dbug.start(rxpin, txpin, mode, baudrate) dbug.startRxTx(31, 30, 0, 115200) ' start debug object waitcnt(clkfreq*3+cnt) 'Wait for Debug Startup dbug.Str(string(13,"Debug Start ")) pos.Setup ' start position sensor object ' pwmX.start(n, pin, freq) ' start PWM object pwmA.start(1, 4, 100) ' start a PWM to drive A side of valve pwmB.start(1, 5, 100) ' start a PWM to drive B side of valve repeat ' swing until power off dir := 1 move ' move up to measured top of swing dir := -1 move ' move dn to measured bottom of swing PUB Move dbug.Str(string(13,"Move Start ")) case driverState RAMP_UP: RampUp FULL_SPEED: FullSpeed RAMP_DOWN: RampDown distance += speed '??? needed might overflow dbug.Dec(distance) dbug.Char(13) PUB RampUp result := speed + ACCELERATION if result => MAX_SPEED speed := MAX_SPEED rampDownTarget := DISTANCE_TO_MOVE - distance if rampDownTarget =< 0 driverState := RAMP_DOWN else driverState := FULL_SPEED else speed := result ptmove(speed / 200) PUB FullSpeed ptmove(1) if distance + speed => rampDownTarget driverState := RAMP_DOWN PUB RampDown result := speed - ACCELERATION result #>= 1 if distance + result => DISTANCE_TO_MOVE speed := DISTANCE_TO_MOVE - distance driverState := STOP else speed := result ptmove(speed / 200) pub ptmove(level) | oldPos, apos ' Move one point up or down with position sense and ' Safety Stop or delay if actual move <> one point (from external forces). ' dir - plus = up, minus = down ' speed = duty = level = % of valve open or width of pulse ' PID would adjust level to meet a time and keep position until control change ' PID will be useful when motion is not continuous, to hold position oldPos := pos.avgpos repeat until pos.avgpos == (oldPos + 1) 'loop move to next position if dir == 1 'open correct A B valve coil pwmA.set(1, level) 'emit pwm to coil A up else pwmB.set(1, level) 'emit pwm to coil B dn ' Safety stop if motion faster or slower than expected apos := pos.avgpos ' if apos > oldPos + 1 'close valves pwmA.set(1, 0) ' pwmB.set(1, 0) abort if apos < oldPos - 1 'close valves pwmA.set(1, 0) ' pwmB.set(1, 0) abort {{ }}The safety algorithm hasn't been tested but a lot of the literature on professional robotics demands safety compliance because people have been killed by robots that don't stop when a human is in range of motion. It looks like detecting unexpected motion this way should work without adding special springy limbs or touch sensors but it may need tweaking.
Kirk,
As you know I've been working on a CNC machine which uses stepper motors. I'm still trying to figure out a good substitute for your hydraulic actuators. I wonder if using my own h-bridge to control a modified hobby servo motor would allow me to test a control algorithm using PWM as output and the magnetic encoder as input. I'm afraid the CNC stepper motors wouldn't be a good substitute. (I'm also not sure I want/need to use magnetic encoders on my CNC machine.)
If a hobby servo could be used as a substitute, then I could use one of the servos I'm modifying in this thread to test an algorithm with "jerk" (variable acceleration).
What do you think about my using a hobby servo to test the control algorithm?
I don't know how to control a CNC stepper motor or remote control hobby servo with PWM but how about just do what you think would work best in theory without physical hardware testing, except of course that it compiles and produces mathematically correct output?
I doubt an ordinary hobby servo would show much acceleration control although the larger quarter scale size with more weight attached might - it's just a guess.
Not surprisingly, getting a physical object to move in a way described by an equation isn't nearly as easy as plotting the positions in a spreadsheet.
There are several challenges to writing code which uses jerk. One of the challenges is to figure out when to apply the jerk and for how long in order to achieve a predetermined outcome. In the spreadsheets I just used arbitrary values for time and distance in order to have the curves the general shape I was after. Determining when to apply jerk to influencement precise movement is much more of a challenge.
Wikipedia has a good article about jerk. The bottom three equations on the page provide ways of calculating the required jerk needed in order to produce movement of a desired acceleration, speed or displacement. Since calculating the required jerk will likely involve cube roots, I'll probably shift to using floating point numbers in the calculations.
Rather than using the equations to calculate when to apply jerk to achieve a desired outcome, it's also possible to just monitor the system and observe what the effects of applied jerk are. These effects could then be used to determine when to apply jerk to achieve the opposite effect.
For example, the initial change in speed while jerk is applied to a system may be used to calculate when to apply negative jerk in order to achieve a desire maximum speed. The distance traveled while reaching maximum speed should be the same distance required to stop if the same magnitude of jerk (and max acceleration) is used to stop the system as was originally used to get the system moving.
The graphs in post #23 assume there was time to both reach the maximum acceleration and the maximum speed of a system. There will likely be times when the maximum speed will be reached prior to reaching the maximum acceleration. Here are a couple of graphs showing the displacement, speed and acceleration curves for such a system.
You can see the ramp up of the speed graph doesn't have a flat area representing constant acceleration.
There will likely be times when the distance to be moved will not be enough to allow the system to achieve its maximum speed. Here are a few graphs of this scenario.
You can see the speed curve not only doesn't have flat areas of constant acceleration but there also isn't a flat top area of constant speed.
I think the movement of both these cases could be computed by monitoring the position of and speed of a system and by using the information from initial movements to calculate appropriate actions to achieve a desired outcome.
While complex calculations may be avoided by monitoring initial speed and displacement, I think any sort of control system using these complex motions should have the ability to calculate actions required without the need of first monitoring initial movements.
There are likely times when the motion of a system will not be symmetrical with its initial and final movements. Particularly in a case of an emergency. In an emergency situation it may be required to use a larger jerk than normal. The system should be able to accommodate these situations.
I may use the simpler method of monitoring speed and displacement and assume symmetrical movements in order to come up with some initial test code to use with the encoder, however any sort of code used in a system with valuable equipment should have a more robust algorithm. The requirements of a robust control algorithm would also apply to any system large or strong enough to injure humans.
I don't think I have a question in all this. I'm just posting some thoughts on the subject.
The acceleration and speed were then computed based on this jerk amount with the following equations.
I use the prefix "pseudo" to remind myself I'm dealing with a scaled integer. The scaled value will need to be divided by the scale factor before being used in the real world. In this case, since I'm using the speed to control a continuous rotation servo, I use the following code in order to conver the "pseudoServoTargetSpeed" into a pulse used by the servo driver.
servoTargetSpeed[localIndex] := STOP + (direction[localIndex] * { } (pseudoServoTargetSpeed[localIndex] / PSEUDO_MULTIPLIER)) Servo.Set(servoPin[localIndex], servoTargetSpeed[localIndex])"direction" is either one or negative one depending on which way the servo is to rotate. "STOP" is 1500 which is the pulse length used to stop a CR servo. "PSEUDO_MULTIPLIER" is the scaling factor (1000 in this case).
Since I wasn't using any sort of feedback system to monitor the speed (at least not in a useful way), the ramped acceleration is kind of hard to see when using a motor with such limited speed range. I thought the whole concept of ramped acceleration might be easier to see if I had some sort of gauge to visually show the acceleration and speed amounts.
Having recently purchased some NeoPixel rings from AdaFruit, I decided to try using them as gauges to show the various motion parameters.
Here's the first of several video attempting to use the NeoPixel rings as demonstration tools.
Hopefully seeing the LEDs light up will make the concept of ramped acceleration more intuitive to some viewers. (If any views feel like the LEDs aided in their understanding off ramped acceleration, I hope you let me know.)
I'm been working with Kirk Fraser on this project. Kirk, do you think there would be any benefit in modifying this ramped acceleration algorithm to use a PWM object rather than the servo object? Or do you want to wait until I have some sort of sensor feedback with a PID control algorithm before switching to a PWM object?
I have a couple more videos I'm uploading to YouTube exploring the use of these LEDs as visualization aids. The one currently uploading shows when jerk is applied.
While I thought the combined gauge of acceleration, speed and distance wasn't intuitive, I like the combined gauge of jerk and acceleration.
In case I didn't make it clear in my earlier post, I do not think a CR servo is the best application for ramped acceleration. Ramped acceleration has its applications as explained in the Wikipedia article. Servos will generally be fine with ramping just the speed (with constant acceleration). I used a servo since it was an easy way to test the ramped acceleration algorithm on a real world device.
I have another video in the process of uploading to YouTube. I'll be posting it soon. It's every bit as exciting as these last two videos.
Coming soon, the final installment of the ring trilogy, Lord of the Jerk.
These segments are:
1. Acceleration build-up, with maximum positive jerk. {Labelled "a+, s+" in the video.}
2. Constant maximum acceleration (zero jerk) {Labelled "max a, s+" in the video.}
3. Acceleration ramp-down. approaching the desired maximum velocity, with maximum negative jerk. {Labelled "a-, s+" in the video.}
4. Constant maximum speed (zero jerk, zero acceleration) {Labelled "constant speed" in the video.}
5. Deceleration build-up, approaching the desired maximum deceleration, with maximum negative jerk. {Labelled "a+, s-" in the video.}
6. Constant maximum deceleration (zero jerk) {Labelled "max a, s-" in the video.}
7. Deceleration ramp-down. approaching the desired position at zero velocity, with maximum positive jerk. {Labelled "a-, s-" in the video.}
Since I had a couple extra LEDs not used in the "profile gauge" I added an eighth segment.
8. {{Labelled "stopped" in the video.}
I imagine most systems which would benefit from a ramped acceleration would also benefit (if not require) from some form of position and speed feedback.
I had hoped to use a magnetic encoder for position and speed feedback. I've modified my original AS5055 driver to include a speed measurement. Unfortunately wasn't successful in my attempt at aligning the magnet and sensor chip.
I've attached a couple videos demonstrating the erratic speed readings in the encoder hardware thread.
I haven't seen the full code but from the ring display videos it looks like the one way commanded motion system with constant jerk will serve my immediate testing need to operate a hydraulic valve using the PWM driver object. Your videos enlightened me to understand the position feedback is not actually necessary for a mechanical test safe from public contact. I can put that to use.
However, a larger hydraulic device which can damage human body parts that get in its way would be made more safe utilizing a position feedback system which shows error between commanded position and measured position to stop motion since it could indicate human contact, I can either use what you have with the PWM object or wait for this safety feature.
The Proportional Integral Derivative (PID) program apparently presents control features I do not need for my first round of testing. However, I do believe it will be useful in future testing and motion control applications. Thank you.
Kirk,
I uploaded the final video to the encoder PCB thread. I know you've mentioned a desire for a small PCB to use with the encoders. Are you planning on using the encoders to monitor the position of the equipment being moved by your hydraulic system?
I'll get to work on switching this demo from output intended for a servo to PWM output if you think it would be useful without position feedback.