Shop OBEX P1 Docs P2 Docs Learn Events
NEW! Quadrature Encoder + PID Controller — Parallax Forums

NEW! Quadrature Encoder + PID Controller

KyeKye Posts: 2,200
edited 2012-03-03 06:11 in Propeller 1
Haven't been able to post many driver updates in a while. Thank school for that =).

Anyway, here's a new piece of code that I designed for the stingray robot with quadrature encoders installed. http://www.lynxmotion.com/p-448-quadrature-motor-encoder-wcable.aspx

I don't know why Parallax does not sell these encoders. They work perfectly with the motors in the stingray robot. They also can be directly interfaced to the 5V port on the robot controller board. So everything is REALLY easy.

So, What's nice about this driver is that it preforms the sampling of the quadrature encoder... feeds that to an instantaneous speed measurement algorithm... then using that speed measurement it uses PID control to drive the motor the quadrature encoder is installed on to turn at a target speed.

All this power and control is handled in one core for the user. The API is posted below.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// PWM1C Proportional Integral Derivative Engine
//
// Author: Kwabena W. Agyeman
// Updated: 10/15/2010
// Designed For: P8X32A
// Version: 1.0
//
// Copyright (c) 2010 Kwabena W. Agyeman
// See end of file for terms of use.
//
// Update History:
//
// v1.0 - Original release - 10/15/2010.
//
// For each included copy of this object only one spin interpreter should acess it at a time.
//
// Nyamekye,
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Feedback Circuit:
//
// Motor Left - To H-Bridge, driven by the propeller chip.
//
// Motor Right - To H-Bridge, driven by the propeller chip.
//
// Encoder Left - From the quadrature encoder, driven by the encoder.
//
// Encoder Right - From the quadrature encoder, driven by the encoder.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Object "PWM1C_PIDEngine" Interface:
PUB  quadraturePosition(deltaPosition)
PUB  quadratureSpeed(targetSpeed)
PUB  PIDEngineStart(motorLeft, motorRight, encoderLeft, encoderRight, PGain, IGain, DGain)
PUB  PIDEngineStop
Program:     136 Longs
Variable:      4 Longs
______________________________________
PUB  quadraturePosition(deltaPosition)
 4 Stack Longs
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Returns the delta or absolute position of the quadrature encoder in ticks.
 //
 // DeltaPosition - True to get the delta position since the last call. False to get the absolute position since last call.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_________________________________
PUB  quadratureSpeed(targetSpeed)
 4 Stack Longs
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Drives the quadrature encoder at a target speed using proportional, integral, and derivative control.
 //
 // TargetSpeed - The target speed in ticks per second. The target speed should be reachable by the system.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
__________________________________________________________________________________________
PUB  PIDEngineStart(motorLeft, motorRight, encoderLeft, encoderRight, PGain, IGain, DGain)
 13 Stack Longs
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Starts up the PID driver running on a cog.
 //
 // Returns true on success and false on failure.
 //
 // MotorLeft - The left drive pin of the h-bridge. -1 to disable output.
 // MotorRight - The right drive pin of the h-bridge. -1 to disable output.
 // EncoderLeft - The left leading signal pin of a quadrature encoder. -1 to disable input.
 // EncoderRight - The right leading signal pin of a quadrature encoder. -1 to disable input.
 // PGain - Power of two gain for the proportional drive. (0 to 31). Where zero disables the proportional factor.
 // IGain - Power of two gain for the integral drive. (0 to 31). Where zero disables the integral factor.
 // DGain - Power of two gain for the derivative drive. (0 to 31). Where zero disables the derivative factor.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
__________________
PUB  PIDEngineStop
 3 Stack Longs
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Shuts down the PID driver running on a cog.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                  TERMS OF USE: MIT License
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
I'll be posting this to the OBEX soon. Enjoy!
zip
zip
#PWM1C_PIDDemo.zip
16K

Comments

  • KyeKye Posts: 2,200
    edited 2010-10-15 15:04
    Why can't I edit the title... qadrature should be quadrature.
  • John AbshierJohn Abshier Posts: 1,116
    edited 2010-10-15 15:35
    What PID values worked for your Stingray?

    John Abshier
  • KyeKye Posts: 2,200
    edited 2010-10-15 16:19
    The default values will work. Once you connect using the serial terminal try to set the speed to ~1000 ticks per seond.

    Then play arround with the pin numbers of the encoder and motor until the wheel starts to spin slowly vs just going at full speed in one direction or another.

    The alogrithm can't home in on the target if the effect it applies goes the opposite direction that it thinks.
  • ercoerco Posts: 20,259
    edited 2010-10-15 16:22
    Kye: Sounds very promising; I would be interested to see a video of what these encoders can make the Stingray do. Not much other Stingray encoder activity here lately, so YOU DA MAN! :)
  • KyeKye Posts: 2,200
    edited 2010-10-15 19:01
    Mmm, that's going to be a PITA for me to post. Sorry.
  • WBA ConsultingWBA Consulting Posts: 2,935
    edited 2010-11-24 23:42
    Kye, I corrected the typo in the title.
  • KyeKye Posts: 2,200
    edited 2010-11-25 06:33
    Thanks,

    I haven't got many hits from people asking about this driver. It really does give almost perfect speed control to the stingray robot if you have quadrature encoders installed.
  • lardomlardom Posts: 1,659
    edited 2010-11-25 08:56
    I am one of the silent members that study your code from time to time. Your talent is evident.
  • potatoheadpotatohead Posts: 10,261
    edited 2010-11-25 10:43
    I'll easily second that Kye. You rock, and your efforts are seriously appreciated.

    Happy Turkey Day.

    Personally, I'm not in a mode where this code makes sense to run. That's probably all it is. Everybody is doing their thing, sometimes aligned with a code project, sometimes not.
  • RobotWorkshopRobotWorkshop Posts: 2,307
    edited 2010-11-26 18:06
    Hello Kye,

    I'm going to see if I can try this out on the weekend since I have the Stingray motor kit, lynxmotion encoders, and the Parallax control board for the Singray. When I briefly looked over the demo code to see what pins you assigned to the encoder I was a bit puzzled since I only see one pin for the right and one pin for the left instead of two pins each. Are you only using half of each encoder??

    Robert
  • KyeKye Posts: 2,200
    edited 2010-11-26 19:51
    The quadrature encoder has two pins... so I was like... lets name them left and right.

    Because one leads the other when the wheel is spinning left, etc.
  • evanhevanh Posts: 16,113
    edited 2010-11-26 20:53
    The two phases have been named P and Q at times but industrial naming is pretty consistently A and B.

    Direction is completely arbitrary although there does appear to be some consistency with industrial servos. I'll see if I can find out what it is ...
  • RobotWorkshopRobotWorkshop Posts: 2,307
    edited 2010-11-26 21:02
    Kye wrote: »
    The quadrature encoder has two pins... so I was like... lets name them left and right.

    Because one leads the other when the wheel is spinning left, etc.

    It wasn't clear when I first looked at it and it appeared that you were using two motors and two encoders (using only 1 channel per encoder) instead of just one motor and encoder combo.

    Robert
  • KyeKye Posts: 2,200
    edited 2010-11-26 22:11
    I guess that's true. Mmm, A and B are not verbose enough however. Left and right it will stay for now.

    Thanks though,
  • evanhevanh Posts: 16,113
    edited 2010-11-27 06:48
    Common sense says A leads B for clockwise and B leads A for anticlockwise but even making that assumption it's still meaningless unless a particular side of the motor/encoder is specified for evaluating from.

    Ok, dug out an old 24 volt, 100 line, single-ended, push-pull encoder that came off a PLC. Hooked it up to a power pack and scope ... and ... A leads B when turning the shaft clockwise and viewed from the shaft attachment end. This makes sense. It is the way I would have built it myself.

    And motors typically have the encoder attached on the rear end of the motor, so there wouldn't be any reversal of direction from that.

    So, from my sample of one, if this is the norm, then it would explain why I've noticed some consistency.

    As for naming; you could use quadrature, phase or channel. Channel is the common name in industry because there is always a third signal allocated in the cabling - known as an index or reference - so you have Channel A, Channel B and Channel C. Channel C can also be labeled Channel Z or Channel I to denote it's special reference finding purpose.
  • evanhevanh Posts: 16,113
    edited 2010-11-27 07:11
    Oh, and clockwise is presumed to be positive motion, while anticlockwise is negative motion. But, again, not a guarantee.

    Any one of these points can cause a direction reversal. Consequently, software/jumper settings for flipping the direction is a common feature on servo drives.
  • KyeKye Posts: 2,200
    edited 2010-11-27 11:27
    Good points. I'm not gonna focus on renaming that stuff now however, I'm working on a new major file sysem driver release.
  • Ray0665Ray0665 Posts: 231
    edited 2010-12-29 13:49
    Nice work, at the risk of bringing up and old issue I think you need to add a comment somewhere indicating that this is for one wheel. My old brain was very confused at first.

    I am running very successfully on my stingray but like most hardware I had to tweek the PID values slightly (P=15,I=12,D=12) as there was initially considerable overshoot and about 2 seconds of hunting when given a step setting (IE start with 10k after a few seconds goto 50K wait again and goto 10k) I did it mostly by ear with the bot sitting on its stand. Tricky adjustment but take your time it works great.
  • KyeKye Posts: 2,200
    edited 2010-12-29 18:46
    I think I should then!

    Please if you have time give the object a review. Thanks,
  • Ray0665Ray0665 Posts: 231
    edited 2011-01-07 12:21
    So after using this for a few days now with no problems. I would like to suggest that you incorporate a ramping function.
    This will take some of the bumpiness out of speed changes and in the case of the stingray maybe keep the tail from lifting.
    WaddaUsay Kye (I also gave the object a review)
  • KyeKye Posts: 2,200
    edited 2011-01-07 14:38
    Mmm, I have no time however. I will keep that in mind.

    Thanks,
  • whitehat0702whitehat0702 Posts: 3
    edited 2012-03-01 23:18
    I have just bought QME-01 and installed on 1 wheel of Stingray for testing. I tested reading the encoder value first and it seems normal. But when I tried your PID object, the speed is the same for all speed value in quadratureSpeed function. I tried from 100, 200, 3875, ... value for that function but the output result always be around 30.000.
    I don't know what's wrong here. Do you have any idea about this?
    Thanks.

    P/S: The piece of code for testing is below:

    [FONT=courier new]ifnot( pid.PIDEngineStart(_motorLeftOutputB, _motorLeftOutputA, _encoderLeftInputB, _encoderLeftInputA, _PGain, _IGain, _DGain))
[/FONT][FONT=courier new]  reboot[/FONT]
[FONT=courier new]pid.quadratureSpeed(3875)
[/FONT][FONT=courier new]Delay_MS(2000)  [COLOR=#008080]'Wait for stable speed[/COLOR]
repeat 5[/FONT]
[FONT=courier new]  serial_io.txstring(str.integerToDecimal(pid.quadraturePosition(true), 10))   [COLOR=#008080]'Output delta position[/COLOR]
  Delay_MS(500)
[/FONT]
  • Duane DegnDuane Degn Posts: 10,588
    edited 2012-03-01 23:58
    whitehat0702,

    I've been playing around with encoders a lot lately and something like this just happened to me. I had the encoder pins switched from what they should be on one of the motors. Switching the wires solved the problem.

    You wouldn't have to switch the physical pins with Kye's method. You could just switch the pin names in the call the the PID object.

    If the encoder pins are switched, the motor will think it has never reached the correct speed and just go to top speed no matter the speed setting (except zero).
  • whitehat0702whitehat0702 Posts: 3
    edited 2012-03-02 00:29
    Oh, that really works. Thank you very much !!!

    The PID object works on my Stingray now. But I still have one thing I do not understand. The value input to quadratureSpeed function is 3875 but when the output of deltaPosition is around 1530. Is there some kind of ratio between the input and output value?
  • Toby SeckshundToby Seckshund Posts: 2,027
    edited 2012-03-02 01:03
    So that I do not sit staring at a screen for another week I pulled out my finger and started to refurbish a small model makers lathe, that I rescued from my fathers bin (It is older than me, which would place it in the later part of the Iron Age). The purpose of this is so that I can get around to that X,Y,Z table for PCBs etc.

    On this I am going to try the encoders out of a ball mouse on the tread drives so these drivers will be tried out. The Prop will give all of the display options too.
  • KyeKye Posts: 2,200
    edited 2012-03-02 06:11
    Oh, that really works. Thank you very much !!!

    The PID object works on my Stingray now. But I still have one thing I do not understand. The value input to quadratureSpeed function is 3875 but when the output of deltaPosition is around 1530. Is there some kind of ratio between the input and output value?

    The problem is the physical mechanical make-up of the system. You'll notice that by just tapping the stingray wheel (very little deflection) the encoder object reads about +-40 counts, yes it is that sensitive. This makes it very hard to control the system. My PID driver object is pretty minimal in design. Its meant to work good enough. Not prefect.

    If you want position feedback control you will need to adjust the speed input value until the delta changes at the right value per second. You could do this by using PID! o_O

    ---

    There's actually alot of messy stuff in the code that runs the PID control for the encoder feedback. Doing speed control using encoders is not as simple as you would think. There is a really subtle but huge problem with making the system work right that is due to the sample rate the system runs at and when you calculate the speed.

    Position feedback control with encoders is actually easier. But my driver does speed control instead.

    I guess I would need another PID loop in the code to give you position control. Or I could have done position control first with speed control on-top of position control. Or maybe I could have done some numerical methods stuff...

    Anyway,

    Thanks,
  • whitehat0702whitehat0702 Posts: 3
    edited 2012-03-03 06:11
    Oh, anyway, thanks for your work and your reply. I'll try to tune the PID.
Sign In or Register to comment.