loading

This is an improved version of my earlier instructable titled "A Simple and Very Easy Inverted Pendulum Balancing Robot". There are four advantages in this version.

  1. Easier to get a material
  2. Fewer materials needed
  3. Simpler program to balance itself
  4. Higher stability in balancing

I have published the original version in English and Japanese. I have gotten a considerable number of successful comments with videos (e.g. 1, 2, 3, 4, 5) to Japanese version but rare to English version. The first and the second points above are based on comments to the original version in English. These two improvements could eliminate a barrier from residents outside Japan. Under the introduction video below, some details for each these four points are described.

:

[Introduction Video]

:

(1) Digital Gyro Sensor
In the original version a gyro module with two analog output sensors (*1) is used. But outside Japan, it is not easy to get it or its equivalent product (*2). In this version a gyro module with a digital output sensor made by STMicroelectronics is used. It is easy to get it or its equivalent product either inside or outside Japan.

(*1) This analog output sensor has no constant value for the output at rest. Hence it is needed to measure the value of output in static state before balancing itself in the original version. But it is not needed in this version.

(*2) The gyro module used in the original version has angular acceleration contamination in angular velocity through its HPF. Therefore a fine soldering is required to cancel this contamination. However, it is not required in this improved version.

:

(2) Single Motor Driver IC

The inverted robot we would make here has two motors. In the original version two ICs drive them respectively. But in this version only an IC drives two motors.

:

(3) No Timer Library in Program

In the original version a timer library, Ms Timer2, is used in program for interrupt handling (*1). In this version no interrupt handling or no timer library is used in program (*2).

(*1) My original project was started with a numerical simulation on PC. The interrupt handling follows it.

(*2) See below.

:

(4) Noise Reduction Step in Program

In program, another revision is made also to reduce noise in output of gyro module (*2).

(*2) An alternative program with these revises was added to my original (earlier) instructable.

:

In following steps I will not avoid the same statements described in the original version to make this instractable a complete one.

* A Japanese version for this instructable is available.

Step 1: Gather Materials

Body (using TAMIYA's Educational Construction Series)

Electrics

:

[Tutorial for Step1]

:

[Commentary for Gyro Module]

Here a digital output gyro module is used instead of analog output one. This module is a L3GD20 carrier. The detail of L3GD20 is described subsequently. I have encountered with some troubles below while using it. So I recommend you to get 2 or 3 modules at once to isolate the problem.

:

(1) Inconsistent Sign of Gyro Output

I have tried two gyro modules with same model number alternatively to know individual difference of the module. Both of them are made by Akizuki-Denshi and they are L3GD20 carriers. I was surprised that signs of output of them are different around two axes. Therefore "R + ry" might have to be rewritten to "R - ry" in the line "DL29" in the program in Step 5.

:

(2) Abnormal Output of Gyro

A digital gyro module used here would begin to output strange data for some reason (*1). In that case, try to disconnect power supply to module or Arduino. If that fails, once upload a very simple program with the module to Arduino (*2), and upload the program for an inverted robot again.

(*1) I have seen non-zero value in a still state or constant value in motion, though I do not know the causes of them well.

(*2) The simple program is shown in the pdf file "dgtlGYRO_L3GD20_SPI_recover.pdf" attached to the end of Step 5.

:

(3) Equivalent or Alternative Gyro Module

In this instructable, a digital gyro module AE-L3GD20 made by Akizuki-Denshi is used as a carrier board for the L3GD20 three-axis gyroscope provided STMicroelectronics. It could be replaced by a near-equivalent module "item #: 2125" made by Pololu (*1), which is a carrier for the same gyroscope L3GD20. Outside Japan, it would be easier to get the latter. Pin mapping between each module and Arduino is shown below (*2).

  • (Function: Pin of Akizuki's, [Pin of Pololu's], {Pin of Arduino})
  • VIN: #1, [VIN], {3.3V}
  • GND: #8, [GND], {GND}
  • SPC: #2, [SCL], {D13}
  • SDI: #3, [SDA], {D11}
  • SDO: #4, [SDO], {D12}
  • CS: #5, [CS], {D10}

(*1) I have not gotten or tried the Pololu's module. I do not know well the attaching direction of L3GD20 chip to the board of the Pololu's module. In this instructable with Akizuki's module, the angle around Y-axis is measured to balance. But when the Pololu's is used, other axis (X or Z) might be measured or sign of output might be inverted. If such the differences are recognized, the program in Step 5 should be corrected.

(*2) Pololu's module has not only VDD pin but also VIN pin. It is described in its website, "Never supply voltage to VDD when VIN is connected, and never supply more than 3.6 V to VDD".

Step 2: Solder Wires to Some Components

First you should solder wires and pins to some electrical components.

  1. Solder wire to each point of 2 motors (4 points in all)
  2. Cut 4 jumper wires in half (8 half jumpers gotten)
  3. Solder these half jumpers to 8 wires of 2 motors and 2 battery snaps
  4. Cover each point soldered with chip of tape for insulation
  5. Solder header pins to an Akizuki's gyro module

:

[Tutorial for Step2]

Step 3: Assemble the Body of Robot

To build the body of inverted robot, TAMIYA's Educational Construction Series are used.

  1. Make two V-shaped cuts to Universal Plate
  2. Attach 4 L-angles included in Universal Arm Set to this Plate
  3. Assemble Double Gearbox to make its gear ratio 114.7:1
  4. Set 2 motors with 4 wires in this Gearbox
  5. Attach this Gearbox to Universal Plate above
  6. Assemble Slim Tire Set (two 55mm Dia. wheels gotten)
  7. Install these wheels to shafts of Gearbox above

:

[Tutorial for Step3]

Step 4: Put Electrical Components Together

Put electrical components together and attach them to the body built in the former step. Then a robot without program is gotten.

  1. Referring to Note 1 below, adjust two lines of pins of motor driver IC (L298N) respectively
  2. Referring to the picture of breadboard above, plug a gyro module and motor driver IC into your breadboard
  3. Wind some rubber bands around the breadboard and attach it on the lower L-angles of body of robot
  4. According to a wiring diagram above, plug jumper wires into the breadboard
  5. Also plug both wires of the battery snap into breadboad, which will be connected to a 4AA battery holder
  6. Wind a small rubber band around Arduino and mount it on the upper L-angles of the body of robot
  7. According to a wiring diagram above, plug jumper wires into the pinholes of Arduino
  8. Insert four AA batteries to 4AA battery holder
  9. Wind a bigger rubber band around the body of robot and fix the battery holder to the back side of robot

:

[Tutorial for Step4]

:

[Note 1]

L298N should be plug into breadboard straddling over its center ditch. But two lines of pins of this IC are not fitted for this plugging. So we make two adjustments for these pins.

  • Shorter pins: Shift the line of shorter pins to backside of IC by 2.5mm (making a wider distance to the line of longer pins)
  • Longer pins: Shift the line of longer pins to right-hand side of IC by a little bit

:

[Note 2]

L298N has two pins (#1 and #15) to measure current flowing through each motor respectively. Here no current is measured. So both of them are connected to GND.

:

[Note 3]

The robot gotten here has no power switch. To turn on/off power, battery snap is connected/disconnected.

Step 5: Upload the Sample Sketch to Arduino

Using IDE, the sample Sketch for inverted robot is uploaded to Arduino.

  1. Connect Arduino to PC via a USB cable
  2. Disconnect the battery snap from 4AA battery holder
  3. Lay down robot on desktop or floor and leave it at rest
  4. Download (*) the pdf file attached to the next to the last line in this step (* You get a corrupted program if you open this file in web browser and copy its content)
  5. Open the file in proper reader and copy the whole of text in the file
  6. Paste the copy to IDE and correct misprints
  7. Upload corrected sketch to Arduino using IDE

:

[note 4]

To make an inverted pendulum balance itself, motor torque is controlled dynamically. This control method is expressed as a plane formula with 4 variables related to the state of pendulum.

"motor torque = k1 * angle of pendulum
+ k2 * angular velocity of pendulum
+ k3 * velocity of the lower end of pendulum
+ k4 * displacement of the lower end of pendulum from a reference point"

k1, k2, k3 and k4 are static coefficients with appropriate values. In a sample sketch in pdf file below, a long equation at line 72 expresses this formula. To make inverted robot balance itself, we have to select proper values for these 4 coefficients respectively and know right value of each of these 4 variables in real time.

The gyro sensor gives the second variable value, angular velocity of pendulum, dynamically by a 16-bit integer. And Arduino estimates the first variable value, angle of pendulum, contemporaneously by integrating the second variable.

On the other hand, the third or the fourth variable value cannot be measured by gyro sensor. So another kind of sensor or meter should be used to get them in real time. But our inverted robot has no more device than a gyro module. Thus it becomes a puzzle to estimate these 2 variables values dynamically. In a sample sketch in pdf file below, 2 masked equations at lines 76 and 77 should be used to estimate them. Here they are left masked not to spoil solving this puzzle.

:

[Note 5]

L3GD20 digital gyro sensor has three different measurement ranges; +/- 250dps, +/- 500dps and +/- 2000dps (dps: Degree per Second). This sensor outputs angular velocity as a 16bit integer. In this instructable the minimum range (+/- 250dps) is adopted, where a digit of output means 0.00875dps.

:

[Note 6]

The output of the gyro sensor is transmitted to Arduino by SPI interface. General technical discussion about SPI or wiring for it is not described here. If the gyro module made by Pololu were used instead of Akizuki’s one, see "(3) Equivalent or Alternative Gyro Module" [in Commentary for Gyro Module] in Step 1. I have not tried Pololu’s module. In the sample program attached to the end of this step, the angle around Y-axis is measured to balance. But when the Pololu's were used, other axis (X or Z) might be measured or sign of output might be inverted. For example, if the posture of robot were detected around X-axis of L3GD20 and sign of output is inverted, the right hand of line DL28 in the sample program should be rewritten “- ( (L3GD20_read(0x29) << 8) | L3GD20_read(0x28) )”.

:

[The Sample sketch (program) for inverted robot]

Copy whole text of "invertedRobot_v20d_noTimer.pdf" below, paste it to IDE and upload it to Arduino (**1-5).

(*1) Save the pdf file in your PC and open it in proper pdf-reader. Or you get a corrupted program by opening it in web-browser.

(*2) If "; //72" is not shown at the end of line 72, substitute whole a equation below for it. It is not the same as the original version.

powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 ); //72

(*3) Pasting the copy to IDE or editor, misprints such as "/ /" as "//" (an obstacle space inserted) might happen, then correct them.

(*4) The line number written in the sample sketch as comment (e.g. //DL2) is consistent with the earlier sketch ”invertedRobot.pdf” or ”invertedRobot_v20_noTimer.pdf” in the original version.

(*5) Copyright (C) 2015 ArduinoDeXXX All Rights Reserved.

:

[The sketch to restore L3GD20 carrier gyro module]

If the output of gyro module becomes strange, see "(2) Abnormal Output of Gyro" [in Commentary for Gyro Module] in Step 1. There two countermeasures are shown. The sketch used in the second measure, “dgtlGYRO_L3GD20_SPI_recover.pdf,” is attached to the last line of this step.

Step 6: Get an Inverted Following Robot

When the sample sketch uploaded to Arduino in the former step, you ought to get an inverted following robot. Though it needs your finger for standing, it follows your finger.

  1. After the upload finished, hold the body of your robot and pick it up (see picture 2)
  2. Connect battery snap to battery holder attached to the back side of robot
  3. Wheels of robot start spinning, hold both wheels tight and stop spinning (see picture 3)
  4. Set both wheels on desktop or floor and keep static standing to be made center of gravity of robot to be located on its wheel shafts
  5. After a few second, motor torque becomes down and faint noise or chirping "miii" is listened
  6. Then release your hands from robot gently (see picture 4) and put your finger on the top of the body of robot softly
  7. Move your finger forward and backward and see your robot following
  8. Release your finger and see your robot trying to balance but failing

:

[Tutorial for Step5 and 6]

:

[Note 7]

As turning on or resetting Arduino, the robot perceives its posture (angle of its body) and spins its wheels when the posture is changed. Then the robot has to study the proper posture in well inverted state. This robot is programmed that if it has been in static state for 0.05 seconds it deems itself well inverted.

Hence as its wheels starting spinning, you should hold both wheels tight and stop spinning. And execute line 4 to 8 above.

Further if you do not feel changes at line 5, torque down and faint noise of motors, for 10 seconds waiting, your gyro module might output wrong value temporarily or it might be inconsistent with the sample sketch in the former step.

In that case, you should see "(2) Abnormal Output of Gyro" [in Commentary for Gyro Module] in Step 1 first. And if you use some L3GD20 carrier other than Akizuki’s module, you should see "(3) Equivalent or Alternative Gyro Module" in Step 1 and Note 6 in Step 5 also.

:

[Troubleshooting 1]

If wheels of robot do not start spinning at line 3 above, some faulty wiring must be suspected first. Go back Step 4 and verify wiring carefully. Another doubt is a defective gyro module. If you suspect it, you should see "Commentary for Gyro Module" in Step 1.

On the other hand, if your robot does not follow your finger in spite of wheels starting spinning at line 3, three kinds of faulty wiring must be suspected: (1) Wires of a motor are set opposite, (2) Gyro module is set reversed, (3) Wiring to digital pins of Arduino is wrong. Go back Step 4 and verify wiring carefully.

Further if your gyro module outputs inconsistent value with the sample sketch in Step 5, your robot would not follow your finger despite correct wiring. If you suspect it, see "(1) Inconsistent Sign of Gyro Output" in Step 1. And if you use some L3GD20 carrier other than Akizuki’s module, you should see "(3) Equivalent or Alternative Gyro Module" in Step 1 and Note 6 in Step 5 also.

:

[Reference] Checking an Inverted Following Robot

Step 7: Challenge Yourself to Solve a Puzzle

Let's make your "following inversed robot" balance itself without your finger by modifying the sample sketch in Step 5. It might be a puzzle.

  1. Consider why it fails balancing
  2. Get plausible idea and modify the sample sketch
  3. Make your inverted robot try balancing
  4. If it balances itself you have solved a puzzle and your robot has become an inverted pendulum (Congratulation!)
  5. Or else turn back to line 1 or hesitate to see a solution in Step 8

Step 8: How to Get an Inverted Balancing Robot

Here you see how to get an inverted balancing robot. You need to modify only two lines in the sample sketch in Step 5.

  • Copy 3 lines below and paste them over lines 76 and 77 in the sample sketch (overwriting them)

vE5 = sumPower; //76a
xE5 = sumSumP / 1000; //77a
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

  • Execute Step 6 (without a finger support in lines 6 to 8)
  • If center of gravity of robot is located on near its wheel shafts, your robot moves back and forward for a few seconds and gets well inverted posture
  • See it keeps balancing itself
  • If it moves with forward or backward bias, add to or submit from proper integer after "power" at the end of line 74 in the original sample sketch

:

[Tutorial for Step 8]

:

[Troubleshooting 2]

If your robot can not balance itself well in spite of executing Step 6 well, “low battery” in the battery holder on the back of robot should be suspected first. If it is not observed, constant values at lines 10 to 13 in the sample sketch should be changed. I think that line 12 is most critical.

Step 9: Operate Inverted Robot Via Serial Monitor

Now you have an inverted balancing robot. Then let’s control it by serial monitor. You could make it turn right/left and go forward/back.

  1. Update the modified sample program gotten in Step 8 according to the details below
  2. Upload the updated one to Arduino using IDE
  3. After the upload end, start serial monitor from IDE
  4. Select “Newline” and “115200 baud” at bottom right of serial monitor
  5. Make inverted robot balance itself according to Step 6 (without a finger support in lines 6 to 8)
  6. Input numerical values in upper left window of serial monitor and push the [Enter] key
  7. See action or change of action of your robot

:

[Tutorial for Step 9]

:

[Note 8]

You can control your robot dynamically by serial monitor from PC via USB cable. For example you input "00" in window of serial monitor and push the [Enter] key, then robot receive ‘0’ and ‘0’ separately. Hence it turns right at a regular angle below twice without stopping.

  • A ‘0’ sent makes robot turn right at a regular angle on the spot
  • A ‘1’ sent makes robot turn left at a regular angle on the spot
  • A number (n) of ‘2’ sent make robot accelerate forward in proportion to the number (n)
  • A number (n) of ‘3’ sent make robot accelerate backward in proportion to the number (n)

:

[Update on the sample sketch (for 4 places)]

(1) Copy the following 5 lines and paste them between lines 17 and 18 in the modified sketch gotten in Step 8.

int drct = 0;
boolean right = false;
boolean left = false;
int fwdBck = 0;
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

(2) Copy the following 26 lines and paste them over line 32 in the modified sketch gotten in Step 8, viz. substitute these lines for “if ( power > 0 ) {”.

if ( Serial.available() ) {
drct = Serial.read();
Serial.println(drct);
}
if( drct == 48 ) { right = true; }
else if ( drct == 49 ) { left = true; }
else if (drct == 50 ) { fwdBck++; drct = 0; }
else if (drct == 51 ) { fwdBck--; drct = 0; }
if ( right == true ) {
analogWrite( 6, 140 );
digitalWrite( 4, HIGH );
digitalWrite( 5, LOW );
analogWrite( 9, 140 );
digitalWrite( 7, LOW );
digitalWrite( 8, HIGH );
delay(40);
} else if ( left == true ) {
analogWrite( 6, 140 );
digitalWrite( 4, LOW );
digitalWrite( 5, HIGH );
analogWrite( 9, 140 );
digitalWrite( 7, HIGH );
digitalWrite( 8, LOW );
delay(40);
} else if ( power > 0 ) {
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

(3) Copy the following 4 lines and paste them between lines 55 and DL26 in the modified sketch gotten in Step 8.

drct = 0;
right = false;
left = false;
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

(4) Copy the following 2 lines and paste them over line 74 in the modified sketch gotten in Step 8, viz. substitute these lines for “sumPower = sumPower + power;”.

sumPower = sumPower + power + fwdBck * 4; // 74a
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.

Step 10: Unplug the Cable

Substitute a battery (9V) for USB power (5V) for power supply to Arduino. Then Arduino is unplugged and your robot stands with no outer wire.

  1. Eject a USB cable from Arduino
  2. Connect the second battery snap to a battery (9V)
  3. Attach the battery to the back of robot by rubber band with a battery holder
  4. Plug the positive wire of this snap to Vin pin of Arduino
  5. Plug the negative wire of this snap to GND pin of Arduino
  6. Your robot starts spinning its wheels if the first snap is connected to battery holder
  7. Make your robot balance itself according to Step 6 (without a finger support in lines 6 to 8)
  8. See it stands with no outer wire as a completed inverted pendulum

:

[Tutorial for Step 10]

Step 11: Develop Robot Further

Let’s make your inverted robot or your knowledge evolve further. Here three ways are shown briefly. It would be hard to finish them within half a day.

:

(1) Remote Operation with No Wire

In Step 9 your robot is operated via USB cable. Now your robot is unplugged. So let’s operate it with no wire. In terms of simplicity, an infrared module would be the first choice.

:

[Demonstration of Operating Robot by a TV Remote]

:

(2) Simulating the Action of an Inverted Robot with Excel

Our inverted robot could be thought constructed from three parts: head, body and wheels. In this simplifying, an equation of motion of the robot can be written with a physics learned at High School. As getting it, the action of robot can be simulated using Excel. The near-proper values of two coefficients in Note 4 in Step 5, k1 and k2, could be gotten by this simulation.

:

(3) Capturing and Analyzing the Real Action of Inverted Robot

We can capture the state variables – posture and motion – of our robot in balancing. And it is possible to duplicate it on PC monitor in real time as a virtual 2D animation of balancing robot. Moreover, we can examine the effect of the sketch in detail by analyzing the time series of recorded state variables. An instructable in Japanese has been published, in which these capturing, duplicating and analyzing are described.

:

[Real Time 2D Animation by State Capturing]

<p>why did you put velocity and position equal to zero? how did you set up the K values? I am doing an inverted pendulum with another board (not arduino) and I have difficult to convert the power value in pwm could you help me to do that? I am using the LQR to get the K and I can mesure the position and the angle x=[position velocity angle velAng]; u=-K*x how can I convert the u in pwm I follow your guide but I need a procedure because we have a different hardware and with your value of couse does not work</p>
<p>&gt; Why did you put velocity and position equal to zero?</p><p>In Step5 and 6, we can get an &ldquo;<em><strong>inverted following robot.</strong></em>&rdquo; which cannot balance itself. There we put the estimates of velocity and position of the robot equal to zero. On the other hand, we try to get &ldquo;<strong><em>inverted self-balancing robot</em></strong>&rdquo; in Step 7 and 8, where we should estimate the velocity and position of the robot correctly.<br><br>&gt; How did you set up the K values?</p><p>The original project of this article was started with a numerical simulation on PC. See (*1) in (3) in Introduction in this article. The near-proper values of two coefficients in Note 4 in Step 5, k1 and k2, were gotten by this simulation. See (2) in Step11. The remaining coefficients, k3 and k4, could be gotten by trial and error.</p>
<p>im working from mexico and its very dificult to locate L3GD20 and im using MPU-5060 and i cant find where can i put the pin cs i be very thankfully if you can help me with that great project.</p>
<p>Hi, <br>I don&rsquo;t know MPU5060 well, sorry. Though SPI interface is used in the sample sketch in Step5, I2C interface should be used for MPU5060 or 6050. You can find <a href="http://playground.arduino.cc/Main/MPU-6050" rel="nofollow">good information</a> in other web sites with searching &ldquo;MPU5060&rdquo;.</p>
<p>Hello everybody, very good topic ! Thanks. I want to try this soon ...</p><p>I've question concerning the dual motor used for this robot, I've ordered the same one and before assembly I want to know the best speed for this purpose:</p><p>-Type A: 12.7:1 gear ratio - 94gf*cm 1039rpm rotations.</p><p>-Type B: 38.2:1 gear ratio - 278gf*cm 345rpm rotations.</p><p>-Type C: 114.7:1 gear ratio - 809gf*cm 115rpm rotations.</p><p>-Type D: 344.2:1 gear ratio - 2276gf*cm 38rpm rotations.</p><p>Thanks a lot.</p><p>Vincent.</p>
<p>Hi,<br>In this article &ldquo;Double Gearbox&rdquo; is assembled to make its gear ratio 114.7:1. See Step3. Though another ratio could be chosen, the values of parameter set should be corrected in the sample sketch attached in Step5 to get good robot in Step6 to 8.</p><p>In general, backlash in the gearbox would be smaller with higher ratio. And the robot with lower ratio would become more robust against shocks.</p>
Hi and thanks a lot ArduinoDeXXX, I'll set to this 114.7 ratio ;=))
<p>thanks for replying</p><p>I have uploaded your sketch wich the robot balances itself without using my finger. but the robot is nog working like it supposed to. at some moments the robot works only with my help but sometimes it doesn't work at all (when resetting again). I'm not entirely sure but i think my code is ok. i have read all your steps and followed all your instructions. maybe my gyroscope could sending out a wrong signal?</p><p>thanks again for helping me out</p><p> here is my code again:</p><p>byte countS = 0; </p><p>int recOmegaI[10]; </p><p>int omegaI = 0; </p><p>long thetaI = 0; </p><p>long sumPower = 0; </p><p>long sumSumP = 0; </p><p>const int kAngle = 50; </p><p>const int kOmega = 500; </p><p>const long kSpeed = 60; </p><p>const long kDistance = 20; </p><p>long powerScale; </p><p>int power; </p><p>long vE5 = 0; </p><p>long xE5 = 0; </p><p>#include &lt;SPI.h&gt; </p><p>int ry; </p><p>long R; </p><p>void L3GD20_write(byte reg, byte val) { </p><p> digitalWrite(10, LOW); </p><p> SPI.transfer(reg); </p><p> SPI.transfer(val); </p><p> digitalWrite(10, HIGH); </p><p>}</p><p>byte L3GD20_read(byte reg) { </p><p> byte ret = 0; </p><p> digitalWrite(10, LOW); </p><p> SPI.transfer(reg | 0x80); </p><p> ret = SPI.transfer(0); </p><p> digitalWrite(10, HIGH); </p><p> return ret; </p><p>} </p><p>void setup () { </p><p> Serial .begin(115200); </p><p> pinMode(4, OUTPUT); </p><p>pinMode(5, OUTPUT); </p><p> pinMode(6, OUTPUT); </p><p> pinMode(7, OUTPUT); </p><p> pinMode(8, OUTPUT); </p><p> pinMode(9, OUTPUT); </p><p> for ( int i = 0 ; i &lt; 10 ; i++ ) { recOmegaI[i] = 0; }</p><p> pinMode(10, OUTPUT); </p><p>digitalWrite(10, HIGH); </p><p> SPI.begin(); </p><p>SPI.setBitOrder(MSBFIRST); </p><p> SPI.setDataMode(SPI_MODE3); </p><p>SPI.setClockDivider(SPI_CLOCK_DIV2); </p><p> L3GD20_write(0x20, B11001111); </p><p> L3GD20_write(0x23, B00000000); </p><p> delay(300); </p><p> }</p><p> void loop () { </p><p> chkAndCtl(); </p><p>if ( power &gt; 0 ) { </p><p> analogWrite( 6, power ); </p><p> digitalWrite( 4, HIGH ); </p><p> digitalWrite( 5, LOW ); </p><p> analogWrite( 9, power ); </p><p> digitalWrite( 7, HIGH ); </p><p> digitalWrite( 8, LOW ); </p><p> } else { </p><p> analogWrite( 6, - power );</p><p> digitalWrite( 4, LOW ); </p><p> digitalWrite( 5, HIGH ); </p><p> analogWrite( 9, - power ); </p><p> digitalWrite( 7, LOW ); </p><p> digitalWrite( 8, HIGH ); </p><p> } </p><p>}</p><p>void chkAndCtl() {</p><p> R = 0; </p><p>for ( int i = 0 ; i &lt; 45 ; i++ ) { </p><p> ry = ( (L3GD20_read(0x2B) &lt;&lt; 8) | L3GD20_read(0x2A) ); </p><p> R = R + ry; </p><p> delayMicroseconds(90); </p><p> } </p><p> omegaI = R * 0.00875 / 45; </p><p> if ( abs( omegaI ) &lt; 2 ) { omegaI = 0; } </p><p> recOmegaI[0] = omegaI; thetaI = thetaI + omegaI; countS = 0; </p><p>for ( int i = 0 ; i &lt; 10 ; i++ ) { </p><p>if ( abs( recOmegaI[i] ) &lt; 4 ) { countS++; } </p><p>} if ( countS &gt; 9 ) { thetaI = 0; </p><p> vE5 = 0; </p><p> xE5 = 0; </p><p> sumPower = 0; </p><p> sumSumP = 0; </p><p> }</p><p>for ( int i = 9 ; i &gt; 0 ; i-- ) { recOmegaI[ i ] = recOmegaI[ i-1 ]; } </p><p>powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 );</p><p> power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 ); </p><p> sumPower = sumPower + power; </p><p> sumSumP = sumSumP + sumPower;</p><p>vE5 = sumPower;</p><p>xE5 = sumSumP / 1000;</p><p>}</p>
<p>It is hardly expected that the corrected sample sketch doesn&rsquo;t work well, even though it is uploaded well. It is just a &ldquo;copy and paste&rdquo; and easy trimming.</p><p>Some gyro modules have gone broken in my working. But I have never heard such module in comments of my readers. In the most cases of them, some faulty wiring made robot wrong.</p><p>1. Did you check your robot working as shown in the video &ldquo;[Reference] Checking an Inverted Following Robot&rdquo; in Step6?</p><p>2. Can you show or upload the video of your robot working in the check above?</p><p>See the earlier comments of Maeskaz below.</p>
<p>hi</p><p>I use an L3gd20 on my robot, but your code doesn't seem to work</p><p>I tried correcting to code many times but with no positive result.</p><p>What exactly is the theory of your robot? You explain what it does, but not how it exactly works. what kind of signals does the robot use? </p><p>Maybe you can take a look at my code too? I deleted comments and corrected errors. Now there are no errors with uploading but no good result. For a second the robot seems to balance itself but after the second it immediately fails balancing.</p><p>Here is my code:</p><p>byte countS = 0; </p><p>int recOmegaI[10]; </p><p>int omegaI = 0; </p><p>long thetaI = 0; </p><p>long sumPower = 0; </p><p>long sumSumP = 0; </p><p>const int kAngle = 50; </p><p>const int kOmega = 500; </p><p>const long kSpeed = 60; </p><p>const long kDistance = 20; </p><p>long powerScale; </p><p>int power; </p><p>long vE5 = 0; </p><p>long xE5 = 0; </p><p>#include &lt;SPI.h&gt; </p><p>int ry; </p><p>long R; </p><p>void L3GD20_write(byte reg, byte val) { </p><p> digitalWrite(10, LOW); </p><p> SPI.transfer(reg); </p><p> SPI.transfer(val); </p><p> digitalWrite(10, HIGH); </p><p>}</p><p>byte L3GD20_read(byte reg) { </p><p> byte ret = 0; </p><p> digitalWrite(10, LOW); </p><p> SPI.transfer(reg | 0x80); </p><p> ret = SPI.transfer(0); </p><p> digitalWrite(10, HIGH); </p><p> return ret; </p><p>} </p><p>void setup () { </p><p> Serial .begin(115200); </p><p> pinMode(4, OUTPUT); </p><p>pinMode(5, OUTPUT); </p><p> pinMode(6, OUTPUT); </p><p> pinMode(7, OUTPUT); </p><p> pinMode(8, OUTPUT); </p><p> pinMode(9, OUTPUT); </p><p> for ( int i = 0 ; i &lt; 10 ; i++ ) { recOmegaI[i] = 0; }</p><p> pinMode(10, OUTPUT); </p><p>digitalWrite(10, HIGH); </p><p> SPI.begin(); </p><p>SPI.setBitOrder(MSBFIRST); </p><p> SPI.setDataMode(SPI_MODE3); </p><p>SPI.setClockDivider(SPI_CLOCK_DIV2); </p><p> L3GD20_write(0x20, B11001111); </p><p> L3GD20_write(0x23, B00000000); </p><p> delay(300); </p><p> }</p><p> void loop () { </p><p> chkAndCtl(); </p><p>if ( power &gt; 0 ) { </p><p> analogWrite( 6, power ); </p><p> digitalWrite( 4, HIGH ); </p><p> digitalWrite( 5, LOW ); </p><p> analogWrite( 9, power ); </p><p> digitalWrite( 7, HIGH ); </p><p> digitalWrite( 8, LOW ); </p><p> } else { </p><p> analogWrite( 6, - power );</p><p> digitalWrite( 4, LOW ); </p><p> digitalWrite( 5, HIGH ); </p><p> analogWrite( 9, - power ); </p><p> digitalWrite( 7, LOW ); </p><p> digitalWrite( 8, HIGH ); </p><p> } </p><p>}</p><p>void chkAndCtl() {</p><p> R = 0; </p><p>for ( int i = 0 ; i &lt; 45 ; i++ ) { </p><p> ry = ( (L3GD20_read(0x2B) &lt;&lt; 8) | L3GD20_read(0x2A) ); </p><p> R = R + ry; </p><p> delayMicroseconds(90); </p><p> } </p><p> omegaI = R * 0.00875 / 45; </p><p> if ( abs( omegaI ) &lt; 2 ) { omegaI = 0; } </p><p> recOmegaI[0] = omegaI; thetaI = thetaI + omegaI; countS = 0; </p><p>for ( int i = 0 ; i &lt; 10 ; i++ ) { </p><p>if ( abs( recOmegaI[i] ) &lt; 4 ) { countS++; } </p><p>} if ( countS &gt; 9 ) { thetaI = 0; </p><p> vE5 = 0; </p><p> xE5 = 0; </p><p> sumPower = 0; </p><p> sumSumP = 0; </p><p> }</p><p>for ( int i = 9 ; i &gt; 0 ; i-- ) { recOmegaI[ i ] = recOmegaI[ i-1 ]; } </p><p> powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 ); </p><p> power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 ); </p><p> sumPower = sumPower + power; </p><p> sumSumP = sumSumP + sumPower;</p><p>}</p>
<p>Hi,<br>I read your sketch. I think you should read all steps in this article more carefully. Have you read Step6? Have you challenged the puzzle in Step7? And have you tried Step8?</p><p>In Step6, a <em><strong>following robot</strong></em> ought to be gotten, not a <em><strong>balancing robot</strong></em>.</p>
<p>The wiring on my robot is done right but I dont hear that &quot;miiiii&quot; sound also...</p><p>The problem could be my gyroscope. But it is never used for anything else than this project. And this project is the first project where I used a gyroscope. so it isn't damaged</p><p>Maybe something is wrong with your Arduino sketch? I opened your software the way it supposed to be opened (see previous comment) but my robot doesn't seem to be able to balance itself...</p>
<p>1. Did you check your robot working as shown in the video &ldquo;[Reference] Checking an Inverted Following Robot&rdquo; in Step6?<br><br>2. Can you show or upload the video of your robot working in the check above?<br><br>3. Can you tell me the model number of your gyro module (L3GD20 carrier)?</p>
<p>Thanks for your fast reply and your motivation to help me!</p><p>I've uploaded a video on youtube which shows how my robot works. I tried to test my robot as in your video in step 6 and later with help from my finger (see video). I've tried to modify and upload your sketch many times and this is the best result I have gotten. I will post my Arduino sketch below. Maybe you can notice some errors in it?</p><p>I've noticed something strange too. In your video both of your battery snaps are already connected with your robot (including batteries). But it only works when you plugged in your USB cable. Mine just works if I plug in my USB cable, but without batteries. If I plug the battery holders (including batteries) in, the motors seem to have more power. Is this normal? In my video the robot is working without batteries (if you may ask).</p><p>I don't know where to find the serial number of my gyroscope. But I've bought an L3GD20 3-axis gyro carrier with voltage regulator from pololu.com. The same gyroscope you recommended. Can you help me where to find it?</p><p>You can watch my video over here:</p><p><iframe allowfullscreen="" frameborder="0" height="281" src="//www.youtube.com/embed/awLbiHiyE8E" width="500"></iframe></p><p>Here is my Arduino code:</p><p>// &quot;Another easier inverted pendulum balancing robot&quot;</p><p>// You need only half a day to make it, if you have some Materials.</p><p>// (This sketch is ver.2.0.d for a digital output gyroscope.)</p><p>// No timer library is used in this version.</p><p>// But stability of robot is more improved than earlier version.</p><p>// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved.</p><p>//#include &lt;MsTimer2.h&gt; //01 (This line is omitted in ver.2.0 and the later.)</p><p>//int i = 0; //02 (This line is omitted in this version.)</p><p>byte countS = 0; //03</p><p>//long zeroOmegaI = 0; //04 (This line is omitted in this version.)</p><p>int recOmegaI[10]; //05</p><p>int omegaI = 0; //06</p><p>long thetaI = 0; //07</p><p>long sumPower = 0; //08</p><p>long sumSumP = 0; //09</p><p>const int kAngle = 50; //10</p><p>const int kOmega = 500; //11</p><p>const long kSpeed = 60; //12</p><p>const long kDistance = 20; //13</p><p>long powerScale; //14</p><p>int power; //15</p><p>long vE5 = 0; //16</p><p>long xE5 = 0; //17</p><p>#include &lt;SPI.h&gt; //DL1 (These 17 lines, DL1-DL17, are added in this version.)</p><p>int ry; //DL2</p><p>long R; //DL3</p><p>void L3GD20_write(byte reg, byte val) { //DL4</p><p> digitalWrite(10, LOW); //DL5</p><p> SPI.transfer(reg); //DL6</p><p> SPI.transfer(val); //DL7</p><p> digitalWrite(10, HIGH); //DL8</p><p>} //DL9</p><p>byte L3GD20_read(byte reg) { //DL10</p><p> byte ret = 0; //DL11</p><p> digitalWrite(10, LOW); //DL12</p><p> SPI.transfer(reg | 0x80); //DL13</p><p> ret = SPI.transfer(0); //DL14</p><p> digitalWrite(10, HIGH); //DL15</p><p> return ret; //DL16</p><p>} //DL17</p><p>void setup() { //18</p><p> Serial.begin(115200); //19</p><p> pinMode(4, OUTPUT); //20</p><p> pinMode(5, OUTPUT); //20-a</p><p> pinMode(6, OUTPUT); //21</p><p> pinMode(7, OUTPUT);</p><p> pinMode(8, OUTPUT);</p><p> pinMode(9, OUTPUT);</p><p> for (int i = 0; i &lt; 10; i++) {</p><p> recOmegaI[i] = 0;</p><p> } //25 (&quot;int&quot; is added instead of line 2 omitted.)</p><p> pinMode(10, OUTPUT); //DL18 (These 8 lines, DL18-DL25, are added in this version.)</p><p> digitalWrite(10, HIGH); //DL19</p><p> SPI.begin(); //DL20</p><p> SPI.setBitOrder(MSBFIRST); //DL21</p><p> SPI.setDataMode(SPI_MODE3); //DL22</p><p> SPI.setClockDivider(SPI_CLOCK_DIV2); //DL23</p><p> L3GD20_write(0x20, B11001111); //DL24</p><p> L3GD20_write(0x23, B00000000); //DL25</p><p> delay(300); //26</p><p>// training(); // (This line is omitted in this version.)</p><p>// MsTimer2::set(5, chkAndCtl); // (This line is omitted in ver.2.0 and the later.)</p><p>// MsTimer2::start(); // (This line is omitted in ver.2.0 and the later.)</p><p>} //30</p><p>void loop() { //31</p><p> chkAndCtl(); // NL1 (This line is added in ver.2.0 and the later.)</p><p> if (power &gt; 0) { //32</p><p> analogWrite(6, power);</p><p> digitalWrite(4, HIGH);</p><p> digitalWrite(5, LOW); //35</p><p> analogWrite(9, power);</p><p> digitalWrite(7, HIGH);</p><p> digitalWrite(8, LOW);</p><p> } else {</p><p> analogWrite(6, -power); //40</p><p> digitalWrite(4, LOW);</p><p> digitalWrite(5, HIGH);</p><p> analogWrite(9, -power);</p><p> digitalWrite(7, LOW);</p><p> digitalWrite(8, HIGH); //45</p><p> }</p><p>// delayMicroseconds(3600); // NL2 (This is omitted in this version.)</p><p>}</p><p>//void training(){ //48 (These 7 lines, 48-54, are omitted in this version.)</p><p>// delay (1000);</p><p>// for ( i = 0 ; i &lt; 500 ; i++ ){ //50</p><p>// zeroOmegaI = zeroOmegaI + analogRead(A5);</p><p>// }</p><p>// zeroOmegaI = zeroOmegaI / i;</p><p>//} //54</p><p>void chkAndCtl() { //55</p><p>// omegaI = 0; // NL3 (These 6 lines, NL3-NL8, are omitted in this version.)</p><p>// for ( i = 0 ; i &lt; 10 ; i++ ) { //NL4</p><p>// omegaI = omegaI + analogRead(A5) - zeroOmegaI; //NL5</p><p>// delayMicroseconds(10); //NL6</p><p>// } //NL7</p><p>// omegaI = omegaI / 10; //NL8</p><p> R = 0; //DL26 (These 7 lines, DL26-DL32, are added in this version.)</p><p> for (int i = 0; i &lt; 45; i++) { //DL27 (&quot;int&quot; is added instead of line 2 omitted.)</p><p> ry = ((L3GD20_read(0x2B) &lt;&lt; 8) | L3GD20_read(0x2A)); //DL28</p><p> R = R + ry; //DL29</p><p> delayMicroseconds(90); //DL30</p><p> } //DL31</p><p> omegaI = R * 0.00875 / 45; //DL32</p><p>// omegaI = analogRead(A5) - zeroOmegaI; //56 (This line is omitted in ver.2.0 and the later.)</p><p> if (abs(omegaI) &lt; 2) {</p><p> omegaI = 0;</p><p> } //57 (The lower bound is less than 2 in this version.)</p><p> recOmegaI[0] = omegaI;</p><p> thetaI = thetaI + omegaI;</p><p> countS = 0; //60</p><p> for (int i = 0; i &lt; 10; i++) { // (&quot;int&quot; is added instead of line 2 omitted.)</p><p> if (abs(recOmegaI[i]) &lt; 4) {</p><p> countS++;</p><p> } //62 (The lower bound is less than 4 in this version.)</p><p> }</p><p> if (countS &gt; 9) {</p><p> thetaI = 0; //65</p><p> vE5 = 0;</p><p> xE5 = 0;</p><p> sumPower = 0;</p><p> sumSumP = 0;</p><p> } //70</p><p> for (int i = 9; i &gt; 0; i--) {</p><p> recOmegaI[i] = recOmegaI[i - 1];</p><p> } // (&quot;int&quot; is added instead of line 2 omitted.)</p><p>powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 ); //72</p><p> power = max(min(95 * powerScale / 100, 255), -255);</p><p> sumPower = sumPower + power;</p><p> sumSumP = sumSumP + sumPower; //75</p><p>// vE5 = ??? //76</p><p>// xE5 = ??? //77</p><p>} //78</p><p>// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved. //79</p><p>I've already replaced line &quot;72 in the sketch as you recommended in step 5</p><p>The robot seems to be working the same when I add your gyro recovery sketch to my Arduino sketch (step 5).</p><p>Thanks again for helping me out!</p><p>I really appreciate it!</p>
<p>Your robot seems working well as &ldquo;an inverted <strong><em>following </em></strong>robot&rdquo; in Step6. The faint noise &quot;miii&quot; is heard after 1:19 in your video. But you should know the robot cannot balance itself in Step6.</p><p>I recommend you read details carefully in all steps, 1 to 9, again. So you can know the role of batteries. (I forgot to attach 8 tutorial videos. Now I have attached them.)<br></p><p>And you can see <strong><a href="https://www.instructables.com/id/DIY-Brushless-Gimbal-with-Arduino/#step10" rel="nofollow">the schematic of L3GD20 gyro chip in other article</a>.</strong></p>
<p> I have no clue why my robot is not working...</p><p>I have followed your instructions exactly as you posted (bought exactly the same materials,...), but only with an L3GD20 instead of your Azuki Denshi gyro carrier. </p><p>I downloaded your Arduino program and opened it via an PDF reader exactly as you said.</p><p>The wheels of my robot can turn but it cannot balance itself (with help from my finger).</p><p>Any suggestions?</p>

About This Instructable

12,190views

133favorites

More by ArduinoDeXXX:A Levitating Sphere Rotates Glows and Blinks With Arduino Arduinoで作る浮遊光球 Another Easier Inverted Pendulum Robot 
Add instructable to: