Parallax Balance Bot

graffix · 2011-07-27 12:15

I may be a little wrong on some things.
I have a Us Digital rotory encoder from LynxMotion QME-01
Im using a BS2 MCU.
Memsic 2125 accelerometer
Lisy300 gyro
HB25 motor controllers
Currently all the parts arent being used.I tried creating different code for each at different times with no sucsess.
post # 9 is probably the most functional example of proportional

When power is applied to the motor Im gettn a reading but when the motor stops the encoder resets back to zero.

I'd like to keep the variable from reseting back to zero?

' {$STAMP BS2}
' {$PBASIC 2.5}

' HomeBrew-Gyro-Tilt-levellights-encoder-debug.bs2
' Measure room temperature tilt,and give gyro reading in x plane.level indicator lights.

' -----[ I/O Definitions ]-------------------------------------------------

Dout             PIN     0               ' P0 <-- Dout (LISY300.2)
SCLK             PIN     1               ' P1 --> SCLK (LISY300.4)
CSn              PIN     2               ' P2 --> /CS  (LISY300.5)
tiltx            PIN     6               ' memsic 2125 X axis
tilty            PIN     7               ' memsic 2125 Y axis
encoderA         PIN     9               'Lynx Motion motor encoder QME-01
encoderB         PIN     8               'Lynx Motion motor encoder QME-01
BiColorledGREEN  PIN     13              ' x axis down light led green when high and low 12
BiColorledRED    PIN     12              ' x axis up light led red when high and low 13

' -----[ Constants ]-------------------------------------------------------

Yes             CON     1               ' Yes Constant
No              CON     0               ' No Constant


' -----[ Variables ]-------------------------------------------------------

value           VAR     Word            ' ADC Result Value
x               VAR     Word
y               VAR     Word
a               VAR     Word
b               VAR     Word
' -----[ EEPROM Data ]-----------------------------------------------------



' -----[ Initialization ]--------------------------------------------------

Initialization:
  HIGH CSn
  LOW SCLK
  PAUSE 250


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Read_Gyro
      DEBUG "ADC Value:", DEC5 value
    PAUSE 250
    PULSIN 9, 1, a
    PULSIN 8, 1, b
    PULSIN 6, 1, x
    PULSIN 7, 1, y
      IF x > 2525 THEN HIGH 13: LOW 12        ' x axis down light led 1 green
      IF x < 2525 THEN LOW 13: HIGH 12         ' x axis up light led 2 red
      IF x = 2525 THEN PAUSE 250
    DEBUG CLS, ? X, ? Y, ? a, ? b

    PAUSE 100
  LOOP
  STOP


' -----[ Subroutines ]-----------------------------------------------------

Read_Gyro:
  LOW CSn
  SHIFTIN Dout, SCLK, MSBPOST, [value\13]
  HIGH CSn
  RETURN

This is part of a balance bot project.I have parts of code and changes made to the original design ie.I switched out the LMD 18200 H bridges for HB25 motor controllers

.I need help compiling it all together.Possiblily with a PID loop.
here are more bits of code that need adjustment.

' {$STAMP BS2}
' {$PBASIC 2.5}

' HomeBrew-Gyro-Tilt-motor-encoder-debug.bs2
' Measure room temperature tilt,and give gyro reading in x plane.

' -----[ I/O Definitions ]-------------------------------------------------

Dout            PIN     0               ' P0 <-- Dout (LISY300.2)
SCLK            PIN     1               ' P1 --> SCLK (LISY300.4)
CSn             PIN     2               ' P2 --> /CS  (LISY300.5)
tiltx           PIN     6
tilty           PIN     7
encoderA        PIN     9
encoderB        PIN     8
motorpowr       PIN     15              ' motor power
motorDir        PIN     14              ' motor direction

' -----[ Constants ]-------------------------------------------------------

Yes             CON     1               ' Yes Constant
No              CON     0               ' No Constant


' -----[ Variables ]-------------------------------------------------------

value           VAR     Word            ' ADC Result Value
x               VAR     Word
y               VAR     Word
a               VAR     Word
b               VAR     Word
' -----[ EEPROM Data ]-----------------------------------------------------



' -----[ Initialization ]--------------------------------------------------

Initialization:
  HIGH CSn
  LOW SCLK
  PAUSE 250


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Read_Gyro
      DEBUG "ADC Value:", DEC5 value
      PAUSE 10

      HIGH 15
      PAUSE 100
      LOW 15
      PAUSE 150

    PULSIN 9, 1, a
    PULSIN 8, 1, b
    PULSIN 6, 1, x
    PULSIN 7, 1, y

      IF x > 2525 THEN HIGH 14
      IF x < 2525 THEN LOW 14
      IF x = 2525 THEN PAUSE 250

    DEBUG CLS, ? X, ? Y, ? a, ? b
    PAUSE 10

  LOOP
  STOP


' -----[ Subroutines ]-----------------------------------------------------

Read_Gyro:
  LOW CSn
  SHIFTIN Dout, SCLK, MSBPOST, [value\13]
  HIGH CSn
  RETURN

'   {$STAMP BS2}
'   {$PBASIC 2.5}

' -----[ Program Description ]---------------------------------------------

' This program tests the HB-25 by waiting for it to power up, then pulsing
' the output to ramp the motors up in opposite directions, wait 3 seconds
' then ramp them back down to a stopped position.  While the code is
' written for two HB-25/motors you can use it with just one by commenting
' out or removing the lines for the second motor, commented below.  If you
' have two HB-25/motors connected, remember to remove the jumper block from
' the second HB-25.


' -----[ I/O Definitions ]-------------------------------------------------

HB25            PIN     15              ' I/O Pin For HB-25


' -----[ Variables ]-------------------------------------------------------

index           VAR     Word            ' Counter For Ramping
x               VAR     Word
y               VAR     Word
a               VAR     Word
b               VAR     Word

' -----[ Initialization ]--------------------------------------------------

DO : LOOP UNTIL HB25 = 1                ' Wait For HB-25 Power Up
LOW HB25                                ' Make I/O Pin Output/Low
PAUSE 5                                 ' Wait For HB-25 To Initialize
PULSOUT HB25, 750                       ' Stop Motor 1
PAUSE 1                                 ' 1 mS Delay
PULSOUT HB25, 750                       ' Stop Motor 2 (If Connected)
                                        ' The Above 2 Lines May Be Removed
                                        ' If You Are Using Single Mode
' -----[ Program Code ]----------------------------------------------------

Main:
 DO
  PAUSE 20                              ' Wait 20 mS Before Ramping
  PULSIN 9, 1, a
    PULSIN 8, 1, b
    PULSIN 6, 1, x
    PULSIN 7, 1, y
  DEBUG CLS, ? X, ? Y, ? a, ? b
  FOR index = 0 TO 250                  ' Ramp Up To Full Speed
    PULSOUT HB25, 750 + index           ' Motor 1 Forward
    PAUSE 1                             ' 1 mS Delay For Motor 2 Pulse
    PULSOUT HB25, 750 - index           ' Motor 2 Reverse
    PAUSE 20                            ' 20 mS Smoothing Delay
  NEXT
  PAUSE 3000                            ' Wait 3 Seconds
  FOR index = 250 TO 0                  ' Ramp Back Down
    PULSOUT HB25, 750 + index           ' Motor 1 Forward Slowing
    PAUSE 1                             ' 1 mS Delay For Motor 2
    PULSOUT HB25, 750 - index           ' Motor 2 Reverse Slowing
    PAUSE 20                            ' 20 mS Smoothing Delay
  NEXT
 LOOP

Im out of time for now (off to work) I know its messed
here is sample PID loop info any help I can get with setting scales and algorithms would be excellent!!!
http://forums.parallax.com/showthread.php?77656

stamptrol · 2011-07-27 12:55

I think you're expecting something different from the PULSIN command than its meant to deliver.

PULSIN measures the width of an incoming pulse train and therefore can measure speed (or some other value) by watching the width of the pulses coming in. BUT, if the pulses are too far apart or too large (like when the shaft has stopped, they go to zero). Trying to store the value in EEPROM will not help. As well, if the pulses get too fast, there comes a point the Stamp can't process the value. More detail is available in the Helpfile.

Cheers,

graffix · 2011-07-27 14:39

Thanks Tom.

erco · 2011-07-27 16:14

graffix: Per Tom, there is a timeout on pulse width when using PULSIN, which varies by stamp flavor: 131 ms for vanilla BS2 vs. 53 ms for a BS2P.

I wanna see your balance bot in action, a subject near & dear to my heart!

graffix · 2011-07-28 10:35

It works slightly with just the memsic and the HB25's

' {$STAMP BS2}
' {$PBASIC 2.5}

' HomeBrew-Gyro-Tilt-motor-encoder-debug.bs2
' Measure room temperature tilt,and give gyro reading in x plane.

' -----[ I/O Definitions ]-------------------------------------------------

Dout            PIN     0               ' P0 <-- Dout (LISY300.2)
SCLK            PIN     1               ' P1 --> SCLK (LISY300.4)
CSn             PIN     2               ' P2 --> /CS  (LISY300.5)
tiltx           PIN     6
tilty           PIN     7
encoderA        PIN     9
encoderB        PIN     8
HB25            PIN     15              ' motor power
bicolorledgreen PIN     12              ' motor direction
bicolorledred   PIN     13

' -----[ Constants ]-------------------------------------------------------

Yes             CON     1               ' Yes Constant
No              CON     0               ' No Constant


' -----[ Variables ]-------------------------------------------------------

value           VAR     Word            ' ADC Result Value
x               VAR     Word
y               VAR     Word
a               VAR     Word
b               VAR     Word
index           VAR     Word            ' Counter For Ramping
' -----[ EEPROM Data ]-----------------------------------------------------



' -----[ Initialization ]--------------------------------------------------

Initialization:
  HIGH CSn
  LOW SCLK
  PAUSE 250


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Read_Gyro
      DEBUG "ADC Value:", DEC5 value
      PAUSE 10

      LOW 15
      PAUSE 5


    PULSIN 9, 1, a
    PULSIN 8, 1, b
    PULSIN 6, 1, x
    PULSIN 7, 1, y

      IF x > 2525 THEN PULSOUT 15,650 : HIGH 12 : LOW 13
      IF x < 2525 THEN PULSOUT 15,850 : HIGH 13 : LOW 12
      IF x = 2525 THEN PAUSE 250

    DEBUG CLS, ? X, ? Y, ? a, ? b
    PAUSE 10

  LOOP
  STOP


' -----[ Subroutines ]-----------------------------------------------------

Read_Gyro:
  LOW CSn
  SHIFTIN Dout, SCLK, MSBPOST, [value\13]
  HIGH CSn
  RETURN

graffix · 2011-07-28 14:14

Here is some video of the bot in process.Yes its wood.http://www.youtube.com/watch?v=sQS9N7ZlSxk
http://36ohk6dgmcd1n.yom.mail.yahoo.net/om/api/1.0/openmail.app.invoke/36ohk6dgmcd1n/4/1.0.35/us/en-US/view.html#

graffix · 2011-07-30 04:48

well, per this Im gonna have to use at least a BS2sx to be able to read the encoders?

http://forums.parallax.com/showthread.php?121179-Reading-Quadrature-Encoders

in tryn to use this code with a added DEBUG line im getting errors

' {$stamp bs2}

' Program to act as high speed quadrature counter


' Program Name: quadhicount.bs2
' Last Update: Mar 10/01 tjs

DIRH=%11111111

counter VAR Word
counter2 VAR Word
counter3 VAR Word
old VAR Nib
new VAR Nib
direct VAR Byte

' Channel a and b of encoder go on inputs 4 and 5 in this case.
quad:
   counter=127         'load a dummy value so can count up or down
   new = INB & %0011      ' get initial condition

start:
   old = new & %0011      ' remember initial setting

again:
   new = INB & %0011      ' get new reading
   IF new=old THEN again
   direct=new.BIT1 ^ old.BIT0   ' XOR left bit of new with right bit of old to get direction
   IF direct=1 THEN cw      ' jump here to increment
   LOW 0                    ' pin 0 turns led off id turning counter-clockwise
   counter=counter-1         ' jump here to decrement
   OUTH=counter.LOWBYTE
   DEBUG HOME, DEC3 counter
   GOTO start

cw:
   HIGH 0   ' pin 0 turns on led if turning clockwise
   counter=counter+1
   OUTH=counter.LOWBYTE  'turn on leds on pins 8 through 15 to watch count
   GOTO start

graffix · 2011-08-25 22:12

I have something close I think, but I'd like the Kp value to be 3.35 but it wont accept the decimal digits.How can I change it?
2515 is level divide that by 3.35 it gives me about 750 which is used to control the motor.
as is,it seems to work right but only as if like 2250 were level

' {$STAMP BS2}
' {$PBASIC 2.5}

tiltx          PIN       6
hb25           PIN       15

'---Variable----
x              VAR       Word

' ---Constants---

Kp         CON    3
SetPoint    CON    2515

'---Main---
DO
  PULSIN 6,1,x
  DEBUG CLS, ? X, ? 15
  PULSOUT 15, x / Kp
  PAUSE 100
LOOP

graffix · 2011-08-25 23:07

This is about the closet Ive come it only uses the proportional part of the PID loop and might work well with a super light bot with continuous rotation servos
I added - 93 and it works more at level but LOL its goes the opposite direction any suggestions?fixed
also added a drive variable that displays the pulsout value used to drive the motors and debug setpoint,error,and drive values

' {$STAMP BS2}
' {$PBASIC 2.5}

tiltx          PIN       6
hb25           PIN       15

'---Variable----
x              VAR       Word
drive          VAR       Word
error          VAR       Word(2)

' ---Constants---

Kp         CON    3
SetPoint    CON    2543

'---Main---
DO
  drive = x / Kp -93
  error = x - setpoint
  PULSIN 6,1,x
  DEBUG CLS, ? setpoint, ? X, ? error, ? drive
  PULSOUT 15, drive
  PAUSE 100

LOOP

graffix · 2011-08-25 23:48

Well I solved it I switched the leads to the motors. It needs more though the rate at which Im falling is to much to compansate?Hmm...Maybe the gyro or I need more of the PID loop?

erco · 2011-08-26 03:12

Gyro is required with an accelerometer AFAIK, encoders are optional.

Are you modeling your robot after someone else's? Tough project from scratch. I made a thread with lots of links to other balance bots: http://forums.parallax.com/showthread.php?126312-Balancing-Bot&highlight=balancing+bot Check out those references.

graffix · 2011-08-26 07:36

Yeah Ive read the Programing and Customizing the multicore Propellers section on Hannos Dance bot.It uses different parts but they are the same types.Also what Info that is available about example PID loops.What does AF AIK mean?The examples Ive read dont quiet match though they use heat and fans or IR distance to follow objects the PID principle is the same.The algorithms are not a good match though for a balance bot PID loop.So if I can get some help in the area of using the sensor values and Making the different equations for the PID loop as Ive done with the proportional section.Would be great.Currently It ramps up the motors proportional to the angle at which its falling.It needs more of the accumulative errors over time solved.Like OK im falling in one direction the motor starts to correct the issue but its not enough Im still falling same direction and by the time Id hit the ground the motors are full speed.Seems like a higher Mast (gettn the accellerometer higher up) would help this but its not the answer.

graffix · 2011-08-26 08:04

This snippet of code may work but It needs help there is the original un edited version also

original

' PidMathExample.bs2
' Demonstrates how a combination of proportional, integral, and
' derivative control influence error correction in a feedback loop.
 
' {$STAMP BS2}
' {$PBASIC 2.5}
 
SetPoint       CON     0                     ' Set point
Kp             CON     10                    ' Proportionality constant
Ki             CON     10                    ' Integral constant
Kd             CON     10                    ' Derivative constant
 
Current        CON     0                     ' Array index - current error
Accumulator    CON     1                     ' Array index - accumulated error
Previous       CON     2                     ' Array index - previous error
Delta          CON     3                     ' Array index - change in error
 
sensorInput    VAR     Word                  ' Sensor input variable
error          VAR     Word(4)               ' Four different types of errors
p              VAR     Word                  ' Proportional term
i              VAR     Word                  ' Integral term
d              VAR     Word                  ' Derivative term
drive          VAR     Word                  ' Output
 
DO
 
  DEBUG "Enter sensor input value: "
  DEBUGIN SDEC sensorInput
 
  ' Calculate error.
  error(Current) = SetPoint - sensorInput
 
  ' Calculate proportional term.
  p = Kp * error(current)
 
  ' Calculate integral term.
  error(Accumulator) = error(Accumulator) + error(Current)
  i = Ki * error(Accumulator)
 
  ' Calculate derivative term.
  error(Delta) = error(Current) - error(Previous)
  d = Kd * error(delta)
 
  ' Calculate output.
  drive = p + i + d
 
  ' Display values.
  DEBUG CR, CR, "ERROR", CR,
        SDEC ? SetPoint, SDEC ? sensorInput, SDEC ? error(Current), CR,
        "PROPORTIONAL", CR,
        SDEC ? Kp, SDEC ? error(Current), SDEC ? p, CR,
        "INTEGRAL", CR,
        SDEC ? Ki, SDEC ? error(accumulator), SDEC ? i, CR,
        "DERIVATIVE", CR, 
        SDEC ? Kd, SDEC ? error(Delta), SDEC ? d, CR,
        "OUTPUT", CR, 
        SDEC ? p, SDEC ? i, SDEC ? d, SDEC ? drive, CR, CR
 
  ' Save current error to previous error before next iteration.
  error(Previous) = error(Current)
 
LOOP

edited

' PidMathExample.bs2
' Demonstrates how a combination of proportional, integral, and
' derivative control influence error correction in a feedback loop.

' {$STAMP BS2}
' {$PBASIC 2.5}



SetPoint       CON     2538                     ' Set point
Kp             CON     3                    ' Proportionality constant
Ki             CON     1                    ' Integral constant
Kd             CON     1                    ' Derivative constant

Current        CON     0                     ' Array index - current error
Accumulator    CON     1                     ' Array index - accumulated error
Previous       CON     2                     ' Array index - previous error
Delta          CON     3                     ' Array index - change in error

sensorInput    VAR     Word                  ' Sensor input variable
error          VAR     Word(4)               ' Four different types of errors
p              VAR     Word                  ' Proportional term
i              VAR     Word                  ' Integral term
d              VAR     Word                  ' Derivative term
drive          VAR     Word                  ' Output

DO

  PULSIN 6,1,sensorInput

  DEBUG CLS, ? sensorInput
  PAUSE 100

  ' Calculate error.
  error(Current) = SetPoint - sensorInput

  ' Calculate proportional term.
  p = Kp -88 * error(current)

  ' Calculate integral term.
  error(Accumulator) = error(Accumulator) + error(Current)
  i = Ki * error(Accumulator)

  ' Calculate derivative term.
  error(Delta) = error(Current) - error(Previous)
  d = Kd * error(delta)

  ' Calculate output.
  drive = p + i + d MIN 650 MAX 850

  ' Display values.
  DEBUG CR, CR, "ERROR", CR,
        SDEC ? SetPoint, SDEC ? sensorInput, SDEC ? error(Current), CR,
        "PROPORTIONAL", CR,
        SDEC ? Kp, SDEC ? error(Current), SDEC ? p, CR,
        "INTEGRAL", CR,
        SDEC ? Ki, SDEC ? error(accumulator), SDEC ? i, CR,
        "DERIVATIVE", CR,
        SDEC ? Kd, SDEC ? error(Delta), SDEC ? d, CR,
        "OUTPUT", CR,
        SDEC ? p, SDEC ? i, SDEC ? d, SDEC ? drive, CR, CR

  PAUSE 100
  PULSOUT 15,drive

  ' Save current error to previous error before next iteration.
  error(Previous) = error(Current)

LOOP

graffix · 2011-08-28 05:54

Ive noticed that the setpoint(point at which the bot balances) changes slightly.Ive seen a balance bot that when balanced by hand the operator pushes a button and manualy sets the set point.Any ideas?

graffix · 2011-08-30 06:03

In the example here http://forums.parallax.com/showthread.php?77656
example two on integral
you cant simply change Change the drive = i statement to drive = i + Offset MIN 650 MAX 850.
√······· Run the program and verify that the integral output is limited to values between 650 and 850.· Try entering 2 repeatedly, then -2 repeatedly.
Offset hasnt been declared.
so offset = ?

You cant use offset = ? either
so i have
drive = i + setpoint - current MIN 650 MAX 850
is this correct?

erco · 2011-08-30 17:08

Check out Ratronic's latest work with a ping & prop: http://forums.parallax.com/showthread.php?134189-Barely-balancing

graffix · 2011-08-30 20:13

latest portion of code debugs accelerometer,error,gyro,and drive values
the drive value is a proportional acc. reading

now Im looking to have the gyro feed the derivative portion of the PID loop?Oh boy...

' {$STAMP BS2}
' {$PBASIC 2.5}

Dout            PIN     0               ' P0 <-- Dout (LISY300.2)
SCLK            PIN     1               ' P1 --> SCLK (LISY300.4)
CSn             PIN     2               ' P2 --> /CS  (LISY300.5)
tiltx           PIN     6
hb25            PIN     15

'---Variable----
x              VAR       Word
drive          VAR       Word
error          VAR       Word(2)
value          VAR       Word            ' ADC Result Value

' ---Constants---

Kp          CON    3
SetPoint    CON    2541

' -----[ Initialization ]--------------------------------------------------

Initialization:
  HIGH CSn
  LOW SCLK
  PAUSE 250

'---Main---
DO
  GOSUB Read_Gyro
      DEBUG "Gyro Reading:", DEC5 value
      PAUSE 100

  drive = x / Kp -97
  error = x - setpoint
  PULSIN 6,1,x
  DEBUG CLS, ? setpoint, ? X, ? error, ? drive
  PULSOUT 15, drive
  PAUSE 100

LOOP

' -----[ Subroutines ]-----------------------------------------------------

Read_Gyro:
  LOW CSn
  SHIFTIN Dout, SCLK, MSBPOST, [value\13]
  HIGH CSn
  RETURN

erco · 2011-08-31 08:26

Good progress! I know I'm restating the obvious when I say that the DEBUG statements are just for your observation of data gathered, and that you must comment them out to really test robot balance, as the comments will slow down program execution too much.

Yours,
Captain Obvious

graffix · 2011-08-31 16:04

Thanks erco.It had slipped my mind.I don't have a good grasp on the timing issues I'm certain to face.

graffix · 2011-10-13 03:21

I made some slight changes to my bot.I added a ping sensor to the bottom.Using only that the bot is balancing only slightly better,still very jerky.It stays upright though, probably a short amount of time.Again though no functional PID loop.

erco · 2011-10-13 20:07

A Ping should work great, if your code works properly and your motors are fast enough. Here's my first reported success, using an IR sensor instead of Ping, and just Proportional control, not PID. Code is posted a few replies later.

http://forums.parallax.com/showthread.php?126312-Balancing-Bot&p=965006&viewfull=1#post965006

danny_maertens · 2012-01-24 11:21

If anyone has a finished code i'd love to see it. Or if anyone can help me I am using the bs2, LISY300 Gyroscope module, and the MMA7455 3-axis accelerometer module. It's for a school project, it is an independent study and there is no teacher, so there is no one i can ask for for help. If you could spare some time it would be greatly appreciated. Thanks

Parallax Balance Bot

Comments