Go Down

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

#### Villa

#180
##### Jan 02, 2011, 04:59 am
Quote
It does not must be torgue, but in this case it is. At output you got torgue and you interpret it like speed. Look at my algorithm, and dont use motor speed pid.

Yeah, I understand what you mean, but my question is on what basis/theory did you make that conclusion? Could you please give me a more thorough explanation?

Another thing I wonder is about the sample time of the PID loops? How fast the driving loop, the balance loop and the heading loop should be?

Thanks,

Villa

#### kas

#181
##### Jan 02, 2011, 11:05 amLast Edit: Jan 02, 2011, 11:10 am by kas Reason: 1
Hi SnakeLT, thanks for joining the group
I understand you are from Lituania (cold is it??)
As for most of us here, English is not your native language.

Your bot is really cute and agile, the frame is... a CD box
Which development platform are you using ??

You do not use accelerators and the reason you gave...
Quote
in long term equilibrium angle is always 0deg, so you only need to know short term angle. So in long term angle, program artifically slowly averaging angle to 0deg.
...is pretty smart 8-)

Also I don't see motor velocity in your diagram. Is it computed in the "Driving PID" bloc ??
Please let us have additional info on your control strategy, together with source code or pseudo code

I now realize that, as Villa mentioned earlier, a single PID loop will never produce a completly still robot
Also, a single encoder is not definitly enough to keep a fixed heading

#### kas

#182
##### Jan 02, 2011, 01:37 pm
@niebing1987

You can definitly aquire angle with only one ACC
The angle is small and some folks assume sin(x) = x   (in radians)
Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ??

Code: [Select]
`      Acc = MeasureAcc() + ACC_OFFSET;      MeaAngle = asin((FP32)Acc/GRAVITY_G);      //Measure angle by the accelerometer`Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? (  (ADC value "upward" - ADC value "downward")/2  )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?

Code: [Select]
`      ADData = ReadTLC4541();  //measure the anglespeed by the gyro 5000.0*(ADData-RefAD)/65536.0/0.67/K;      MeaAngleDot = 0.001995*(RefAD-ADData) / K;`You use a 16 bit serial ADC to access the Murata gyro
Try changing the sign of MeaAngleDot

With so few information, those are only shots in the dark
What microcontroler are you using ? is it fast enough ?

Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)
see danielaaroe post #157

Good luck in your quest  , keep us aware

#### SnakeLT

#183
##### Jan 02, 2011, 02:58 pm
Villa, I'm not physicist and can't explain it.  In balancing robot case torgue is something like robot heigth*gravity*sin(angle), balance pid directly deals with angle. In my robot all PID's run at 100Hz.

kas, yes I'm from Lithuania, not so cold -4degC, but alot of snow.
I'm using GCC for avr, main procesor xmega. Yes speed derived from position (D term).

Angle calculation:
Code: [Select]
`volatile float gOffset = 0;float KOF = 0.004; //0.005volatile float G_Angle = 0;float TimeInterval = 0.01;void GetGyroData(int GyroRaw){  float GyroSpeed;  gOffset = KOF * GyroRaw + (1-KOF) * gOffset;  GyroSpeed = GyroRaw - gOffset;  G_Angle += gyroSpeed*TimeInterval;}`

#### niebing1987

#184
##### Jan 02, 2011, 03:08 pm
Hi,kas
Quote
Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ??

When I firstly read the details about the robot, they just used one axis and asin, so i  .....but i will make the change.
Quote
Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? (  (ADC value "upward" - ADC value "downward")/2  )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?

yes,ACC_OFFSET is the sensorZero
however, GRAVITY_G is not the gravity offset, it is the ADC value when the axis is paralleled to the gravity.
Quote
What microcontroler are you using ? is it fast enough ?
Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)

my microcontroller is LPC1114, 32bit, colock 48MHz, it should be fast enough.
For my poor english, i do not understand clearly ACC and Gyro is in phase, is it right like this?

#### richiereynolds

#185
##### Jan 02, 2011, 07:34 pmLast Edit: Jan 02, 2011, 07:34 pm by richiereynolds Reason: 1
Hopefully a quick one folks, in the v2 code for the serial GUI it has this function:

Code: [Select]
`int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)  return int((sensorValue[GYR_Y] * 0.88888888889)*-1);}`

Where does the 0.88888888889 constant come from? it doesn't seem to match any ref voltage etc.

Thanks!

#### Patrik

#186
##### Jan 02, 2011, 09:49 pm
@ richiereynolds

I'm using a nother GYR called LPR510AL and that one has a sensitivity of 10 mV/(°/s) at ±100°/s.

It looks like I have made a miss calculation where it should have been:

// ARef=3.3V, Gyro sensitivity=10mV/(deg/sec)
// in quid/sec:(1024/360)/1024 * 3.3/0.010)
return int((sensorValue[GYR_Y] * 0,9166679)*-1);

Measurement range:       ±100°/s and ±400°/s
Sensitivity:       10 mV/(°/s) and 2.5 mV/(°/s)

That could explain some errors I been having..

Sorry

I have rebuild my robot lighter but I haven't fixed the electronics on It yet hopefully I will get It done next week.

I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way.

Like it is now it will not be able to send it and still be able to keep the fixed loop time at 10ms. I solve it by only sending data to the GUI each 5 loops..

The balancing robot for dummies guide
http://www.x-firm.com/?page_id=145

#### Patrik

#187
##### Jan 02, 2011, 10:17 pm
The functions looks like this to day:
Code: [Select]
`int skipOut=0;void serialOut_GUI() {      if(skipOut++>=updateRate) {                                                            skipOut = 0;    //Filtered and unfiltered angle    Serial.print(ACC_angle, DEC);  Serial.print(",");    Serial.print(actAngle, DEC);   Serial.print(",");        //Raw sensor data    Serial.print(sensorValue[ACC_X], DEC);   Serial.print(",");    Serial.print(sensorValue[ACC_Z], DEC);   Serial.print(",");    Serial.print(sensorValue[GYR_Y], DEC);   Serial.print(",");      Serial.print(pTerm, DEC);      Serial.print(",");    Serial.print(iTerm, DEC);      Serial.print(",");    Serial.print(dTerm, DEC);      Serial.print(",");    Serial.print(drive, DEC);      Serial.print(",");    Serial.print(error, DEC);      Serial.print(",");    Serial.print(setPoint, DEC);   Serial.print(",");        //PID Parameters    Serial.print(K, DEC);    Serial.print(",");    Serial.print(Kp, DEC);   Serial.print(",");    Serial.print(Ki, DEC);   Serial.print(",");    Serial.print(Kd, DEC);   Serial.print(",");        //loop    Serial.print(STD_LOOP_TIME, DEC);        Serial.print(",");    Serial.print(lastLoopUsefulTime, DEC);   Serial.print(",");    Serial.print(lastLoopTime, DEC);         Serial.print(",");    Serial.print(updateRate, DEC);           Serial.print(",");        Serial.print(motorOffsetL, DEC);         Serial.print(",");    Serial.print(motorOffsetR, DEC);         Serial.print(",");          Serial.print(pTerm_Wheel, DEC);     Serial.print(",");    Serial.print(dTerm_Wheel, DEC);     Serial.print(",");    Serial.print(Kp_Wheel, DEC);        Serial.print(",");      Serial.print(Kd_Wheel, DEC);        Serial.print(",");        Serial.print("\n");  }}union u_tag {  byte b[4];  float fval;} u;int skipIn=0;void serialIn_GUI(){    if(skipIn++>=updateRate) {                                                            skipIn = 0;    byte id;        if(Serial.available() > 0)    {                    char param = Serial.read();      //Serial.println("Have bytes");      delay(10);      byte inByte[Serial.available()];      if(Serial.read() == SPLIT){        if(Serial.available() >= 4){          u.b[3] = Serial.read();          u.b[2] = Serial.read();          u.b[1] = Serial.read();          u.b[0] = Serial.read();          Serial.flush();                    switch (param) {            case 'p':              Kp = int(u.fval);              break;            case 'i':              Ki = int(u.fval);              break;            case 'd':              Kd = int(u.fval);              break;            case 'k':              K = u.fval;              break;            case 's':              setPoint = int(u.fval);              break;            case 'u':              updateRate = int(u.fval);              break;            case 'l':              motorOffsetL = u.fval;              break;            case 'r':              motorOffsetR = u.fval;              break;            case 'q':              Kp_Wheel = int(u.fval);              break;            case 'e':              serial_type = int(u.fval);              break;            case 'w':              Kd_Wheel = int(u.fval);          }        }      }          }  }}`

It's a realy long line that are beaning send to the GUI maybe it would be better to split it and sending it like bytes like I do when I send data to the Arduino..
The balancing robot for dummies guide
http://www.x-firm.com/?page_id=145

#### richiereynolds

#188
##### Jan 03, 2011, 12:04 am
Ah, excellent Patrik, thanks, that's that sorted.

My robot's actually working a bit better now, balancing, kind of ...

Going to work on the encoder parts now.
I also put a couple of "emergency wheels" on the bottom that swing down and are also supposed to hold it steady while it does zero calibration at the start, then swing up out of the way to let it balance. Only problem is, the servos only have the torque to hold the robot up when it's near vertical!

Oh well, back to the drawing board ...

#### richiereynolds

#189
##### Jan 03, 2011, 12:11 am
On the sending data to the gui question, would it be any quicker to put the strings into a buffer using sprintf and then send it in one Serial.println?
Other than that, I can only think of usng a fixed format that the GUI understands and sending bytes as you say.

#### Villa

#190
##### Jan 03, 2011, 05:07 am

@Patrik: I think that you shouldn't perform the data acquisition with a sample time of 10 (ms) in Windows (.NET), because Windows is not a real-time OS and 10 (ms) seems to be a little bit too fast.

@richiereynolds: When sending data to the GUI, You should encode your data using numbers in stead of characters. That way you will save a lot of bandwidth as well as processing time.

#### niebing1987

#191
##### Jan 03, 2011, 05:23 am
kas,thanks for your suggestion.it is the parmeter! After modifying them, the effect of kalmanfilter is much better.
Quote

#### kas

#192
##### Jan 03, 2011, 09:08 am
Quote
I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way

Hi Patrik
My first idea is to increase data speed transfer to 115200 bps
I went on your blog http://www.x-firm.com/?p=246
and checked BalancingBotGUI v1.2

- In BalancingBotGUI_v_1_2.pde,
Code: [Select]
`  Version log:    v1.2  ...        Changed the serial speed to 115200 kb/s`

- in serial_screen.pde,
Code: [Select]
`void setupSerial(){  ...  myPort = new Serial(this, portName, 19200);  ...}`
I am confused, are you still transmitting @19200 bps ??  :-?

#### Patrik

#193
##### Jan 03, 2011, 09:30 pm
@kas

I have bean running it on 19200bps I tried with 115200 but I went back to 19200 and just sending data each 5 loops..

Is there any downside with using 115200?

What I saw when sending data each loop the arduino could not hold the loop time any more... But when I send it each 5 loops it seams to be ok running around 7-8ms before delay,,,

But maybe I'm fueling my self and each 5 loops the fixed loop time is of and maybe that will give faults in the regulation of the robot..

I want to find a way to be able to send the amount of data I'm sending now in each loop with out going over 10ms.. But still maybe only send It each 5 loops because I don't think the GUI will be able to read it faster..

I have a idea of sending parts of the data under five loops and then start over like.
Code: [Select]
`switch (loop_count){  case 1:     //send PID values  case 2:     //send PID parameter values     //K value as float     //[id byte][split][value][value][value][value][CR]     //Kp value as int     //[id byte][split][value][value][CR]  case 3:     //send Sensor values     //send Angel values  case 4:     //send Torque values  case 5:     //send Settings values  defualt:    loop_count = 0;}loop_count++;`

What do you think about that? And in your opinion what baud rate should we use if possible?
The balancing robot for dummies guide
http://www.x-firm.com/?page_id=145

#### kas

#194
##### Jan 04, 2011, 10:53 amLast Edit: Jan 04, 2011, 10:55 am by kas Reason: 1
@niebing1987

Quote
For my poor english, i do not understand clearly ACC and Gyro is in phase,

My English is not good either
I mean that the Gyro signal (angle variation, deg/s) can be seen as the first derivative of the ACC signal (deg)
Gyro signal should be zero when ACC signal is horizontal (dX/dt=0)
On the first diagram the red curve cross the zero axis when the black curve reaches a max or a min.

This is also noticiable on the left side of your second diagram.

Kalman parameters are specific to sensors type (noise)
Now your filtered angle is much better, but too much filtered

@Patrik

Quote
What do you think about that? And in your opinion what baud rate should we use if possible?

I will experiment and be back by the end of the week

Go Up