Author Topic: Moving balancing robot  (Read 7100 times)

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Moving balancing robot
« on: August 02, 2014, 10:25:52 PM »
Hi again :)

I'm trying to make a similar to Balanduino balancing robot, but with my own hardware and quite simpler software. My goal is to make a balancing robot which can be controlled remotely. But maybe I will start from the beginning to smoothly introduce to my issue.

My robot is pretty similar to Balanduino, it has smaller wheels, but I made it also lower to compensate it. Another difference is that I use DCs with gearbox 50:1, but these are the same Pololu motors with 64 CPR encoder. To measure the tilt I use MPU-6050 and to control everything-Arduino UNO. Motors are supplied from Lipo battery 14.8 V.

Earlier when I only tried to make my robot balance I created the algorythm of cascade PID control, where the outer loop was controlling the speed of wheels measured by encoders, and the inner loop was controlling the tilt angle. I used Lauszus KalmanFilter library with RestrictPitch variant which gives angles of range (-90 , 90) degrees where 0 is neutral position. It worked quite good. It was even turning back to the spot from which it was pushed. So I eliminated the drift of robot which was a nightmare when I didn't use encoders and controlled it in a single loop PID. Even small push caused drifting and then falling. So cascade solved this problem and I was very happy of the results. But I realized that I need also to move my robot. My first thought was to just give to loop which controls the speed, any value as a setpoint (normally the setpoint was 0). So the output of this loop which is the setpoint for angle loop should be a value which is not a 0 and it should cause a movement.

I haven't got a bluetooth module yet, so I decided to make a remote control using IR receiver and TV remote control.One button was changeing the speed setpoint to some value and another button was changeing back to 0. Indeed the robot was moving, but it was doing it with oscillations and simultanously accelerating till fall. Unfortunately it was another problem. When I was pressing stop button, it was coming back almost to the spot where it was before making a movement. I claimed that my cascade PID is not good for movement and I decided to use Balanduino algorythm.

I used a code from this site:
https://github.com/TKJElectronics/BalancingRobotArduino

I know it is the old version of Balanduino, but for me the code is more understandeble and I noticed it is very similar to Balanduino code.

So I combined this code with KalmanFilter code for MPU-6050 and deleted some lines for remote controlling and steering sideways because for now I just want to be able to steer forward and backward. I tuned PID parameters and changed the constats for encoders such as zone A, velocityScaleMove etc. As I have the gearbox 50:1 which is about 1.7 times more than 30:1 I multiplied all these values by 1.7. I also replaced the velocity 20 for 35 (20*1.75) in this place:

if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if braking
      targetPosition = wheelPosition;
      stopped = true;


When it stays still it works very similar to my earlier cascade PID. It comes back to spot when pushed and doesn't drift. It is visible in this video:
http://vimeo.com/102416130

But I cannot clearly understand the mothod of steering forward and backward and braking. I believe that originally there was a gamepad with something like joysticks and pushing it forward made calling funtion which was setting flag steerForward to true and steerStop to false. And it was also setting the offset which was calculated by scale() method.

In my conditions as long as I haven't got BT module I want to control it by my TV remote control. I tried to make something like this: when i push one button I set steerForward to true, and steerStop to false and sets offset to a particular value, for example 2. Another button sets steerForward to false, steerStop to true, offset to 0 and stopped to false which makes robot able to find new target position. Then I added also a button for steering backward, so I have three buttons. But in    fact movement looks almost like with using cascade PID. Steering forward and backward is proceeding with oscillations, and the braking also looks weird. Sometimes it stays in a place where it was braked, but sometimes it tries to come back to the spot before movement. Also when I tried to steer forward, after several oscillations robot starts to accelerate, so it is even harder for it to stop. It looks that: For example when I steer forward it tilts a little and starts to move, but after a while it slows down softly like it was trying to stop and then goes forward again but with greater speed. And such situation happens until it get so much speed that it falls down. It looks like a swing. All these weird things you can see in this vid:
https://vimeo.com/102416281
(in the end when it almost falls down I just pressed stop button, but robot instead of braking started to move with high speed in opposite direction. Everyting works very randomly and unstable).

What can be wrong in this project? To be honest I strongly rely on you Lauszus because you are the author of the robot which I try to recrate using your ideas. Maybe you had similar problems when you were doing this. I thing it is not about the struture of the code (I was deleting some lines very carefully). I rather think it is a matter of these parameters like zoneA, scaling etc. Also maybe I understand giving the offset in wrong way, because I was just setting it to a certain value like 2 or 3. Maybe it should be gradually incremented or something?

I realize that this post is very sophisticated but I wanted to explain my problem from the beginning to the end. I also could forget about something important, but if it's true I will fix it :)

PS
I've analysed the code of Balanduino one more time and I see it is almost the same from controlling point of view. I've noticed that the resolution of encoders was increased by quariture counting, and it influenced on all this constats by multiplying them by 2 as I did. So the only difference which I see is that I don't have scale function.... I am mainly interested how does this funtion work and what values does it return. I know that such good braking as Balanduino does require a great equilibrium of the platform and very accurate PID tuning and without it some oscillations may appear during braking and it is acceptable for me. The main problem is that the robot cannot steer with constant speed. When it goes with oscillations during steering it is also harder to stop him and propably that's why it behaves in such random and uncontrollable way..
« Last Edit: August 03, 2014, 01:49:28 AM by hubert9999 »

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Re: Moving balancing robot
« Reply #1 on: August 03, 2014, 10:30:43 PM »
Today I was trying to make it working with cascade PID again. I realized that these oscillations may be caused by encoders' feedback. When I set the setpoint of speed PID for a particular value it tries to achieve this value. It accelerates until real speed overshots the setpoint. Then it slows down until undershots the setpoint and it oscillates in such way forever. Another major problem is braking. In my cascade I just change the setpoint to 0, so robot tries to brake on the spot and it can make it very unstable. But in Balanduino code it should be avoided thanks to these zones A,B,C which are made exactly for slight braking. When I try to use Balanduino code, the situation is almost the same. There are problems with slight braking so it must be a matter of scaling constants for encoder. Am I right? I think that in Balanduino there is also implemented something very similar to cascade PID, because calculations from encoders are giving the leaning angle for PID. In cascade PID the output of speed loop is the setpoint for angle loop. So it looks very similar. The only difference is that in balanduino the position is also considered. In my system only velocity is being used during PID calculations. Theretically I believe my way by using cascade PID should also work when I could set proper values for both PID loops.

EDIT:
I succeded to make it work more smoothly. As you can see on vid:
http://vimeo.com/102575466

I just tuned PID parameters and observed how does robot behave during moving. I think I can make it work even much better but I have to spent much time to tune PID well. But I think I am on a good way :)
« Last Edit: August 05, 2014, 10:29:44 AM by hubert9999 »

Lauszus

  • Administrator
  • Full Member
  • *****
  • Posts: 132
    • View Profile
Re: Moving balancing robot
« Reply #2 on: August 05, 2014, 04:20:35 PM »
Hi,

First of all let me try to explain how the Balanduino works:

Essentially the way it is controlled is by setting an offset for the resting angle - see ster function: https://github.com/TKJElectronics/Balanduino/blob/a0a3e872acb763c2563a09e955d7c074e10f1610/Firmware/Balanduino/Bluetooth.ino#L342. For instance an offset of 5 degrees will make the robot tilt 5 degrees forward if standing still.

This value is decreased as the robots starts to travel forward: https://github.com/TKJElectronics/Balanduino/blob/a0a3e872acb763c2563a09e955d7c074e10f1610/Firmware/Balanduino/Motor.ino#L43-L47. This makes sure that the robot does not starts to travel too fast, so it falls over. This is because a offset angle will make it travel with a constant acceleration and we want a constant velocity, so I simply scale it in order to archive that.

Now when it receives a stop command the following code is run: https://github.com/TKJElectronics/Balanduino/blob/a0a3e872acb763c2563a09e955d7c074e10f1610/Firmware/Balanduino/Bluetooth.ino#L486-L493.

This stops the robot and set the new wheel position, but note that the "stopped" variable is set to false! This then checked at the following lines: https://github.com/TKJElectronics/Balanduino/blob/a0a3e872acb763c2563a09e955d7c074e10f1610/Firmware/Balanduino/Balanduino.ino#L349-L351.
This will ensure that it will brake more smoothly! As you can see it waits until the robot is nearly standing still - this will happen when it brakes and just before it starts moving backward. "targetPosition" is updated so it will actually stop at that position and NOT the position where you actually send the stop command.

Also the offset is a double and not an integer, this makes it travel much smoother. I originally just used an integer, but then you could clearly see the robot "stepping" between each offset.

Note that also the steering is also scaled depending on the speed of the robot: https://github.com/TKJElectronics/Balanduino/blob/a0a3e872acb763c2563a09e955d7c074e10f1610/Firmware/Balanduino/Motor.ino#L63-L72.

Your encoders resolution will be fine. It doesn't make much of a difference. Also your robots seems to be almost there ;) Maybe better motors would work. We are working on selling the motors used for the Balanduino separately for people just like you, but it might take a month or two!

Btw why not just stop the robot when you let go of the button? You could properly set a timer every time it receives a command and if it does not receive any command for some period it will stop the robot.

Regards
Lauszus

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Re: Moving balancing robot
« Reply #3 on: August 05, 2014, 06:28:22 PM »
I don't stop the robot when I release the button for going forward or backward because my TV remote control only sends the data on the rising edge, it is like just an impulse. When I hold the button data isn't sent so I decided to sent a stop command with a separate button. If I used a timer I couldn't stop it manually so it would be worse to test it.

I think I used your code properly. I know that when stopping, the new target position is set and to achieve that the "stopped" flag is being set to false. I nearly copied your code, just deleted some lines for serial communication etc because I don't use it.

But I've noticed that these oscillations during movement and smoothiness during braking are dependent on PID parameters. Eariler I didn't care specially about it and when still, it looked not bad but during movement it bahave like a swing. Now after I tuned it more carefully it is much better but still far away from perfect. I also noticed that robot behaves best when PID is tuned in such way that robot comes back to spot after distrubance smoothly and I comes directly to the spot without overshoting the place. As I saw on video, the Balanduino also does it in such way.
But I've spent many hours on tuning these parameters and I cannot make it work as I woud like to. When I try to tune PID without adjusting angle with encoders data, it is impossible to make it balance well. It always drifts when I push it, no matter how I tune PID. This what you can see on last video is the best result I could achieve.
« Last Edit: August 05, 2014, 07:14:14 PM by hubert9999 »

Lauszus

  • Administrator
  • Full Member
  • *****
  • Posts: 132
    • View Profile
Re: Moving balancing robot
« Reply #4 on: August 12, 2014, 06:51:14 PM »
Quote
I don't stop the robot when I release the button for going forward or backward because my TV remote control only sends the data on the rising edge, it is like just an impulse. When I hold the button data isn't sent so I decided to sent a stop command with a separate button. If I used a timer I couldn't stop it manually so it would be worse to test it.

Okay. That makes sense ;)

Quote
But I've spent many hours on tuning these parameters and I cannot make it work as I woud like to. When I try to tune PID without adjusting angle with encoders data, it is impossible to make it balance well. It always drifts when I push it, no matter how I tune PID. This what you can see on last video is the best result I could achieve.

You will NOT be able to make it balance in the same position when you push it without using encoders!

Here is a video of my first prototype: http://blog.tkjelectronics.dk/2011/12/sneak-peak-segway-guide-code/. It didn't use encoders and it was not able to keep balancing if you pushed it.

Regards
Lauszus

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Re: Moving balancing robot
« Reply #5 on: November 02, 2014, 02:08:37 AM »
I think I have figured out what can be a possible cause of problems with PID tuning in my case. I believe it can be a PWM frequency. I didn't set appropriate registers to get 20 kHz PWM. I was just using the analogWrite() function from Arduino, which gives (as I read on arduino.cc) about 1 kHz PWM. I've just studied how to set frequency manually by changing TCCR1A and TCCR1B and ICR registers. Comparing motors movement using 1 kHz PWM and 20 kHz I can notice that at 20 kHz motors work much smoother while changing direction rapidly with small percantage PWM. For 20 kHz motors start even while PWM is about 2%. For 1 kHz it must be higher than 5%. So I deduced that PWM frequency might be crucial for smooth and stable balancing, because robot can react even for very weak signals, which are inexecutable with 1 kHz.
I can't check it on robot now because at the moment it is disassebled. I'm just designing a PCB and I'm planning to continue my tasks in about three weeks when PCB will be printed. Then I will be able to test it and verify my ideas. For now could you tell me  if I'm right with my conclusion? Can PWM frequency affect on balancing quality?
« Last Edit: November 02, 2014, 02:11:34 AM by hubert9999 »

Lauszus

  • Administrator
  • Full Member
  • *****
  • Posts: 132
    • View Profile
Re: Moving balancing robot
« Reply #6 on: November 08, 2014, 08:58:48 PM »
Yes certainly. That's is why I increased the PWM frequency and also because it makes the robot more quite as 20kHz is out of the hearable range.

Regards
Lauszus

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Re: Moving balancing robot
« Reply #7 on: December 06, 2014, 12:10:40 AM »
I finally did it! I've got my new PCB and now I'm able to tune PID and control robot from my PC's keyboard. After hours of  tuning I achieved following result:
http://vimeo.com/113750618

It is on my code with two PID loops in cascade setup. I think it works quite well. Thank you very much for all your help. I woldn't do it withoult you Lauszus :)

Lauszus

  • Administrator
  • Full Member
  • *****
  • Posts: 132
    • View Profile
Re: Moving balancing robot
« Reply #8 on: December 06, 2014, 12:28:44 AM »
Looking good! :)

Can it handle slopes as well? And is your code available somewhere, so I can take a look?

Regards
Lauszus

hubert9999

  • Newbie
  • *
  • Posts: 9
    • View Profile
Re: Moving balancing robot
« Reply #9 on: December 06, 2014, 12:39:25 AM »
I haven't checked it yet. But of course I'll do it soon. I haven't posted my code because I writed it from a scratch today and I would like to update it so there wasn't such mess in it. I will do it in next few days and when I finish I will let you know :)

Lauszus

  • Administrator
  • Full Member
  • *****
  • Posts: 132
    • View Profile
Re: Moving balancing robot
« Reply #10 on: December 06, 2014, 01:22:59 AM »
Okay cool. Looking forward to see the code :)

Regards
Lauszus