Go Down

### Topic: Balancing robot for dummies (Read 75461 times)previous topic - next topic

#### batista1987

#60
##### Apr 14, 2012, 12:12 am
Hi guys. i've a new questions.1) when the wheel complete a fully rotation of 360  degree what is the value of count 486 or 1856? So what is the conversion between count (kas code) and degree position?
2) is the conversion between quid (kas code) and degree angle?

#### Lauszus

#61
##### Apr 14, 2012, 12:39 am
#batista1987
I depends on if you set up an interrupt on every edge.
We will use these motors as an example: http://www.pololu.com/catalog/product/1443
If you only have one interrupt, lets say on a raising edge, then you will get 464 counts per resolution if you trigger the interrupt on the same pin with every change you will get 464*2=928 counts per resolution.
If you also make an interrupt on a rising edge on the other input, you will get 464*3=1392 and if you do an interrupt on change on this input to you will get 464*4=1856 counts per resolution.

For my balancing robot I only created one interrupt on a rising edge (https://github.com/TKJElectronics/BalancingRobot/blob/master/Encoder.h#L8) equal to to 464 counts per resolution, as I decided that I didn't need any more resolution than that.

If you got 464 counts per resolution one counts is equal 360/464=0.775862069 degrees

See wiki for more details: http://en.wikipedia.org/wiki/Rotary_encoder#Incremental_rotary_encoder

The conversion between quids and angle depends on the sensitivity of your IMU. See my other post for more details: http://arduino.cc/forum/index.php/topic,58048.0.html

Regards
Lauszus

#### batista1987

#62
##### Apr 15, 2012, 11:51 amLast Edit: Apr 15, 2012, 12:24 pm by batista1987 Reason: 1
@Lauszus, thanks :-) you are very kind. So considering your thought, this code gives me 464 (i use dc motor  29:1 of pololu) pulses per revolution, or am I making a mistake?
Code: [Select]
`void setup(){.... attachInterrupt(1, rencoder, FALLING);....}void rencoder()  { if (PIND & 0b01000000)    count--;                      else                      count++;        }`

So i know that we talks about interrupt, but why we write this( if (PIND & 0b01000000) )?

#### MAAhelp

#63
##### Apr 15, 2012, 09:38 pm
@Lauszus: I saw your robot (on youtube), I would like to congratulate you on the awesome robot. I have also read the thread: Guide to gyro and accelerometer with Arduino including Kalman filtering, although not identical I though it could help me tackle any problems I might encounter. I'm using an Arduino Uno board and a tilt sensor. Although the robot balances well, it drifts (does not stay in a set position). May you elaborate how you managed to make the robot stay in the same position? As I have tried various methods but none of them seem to be working as I intended them to...
FYI: I am balancing the robot with a basic PD controller.

Thanks in advance for the help

#### Lauszus

#64
##### Apr 15, 2012, 11:21 pm

I got the same problem before I implemented the encoders. Have you any encoders? As I think they make it stay in the same position.

BTW:
I have now ported the code to Arduino.
The code can be found at github: The Arduino version, can now be found at github: https://github.com/TKJElectronics/BalancingRobotArduino

Regards
Lauszus

#### MAAhelp

#65
##### Apr 16, 2012, 11:13 am

I got the same problem before I implemented the encoders. Have you any encoders? As I think they make it stay in the same position.

BTW:
I have now ported the code to Arduino.
The code can be found at github: The Arduino version, can now be found at github: https://github.com/TKJElectronics/BalancingRobotArduino

Regards
Lauszus

Thanks for the quick reply and for the code in an arduino version! My main problem is understanding how you are implementing the encoders. Till now I have managed to read the encoder value via interrupts as you have done, what I am not sure is how I am going to use this information to keep the robot in the same position :S

MAAhelp

#### Lauszus

#66
##### Apr 16, 2012, 11:57 amLast Edit: Apr 16, 2012, 04:14 pm by Lauszus Reason: 1
#MAAhelp

I read the encoders in every 10th loop: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L85 this is equal to every 100ms - I then update the encoder position and velocity (velocity is just the difference between the last and current wheel position).

I then adjust the resting angle according to the position error and wheel velocity: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L116

I only look at the wheel velocity while it's moving, see https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L107 and https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L111

Also take a look at my reply to Kas at my blog (http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/#comment-20112):

"The restAngle is a constant (90 degrees), I only adjusted it before I implemented the encoders, as they keep adjusting the angle, so it remains in the same position.
The forward and backward commands including the targetOffset, are sent from the Arduino via serial. See this line: https://github.com/TKJElectronics/BalancingRobot/blob/master/BalancingRobot.cpp#L198.
But if you just set the offset to for instance 5 degrees, it will eventually fall over, so to avoid this, I adjust the offset depending on the wheel velocity. This can also so act as an break, as if the wheels are going forward and you set the backward command, the target offset is actually getting bigger than the values being sent.
The stop function is a bit different. At first there is three zones - this will slow it down again, when reaching the targetPosition. It adjust the restAngle, so it moves to the right position. At last I have also implemented the wheel velocity. It's main purpose is to keep it in the same spot. In short the positionError force it to go to a certain position and the wheelVelocity keeps it there.
At last I limit the restAngle angle, so it doesn't overreact when the error is great. Think of it as the constrain function for the Arduino.
The next step is just a normal PID controller. The pTerm looks at the error, if it's for instance 5 degrees it will multiply it by a constant, to make the error less. The problem is that this will make the robot start to oscillate - this is what dTerm prevents: it looks at the difference between the last error and the error. So if the last error were 5 degrees, but the new error is 3 degrees, the diference is -2, so it will actually slow the robot down, when it's reaching the correct angle. At last the iTerm will force it not to be satisfied until the error is zero - this will also helps the robot from drifting, as if the error is for instance just 0.5 degrees, it would start to drift, if the iTerm values wasn't there.
At last I split up the PIDValue, and set and offset to it, if the Arduino sends an turn or rotate command. It will keeps balancing if you just set the exact same offset to the difference motors, as I explained in the video."

Regards
Lauszus

#### Lauszus

#67
##### Apr 16, 2012, 12:16 pmLast Edit: Apr 18, 2012, 10:43 am by Lauszus Reason: 1
#batista1987
Yes that is correct.

The reason why you write: PIND & 0b01000000 is beacuse you want to read port D at bit 6 if you look at the port mapping (http://arduino.cc/en/Hacking/PinMapping168) you can see that PD6 is equal to pin 6 - but this is not always the case, if you look at the pin mapping for the ATmega2560 (http://arduino.cc/en/Hacking/PinMapping2560) then you can see that PD6 not connected to anything (NC).

I prefer it to do it like this, as it's much more readable: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L417

Where _BV() is a standard AVR definition:
#define _BV(bit) (1 << (bit))

See: http://www.nongnu.org/avr-libc/user-manual/group__avr__sfr.html

For more information about the reading and writing to the registers, have a look at the port manipulation page:
http://www.arduino.cc/en/Reference/PortManipulation

Regards
Lauszus

#### MAAhelp

#68
##### Apr 16, 2012, 01:09 pm
@Lauszus

Thanks for the detailed post! I will let you know how I get along. Really appreciate it hopefully solves the problem I was encountering

#### batista1987

#69
##### Apr 16, 2012, 06:53 pm
@Lauszus, thank you very much. I've a new questions. I've an Arduino 1, and i would to buy a usb shield to add on this shield a BT dongle to use this with a PS3 controller. I noticed that apart from the shield you can mount imu, motors and encoders on arduino1, in case the shield would I need a new arduino? thanks

#### Lauszus

#70
##### Apr 16, 2012, 06:59 pm
#batista1987
I'm not totally sure what you mean?
But yes use the USB Host shield (http://www.circuitsathome.com/products-page/arduino-shields/usb-host-shield-2-0-for-arduino) for Arduino Uno. All official Arduino are supported by the version 2 of the shield.

But the shield have to use two pins despite MISO, MOSI and SCK - 11,12, and 13 on a small Arduino (Uno, Duemilanove etc.). These are SS and INT. Normally they are located at pin 9 and 10, but can be rerouted to any I/O, see http://www.circuitsathome.com/usb-host-shield-hardware-manual at "Interface modifications".

Regards
Lauszus

#### batista1987

#71
##### Apr 17, 2012, 09:48 amLast Edit: Apr 17, 2012, 09:51 am by batista1987 Reason: 1
@Lauszus, ok so USBSHIED use pin 9 and 10 of digital out (is true?).
Excuse me but is the first approach to arduino, and i've a problem to understand, so i've a new questions. The pin thah you use (apart IMU) on (Arduino1 for example) are:
PDO=pin0 leftA (is INA left out of driver motor?)
PD1=pin1         (is INB left out of driver motor?)
PB1= A4 (is INA right out of driver motor?)
PC5=A5 (is INB right out of driver motor?)
pin2= leftEcoder1
pin4=leftEncoder2
pin3=rightEncoder1
pin5=rightEncoder2

So apart  the imu are these, right? Why you connect the right motor to a analog input?
I use a different imu  (http://www.sparkfun.com/products/10252) that use a i2c protocol, and this imu is connected to a4,a5. Can i  change
Code: [Select]
`#define rightA PINC4 // PC4 - pin A4#define rightB PINC5 // PC5 - pin A5`
to
Code: [Select]
`#define rightA PINC4 // PC3 - pin A3#define rightB PINC5 // PC2 - pin A2`
Regards
Batista1987

#### Lauszus

#72
##### Apr 17, 2012, 12:13 pm
#batista1987
Yes the shield uses pin 9 and 10 normall. But I had to use pin 9 and 10 for 20kHz pwm output, so I rerouted them to pin 7 and 8.
Here is a list of the pins I used:

pin 0 (PD0) = left motor logic A -  output
pin 1 (PD1) = left motor logic B -  output
pin 2 (PD2) = fist encoder input A -  input
pin 3 (PD3) = fist encoder input B -  input
pin 4 (PD4) = second encoder input A -  input
pin 5 (PD5) = second encoder input B -  input
pin 6 (PD6) = buffer (used to indicated when finished calibration) -  output
pin 7 (PD7) = INT of USB Host Shield -  input
pin 8 (PB0) = SS of USB Host Shield -  output
pin 9 (PB1) = left motor pwm (OC1A) -  output
pin 10 (PB2) = right motor pwm (OC1B) -  output
pin 11 (PB3) = MOSI for USB Host Shield -  output
pin 12 (PB4) = MISO for USB Host Shield -  input
pin 13 (PB5) = SCK for USB Host Shield -  output
pin 14 - A0 (PC0) = gyro y-axis -  input
pin 15 - A1 (PC1) = accelerometer x-axis -  input
pin 16 - A2 (PC2) = accelerometer y-axis -  input
pin 17 - A3 (PC3) = accelerometer z-axis -  input
pin 18 - A4 (PC4) = right motor logic A -  output
pin 19 - A5 (PC5) = right motor logic B -  output

As you can see the reason why I the analog pin for the motor is because I had used all other pins - remember that an analog pin can also be used as a general I/O.

You could actually save two pin, as your IMU uses I2C while mine is analog. If I had to order a new one, I would also go for a digital IMU to save more pins.

There is no difference in the below:
Code: [Select]
`#define rightA PINC4 // PC4 - pin A4#define rightB PINC5 // PC5 - pin A5toCode:#define rightA PINC4 // PC3 - pin A3#define rightB PINC5 // PC2 - pin A2`

You have only edited the comment NOT the pins definition. The right approach would be:

Code: [Select]
`#define rightA PINC3 // PC3 - pin A3#define rightB PINC2 // PC2 - pin A2`

Regards
Lauszus

#### aasanchez

#73
##### Apr 17, 2012, 11:41 pm
I'm trying to test the code of how to calculate the arctang that aparcere here  http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization and is used in this project, I'm testing with a calculator but I can not give a result not even like ... can someone please show me some input values ??and assume something that should out ..

#### rob4white

#74
##### Apr 18, 2012, 01:47 am
GYROSCOPE DRIFT...

Hi guys! I finally got my robot balancing... Far from perfectly but it stands up pretty well... Thanks to all for your help!

Just a query about the dreaded Gyroscope Drift!
Is it still an issue with modern gyroscopes?
I've mapped the output of the rate pin from my IMU on the serial monitor of the arduino program for over a 30 minute duration and I have failed to see any meaningful drift... The signal is noisy alright but there is no indication of a drifting from the set point while motionless!?

The IMU that I am using is a 3dof from Sparkfun and is now retired "IMU Combo Board - 3 Degrees of Freedom - ADXL203/ADXRS613"

the link is: http://www.sparkfun.com/products/9127

Any thoughts on this...?

Go Up