BalancingRobot

caskaz · 2009-11-21 14:34

I also made up BalancingRobot to read it for stamp.
I used MX2125 instead of Ping-sensor and I used single motor.
Rewrited program to Prop it for stamp.

But it don't operate at all. Soon fall out.
I changed Kp/Ki/Kd much , but no good.
Why not operate?
Please give me any hint.

CON
  _clkmode = xtal1 + pll16x                  ' System clock &#8594; 80 MHz
  _xinfreq = 5_000_000
  scale = 80

  Kp = 4
  Ki = 3
  Kd = 5


  DriveEnablePin = 3
  DriveForwardPin = 4
  DriveReversePin = 5

VAR
  long  stack1[noparse][[/noparse]20]
  long xmG

OBJ
  debug : "FullDuplexSerialPlus"             ' Debugger                                              

PUB main | setPoint, pTime, cur_error, sum_error, last_error, secondlast_error, delta_error, p, i, d, drive
' Initialize
  debug.Start(31, 30, 0, 57600)              ' Start FullDuplexSerialPlus 
  waitcnt(clkfreq+cnt)                       ' Wait for PST to get connected
  debug.tx(Debug#CLS)
  setPoint := 500
  dira[noparse][[/noparse]driveEnablePin]~~ ' make output pin *output*
  dira[noparse][[/noparse]driveForwardPin]~~ ' Make drive forward pin output
  dira[noparse][[/noparse]driveReversePin]~~ ' Make drive Reverse pin output
  outa[noparse][[/noparse]driveEnablePin]~~
  outa[noparse][[/noparse]driveForwardPin]~~
  outa[noparse][[/noparse]driveReversePin]~~
  cognew(mx2125, @stack1)

  repeat
    debug.Str(String("xmG:"))
    debug.Dec(xmG)
    debug.tx(13) 

    pTime := xmG + 500
    cur_error := setPoint - pTime
    p := cur_error * Kp
    sum_error := cur_error + last_error + secondlast_error
    i := Ki * sum_error
    delta_error := cur_error - last_error
    d := Kd * delta_error
    drive := p + i + d

    debug.Str(String("p"))
    debug.Dec(p)
    debug.tx(13)
    debug.Str(String("i"))
    debug.Dec(i)
    debug.tx(13)
    debug.Str(String("d"))
    debug.Dec(d)
    debug.tx(13)

    debug.Str(String("drive:"))
    debug.Dec(drive)
    Motor(drive)
    secondlast_error := last_error
    last_error := cur_error
    debug.tx(13)

    waitcnt(clkfreq/50 + cnt)
 
PUB mx2125 | xRaw, _xRaw
  repeat    
    xRaw := PULSIN_Clk(0,1)
    _xRaw := xRaw/scale
    xmG := ((_xRaw /10) - 500) * 8

PRI PULSIN_Clk(Pin, State) : Duration
  DIRA[noparse][[/noparse]Pin]~
  ctra := 0
  ctra := (%11010 << 26 ) | (%001 << 23) | (0 << 9) | (PIN) ' set up counter, A level count
  frqa := 1
  waitpne(State << pin, |< Pin, 0)                         ' Wait for opposite state ready
  phsa:=0                                                  ' Clear count
  waitpeq(State << pin, |< Pin, 0)                         ' wait for pulse
  waitpne(State << pin, |< Pin, 0)                         ' Wait for pulse to end
  Duration := phsa                                         ' Return duration as counts
  ctra :=0                                                 ' stop counter

PUB Motor(drive)
  if drive > 0
    outa[noparse][[/noparse]DriveForwardPin]:= 0   ' L-active
    outa[noparse][[/noparse]DriveReversePin]:= 1   ' L-active
    debug.Str(String("forward:")) 
    Pulsout(driveEnablePin, drive+500)
    debug.Dec(drive+500)
    debug.tx(13)
  elseif drive < 0
    outa[noparse][[/noparse]DriveForwardPin]:= 1   ' L-active
    outa[noparse][[/noparse]DriveReversePin]:= 0   ' L-active
    debug.Str(String("reverse:"))     
    Pulsout(driveEnablePin, ||drive+500)
    debug.Dec(||drive+500)
    debug.tx(13)
  else
    outa[noparse][[/noparse]DriveForwardPin]:= 1   ' L-active
    outa[noparse][[/noparse]DriveReversePin]:= 1   ' L-active
    debug.Str(String("stop:")) 
    debug.tx(13)

PRI Pulsout(Pin, Duration)
  debug.Str(String("pulse:"))
  !outa[noparse][[/noparse]Pin]
  waitcnt(clkfreq/100000*Duration + cnt)
  !outa[noparse][[/noparse]Pin]

SRLM · 2009-11-21 18:26

You can't use just an accelerometer to determine tilt in a moving body. You need to add a gyro and a bunch of math to your setup. If you search around some you can find hundreds of threads and webpages that discuss the whole process.

caskaz · 2009-11-21 23:27

Why cannot accelerometer(MX2125) to determine tilt use?

Alsowolfman · 2009-11-21 23:51

An accelerometer can determines direction from acceleration. if it is accelerating in addition to gravity you cannot separate the two. if you attach a string to a weight you can determine which direction is down in the same way an accelerometer does, and if you move the top of the string around it will not give the the correct direction.

SRLM · 2009-11-22 02:24

See the link below. Even though it is targeted at a UAV audience, the same principles apply.

diydrones.com/profiles/blog/show?id=705844:BlogPost:41145

caskaz · 2009-11-22 03:22

I'm confused.
There is how to measure of InclinationSensing in application-note(www.memsic.com/data/pdfs/an-00mx-007.pdf) of MEMSIC.

But InclinationSensing for BalancingRobot cannot measure?

SRLM · 2009-11-22 05:14

From the first paragraph of the appnote linked (emphasis mine):

http://www.memsic.com/data/pdfs/an-00mx-007.pdf said...
A special case of constant acceleration is the force of gravity. When the accelerometer is stationary (i.e. no lateral or vertical accelerations are present) the only force acting on the accelerometer is gravity. The angle between the (vertical) gravitational force, and an accelerometer sensing axis is the inclination angle.

In short, if your robot isn't moving then you can use the accelerometer to measure angle. You may be able to pull some fancy tricks that take a sample at a stationary moment, but that would probably be very difficult to implement.

caskaz · 2009-11-22 06:43

I am still confused.

There is document of Pallax.
http://forums.parallax.com/attachment.php?attachmentid=40342
Above, there is application for monitoring angle for moving object.

Self balancing robots

I found out this pdf.
www.parallax.com/Portals/0/Downloads/docs/article/ROBOT-Fall06-BalancingBot.pdf
MX2125 use as sensor of BalancingRobot.
Maybe, I have mistake program or hardware or both.

Post Edited (caskaz) : 11/22/2009 7:07:58 AM GMT

mctrivia · 2009-11-22 06:54

if you know the radius of the arc that it can fall in you could compute the current angle based on the centripital force on the acceleromiter. the angle read by the acceleromiter would not be the actual angle.

the angle do to centripital force would be 90 degrees out of phase with its actual angle and the percentage of effect would be greater the farther down it is. Not the simplest math but doable.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
24 bit LCD Breakout Board coming soon. $21.99 has backlight driver and touch sensitive decoder.

Alsowolfman · 2009-11-22 08:48

i'm sorry,I think there was confusion for what you wanted to do, from that pdf it shows that you can make a robot that balances using just an accelerometer's inputs, but it does not work as well if you want the robot to be able to move around. Could you describe your troubles more? does it just fall does, or does it do a couple of corrections and then eventual fall?

also can you describe your setup some? i think you want the accelerometer as close to the center of rotation as possible.

Post Edited (Alsowolfman) : 11/22/2009 9:10:44 AM GMT

caskaz · 2009-11-22 09:29

SEtup
1.Kp=1 Ki=Kd=0
2.When robot lean to left, it move to left to fall down. When robot lean to right, it move to right to fall down. Increasing Kp step by one,
But Same result
3.When I place hands at both side of robot not to fall down, robot repeat to lean from left to right and from right to left.

I tried various Ki and Kd. But Same result

I cut off motor-power, and I checked value of xmG on Serial Terminal.
Kp=2 Ki=Kd=0
0degree xmG = 0 drive = 0
30degree xmG = 500 drive = -1000
-30dgree xmg = -500 drive = 1000

caskaz · 2009-11-22 10:21

I got conclusion that BalancingRobot of single-motor should has measurement for rotation speed.
Because rotation speed of wheel is max at passing 0degree, signal from sensor is not enough to move it to reverse maybe.

I go to purchase rotary-encoder tomorrow. and try it.

Thanks.

BalancingRobot

Comments