├── 3D
├── HorizontalPlate.dxf
├── HorizontalPlate.scad
├── HorizontalPlate.svg
├── LateralPlate.dxf
├── LateralPlate.scad
├── LateralPlate.svg
└── MotorToMeccanoWheel_Adapter.scad
├── LICENSE
├── README.md
├── arduino
└── SelfBalancingRobot4_ESP32
│ ├── SelfBalancingRobot4_ESP32.ino
│ ├── SerialControl.ino
│ └── Wifi.ino
├── docs
├── MPU6050_DigitalLowPassFilter-configurations.jpg
├── SelfBalancingRobot_v4.txt
├── SelfBalancingV4_Fritzing.fzz
└── SelfBalancingV4_Fritzing_schem.png
└── javascript_espruino
├── SelfBalancing_V4_CODE.js
└── SelfBalancing_V4_ManualIntegration.js
/3D/HorizontalPlate.scad:
--------------------------------------------------------------------------------
1 | $fn=72;
2 |
3 | difference() {
4 | linear_extrude(height=5) import(file = "HorizontalPlate.dxf");
5 |
6 | rotate([0, 90, 0]){
7 | translate([-2.5, 5.5, -10]){
8 | cylinder(h=150, r=1.75);
9 | }
10 | translate([-2.5, 36.5, -10]){
11 | cylinder(h=150, r=1.75);
12 | }
13 | }
14 | }
15 |
16 |
17 |
18 |
19 |
--------------------------------------------------------------------------------
/3D/HorizontalPlate.svg:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
808 |
--------------------------------------------------------------------------------
/3D/LateralPlate.scad:
--------------------------------------------------------------------------------
1 | linear_extrude(height=5) import(file = "LateralPlate.dxf");
2 |
3 |
4 |
5 |
6 |
7 |
--------------------------------------------------------------------------------
/3D/LateralPlate.svg:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
297 |
--------------------------------------------------------------------------------
/3D/MotorToMeccanoWheel_Adapter.scad:
--------------------------------------------------------------------------------
1 | rMain=18.4;
2 | hMain=6;
3 | dHub = 20;
4 | hHub = 14;
5 | wheelHolesDistance=13;
6 | $fn=180;
7 |
8 |
9 | difference() {
10 | union(){
11 | cylinder(h=hMain, r=rMain, center=true);
12 | rotate([180, 0, 60]){
13 | translate([0, 0, (hHub + hMain)/2]){
14 | hub(hHub, dHub, 5.5, 5.1, 1, [6, 3], 3.2, -2, 0, 4, 4);
15 | }
16 | }
17 | }
18 |
19 | cylinder(h=hMain, r=2.75, center=true);
20 | wheelScrews(hMain+0.1, 2);
21 | }
22 |
23 |
24 |
25 |
26 |
27 | module wheelScrews(height, recess) {
28 | translate([wheelHolesDistance,0,0]){
29 | wheelScrew(height, recess);
30 | }
31 | rotate([0,0,120]){
32 | translate([wheelHolesDistance,0,0]){
33 | wheelScrew(height, recess);
34 | }
35 | }
36 | rotate([0,0,-120]){
37 | translate([wheelHolesDistance,0,0]){
38 | wheelScrew(height, recess);
39 | }
40 | }
41 | }
42 |
43 | module wheelScrew(height, recess){
44 | cylinder(r=1.7, h=height, center=true);
45 | translate([0,0,-height/2 - 100]){
46 | rotate([0, 0, 30]){
47 | cylinder(r=3.3, h=2*(recess + 100), center=true, $fn=6);
48 | }
49 | }
50 | }
51 |
52 |
53 |
54 |
55 |
56 | // The hub (the part that holds the wheel onto the motor
57 | module hub( height,
58 | diameter,
59 | boreDiameter, // The diameter of the motor shaft
60 | shaftFlatDiameter, // The diameter of the motor shaft at the flat, or shaftDiameter for no flat.
61 | nuts, // The number of set screws/nuts to render, spaced evenly around the shaft
62 | nutSize, // Size [indiameter, thickness] of set screw nut. The depth is set automatically.
63 | setScrewDiameter, // The diameter of the set screw. 3 is the default for an M3 screw.
64 | setScrewNutOffset=0, // The distance to offset the nut from the center of the material. -/+ = in/out
65 | hubZOffset=0,
66 | baseFilletRadius=0, // The radius of the fillet (rounded part) between the hub and wheel.
67 | topFilletRadius=0, // The radius of the fillet (rounded part) at the top of the hub.
68 | chamferOnly=false, // Set to true to use chamfers (straight 45-degree angles) instead of fillets.
69 | hexbore=false // Make the bore a hex shaft vs a circle
70 | )
71 | {
72 |
73 | hubWidth=(diameter-boreDiameter)/2;
74 |
75 | union() {
76 | difference() {
77 |
78 | // Main hub shape
79 | union() {
80 | difference() {
81 | union() {
82 | cylinder( h=height, r=diameter/2, center=true );
83 |
84 | // First chamfer the base...
85 | rotate_extrude()
86 | translate([diameter/2,-(height/2)-hubZOffset,0])
87 | polygon(points=[[0,0],[0,baseFilletRadius],[baseFilletRadius,0]]);
88 | }
89 |
90 | // Chamfer the top...
91 | rotate_extrude()
92 | translate([diameter/2,height/2,0])
93 | polygon(points=[[0.5,0.5],[-topFilletRadius-0.5,0.5],[0.5, -topFilletRadius-0.5]]);
94 |
95 | // Carve the bottom fillet from the chamfer
96 | if ( !chamferOnly ) {
97 | rotate_extrude() {
98 | translate([(diameter/2)+baseFilletRadius,
99 | -(height-(2*baseFilletRadius))/2-hubZOffset,0]) {
100 | circle(r=baseFilletRadius);
101 | }
102 | }
103 | }
104 | }
105 |
106 | // Add the fillet back on top of the top chamfer
107 | if (!chamferOnly) {
108 | rotate_extrude() {
109 | translate([
110 | (diameter/2)-topFilletRadius,
111 | (height-(2*topFilletRadius))/2,
112 | 0])
113 | circle(r=topFilletRadius);
114 | }
115 | }
116 | }
117 |
118 | // Remove the bore
119 | difference() {
120 | if (hexbore) {
121 | cylinder(r=boreDiameter/2/ cos(180/6),h=height+1,$fn=6, center=true);
122 | } else {
123 | difference(){
124 | cylinder( h=height+1, r=boreDiameter/2, center=true );
125 | translate([(boreDiameter-shaftFlatDiameter+1)/2 + (boreDiameter/2)
126 | - (boreDiameter - shaftFlatDiameter),0,0])
127 | cube( [boreDiameter-shaftFlatDiameter+1,boreDiameter,height+2], center=true );
128 | }
129 | }
130 | }
131 |
132 |
133 | // Remove the captive nut
134 | for( i=[0:nuts-1] ) {
135 | if (hexbore) {
136 | rotate([ 0,0, (360/nuts)*i+30 ])
137 | translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset, 0, height/2 - (height+hubZOffset)/2]) {
138 | rotate([0,-90,0]) {
139 | captiveNut( nutSize, setScrewDiameter, depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
140 | +(boreDiameter-shaftFlatDiameter), holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
141 | }
142 | }
143 | } else {
144 | rotate([ 0,0, (360/nuts)*i ])
145 | translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset, 0, height/2 - (height+hubZOffset)/2]) {
146 | rotate([0,-90,0]) {
147 | captiveNut( nutSize, setScrewDiameter, depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
148 | +(boreDiameter-shaftFlatDiameter), holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
149 | }
150 | }
151 | }
152 | }
153 | }
154 | }
155 | }
156 |
157 |
158 | // This is the captive nut module
159 | module captiveNut( nutSize, setScrewHoleDiameter=3,
160 | depth=10, holeLengthTop=5, holeLengthBottom=5 )
161 | {
162 | render()
163 | union() {
164 | nut( nutSize );
165 |
166 | if ( depth > 0 )
167 | translate([depth/2,0,0])
168 | cube( [depth, nutSize[0], nutSize[1]], center=true );
169 |
170 | translate([0,0,-(nutSize[1]/2)-holeLengthBottom])
171 | cylinder(r=setScrewHoleDiameter/2, h=nutSize[1]+holeLengthTop+holeLengthBottom, $fn=15);
172 | }
173 | }
174 |
175 | // nutSize = [inDiameter,thickness]
176 | module nut( nutSize ) {
177 | side = nutSize[0] * tan( 180/6 );
178 | if ( nutSize[0] * nutSize[1] != 0 ) {
179 | for ( i = [0 : 2] ) {
180 | rotate( i*120, [0, 0, 1])
181 | cube( [side, nutSize[0], nutSize[1]], center=true );
182 | }
183 | }
184 | }
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | Apache License
2 | Version 2.0, January 2004
3 | http://www.apache.org/licenses/
4 |
5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6 |
7 | 1. Definitions.
8 |
9 | "License" shall mean the terms and conditions for use, reproduction,
10 | and distribution as defined by Sections 1 through 9 of this document.
11 |
12 | "Licensor" shall mean the copyright owner or entity authorized by
13 | the copyright owner that is granting the License.
14 |
15 | "Legal Entity" shall mean the union of the acting entity and all
16 | other entities that control, are controlled by, or are under common
17 | control with that entity. For the purposes of this definition,
18 | "control" means (i) the power, direct or indirect, to cause the
19 | direction or management of such entity, whether by contract or
20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the
21 | outstanding shares, or (iii) beneficial ownership of such entity.
22 |
23 | "You" (or "Your") shall mean an individual or Legal Entity
24 | exercising permissions granted by this License.
25 |
26 | "Source" form shall mean the preferred form for making modifications,
27 | including but not limited to software source code, documentation
28 | source, and configuration files.
29 |
30 | "Object" form shall mean any form resulting from mechanical
31 | transformation or translation of a Source form, including but
32 | not limited to compiled object code, generated documentation,
33 | and conversions to other media types.
34 |
35 | "Work" shall mean the work of authorship, whether in Source or
36 | Object form, made available under the License, as indicated by a
37 | copyright notice that is included in or attached to the work
38 | (an example is provided in the Appendix below).
39 |
40 | "Derivative Works" shall mean any work, whether in Source or Object
41 | form, that is based on (or derived from) the Work and for which the
42 | editorial revisions, annotations, elaborations, or other modifications
43 | represent, as a whole, an original work of authorship. For the purposes
44 | of this License, Derivative Works shall not include works that remain
45 | separable from, or merely link (or bind by name) to the interfaces of,
46 | the Work and Derivative Works thereof.
47 |
48 | "Contribution" shall mean any work of authorship, including
49 | the original version of the Work and any modifications or additions
50 | to that Work or Derivative Works thereof, that is intentionally
51 | submitted to Licensor for inclusion in the Work by the copyright owner
52 | or by an individual or Legal Entity authorized to submit on behalf of
53 | the copyright owner. For the purposes of this definition, "submitted"
54 | means any form of electronic, verbal, or written communication sent
55 | to the Licensor or its representatives, including but not limited to
56 | communication on electronic mailing lists, source code control systems,
57 | and issue tracking systems that are managed by, or on behalf of, the
58 | Licensor for the purpose of discussing and improving the Work, but
59 | excluding communication that is conspicuously marked or otherwise
60 | designated in writing by the copyright owner as "Not a Contribution."
61 |
62 | "Contributor" shall mean Licensor and any individual or Legal Entity
63 | on behalf of whom a Contribution has been received by Licensor and
64 | subsequently incorporated within the Work.
65 |
66 | 2. Grant of Copyright License. Subject to the terms and conditions of
67 | this License, each Contributor hereby grants to You a perpetual,
68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69 | copyright license to reproduce, prepare Derivative Works of,
70 | publicly display, publicly perform, sublicense, and distribute the
71 | Work and such Derivative Works in Source or Object form.
72 |
73 | 3. Grant of Patent License. Subject to the terms and conditions of
74 | this License, each Contributor hereby grants to You a perpetual,
75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76 | (except as stated in this section) patent license to make, have made,
77 | use, offer to sell, sell, import, and otherwise transfer the Work,
78 | where such license applies only to those patent claims licensable
79 | by such Contributor that are necessarily infringed by their
80 | Contribution(s) alone or by combination of their Contribution(s)
81 | with the Work to which such Contribution(s) was submitted. If You
82 | institute patent litigation against any entity (including a
83 | cross-claim or counterclaim in a lawsuit) alleging that the Work
84 | or a Contribution incorporated within the Work constitutes direct
85 | or contributory patent infringement, then any patent licenses
86 | granted to You under this License for that Work shall terminate
87 | as of the date such litigation is filed.
88 |
89 | 4. Redistribution. You may reproduce and distribute copies of the
90 | Work or Derivative Works thereof in any medium, with or without
91 | modifications, and in Source or Object form, provided that You
92 | meet the following conditions:
93 |
94 | (a) You must give any other recipients of the Work or
95 | Derivative Works a copy of this License; and
96 |
97 | (b) You must cause any modified files to carry prominent notices
98 | stating that You changed the files; and
99 |
100 | (c) You must retain, in the Source form of any Derivative Works
101 | that You distribute, all copyright, patent, trademark, and
102 | attribution notices from the Source form of the Work,
103 | excluding those notices that do not pertain to any part of
104 | the Derivative Works; and
105 |
106 | (d) If the Work includes a "NOTICE" text file as part of its
107 | distribution, then any Derivative Works that You distribute must
108 | include a readable copy of the attribution notices contained
109 | within such NOTICE file, excluding those notices that do not
110 | pertain to any part of the Derivative Works, in at least one
111 | of the following places: within a NOTICE text file distributed
112 | as part of the Derivative Works; within the Source form or
113 | documentation, if provided along with the Derivative Works; or,
114 | within a display generated by the Derivative Works, if and
115 | wherever such third-party notices normally appear. The contents
116 | of the NOTICE file are for informational purposes only and
117 | do not modify the License. You may add Your own attribution
118 | notices within Derivative Works that You distribute, alongside
119 | or as an addendum to the NOTICE text from the Work, provided
120 | that such additional attribution notices cannot be construed
121 | as modifying the License.
122 |
123 | You may add Your own copyright statement to Your modifications and
124 | may provide additional or different license terms and conditions
125 | for use, reproduction, or distribution of Your modifications, or
126 | for any such Derivative Works as a whole, provided Your use,
127 | reproduction, and distribution of the Work otherwise complies with
128 | the conditions stated in this License.
129 |
130 | 5. Submission of Contributions. Unless You explicitly state otherwise,
131 | any Contribution intentionally submitted for inclusion in the Work
132 | by You to the Licensor shall be under the terms and conditions of
133 | this License, without any additional terms or conditions.
134 | Notwithstanding the above, nothing herein shall supersede or modify
135 | the terms of any separate license agreement you may have executed
136 | with Licensor regarding such Contributions.
137 |
138 | 6. Trademarks. This License does not grant permission to use the trade
139 | names, trademarks, service marks, or product names of the Licensor,
140 | except as required for reasonable and customary use in describing the
141 | origin of the Work and reproducing the content of the NOTICE file.
142 |
143 | 7. Disclaimer of Warranty. Unless required by applicable law or
144 | agreed to in writing, Licensor provides the Work (and each
145 | Contributor provides its Contributions) on an "AS IS" BASIS,
146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 | implied, including, without limitation, any warranties or conditions
148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 | PARTICULAR PURPOSE. You are solely responsible for determining the
150 | appropriateness of using or redistributing the Work and assume any
151 | risks associated with Your exercise of permissions under this License.
152 |
153 | 8. Limitation of Liability. In no event and under no legal theory,
154 | whether in tort (including negligence), contract, or otherwise,
155 | unless required by applicable law (such as deliberate and grossly
156 | negligent acts) or agreed to in writing, shall any Contributor be
157 | liable to You for damages, including any direct, indirect, special,
158 | incidental, or consequential damages of any character arising as a
159 | result of this License or out of the use or inability to use the
160 | Work (including but not limited to damages for loss of goodwill,
161 | work stoppage, computer failure or malfunction, or any and all
162 | other commercial damages or losses), even if such Contributor
163 | has been advised of the possibility of such damages.
164 |
165 | 9. Accepting Warranty or Additional Liability. While redistributing
166 | the Work or Derivative Works thereof, You may choose to offer,
167 | and charge a fee for, acceptance of support, warranty, indemnity,
168 | or other liability obligations and/or rights consistent with this
169 | License. However, in accepting such obligations, You may act only
170 | on Your own behalf and on Your sole responsibility, not on behalf
171 | of any other Contributor, and only if You agree to indemnify,
172 | defend, and hold each Contributor harmless for any liability
173 | incurred by, or claims asserted against, such Contributor by reason
174 | of your accepting any such warranty or additional liability.
175 |
176 | END OF TERMS AND CONDITIONS
177 |
178 | APPENDIX: How to apply the Apache License to your work.
179 |
180 | To apply the Apache License to your work, attach the following
181 | boilerplate notice, with the fields enclosed by brackets "{}"
182 | replaced with your own identifying information. (Don't include
183 | the brackets!) The text should be enclosed in the appropriate
184 | comment syntax for the file format. We also recommend that a
185 | file or class name and description of purpose be included on the
186 | same "printed page" as the copyright notice for easier
187 | identification within third-party archives.
188 |
189 | Copyright {yyyy} {name of copyright owner}
190 |
191 | Licensed under the Apache License, Version 2.0 (the "License");
192 | you may not use this file except in compliance with the License.
193 | You may obtain a copy of the License at
194 |
195 | http://www.apache.org/licenses/LICENSE-2.0
196 |
197 | Unless required by applicable law or agreed to in writing, software
198 | distributed under the License is distributed on an "AS IS" BASIS,
199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 | See the License for the specific language governing permissions and
201 | limitations under the License.
202 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # esp32-self_balancing_robot
2 | 3D printed self balancing robot using and ESP32 for the brains, MPU6050 for gyros & accelerometres and DRV8825 to drive the 2 servo motors
3 |
--------------------------------------------------------------------------------
/arduino/SelfBalancingRobot4_ESP32/SelfBalancingRobot4_ESP32.ino:
--------------------------------------------------------------------------------
1 | /**
2 | * Copyright 2017 Dan Oprescu
3 | *
4 | * Licensed under the Apache License, Version 2.0 (the "License");
5 | * you may not use this file except in compliance with the License.
6 | * You may obtain a copy of the License at
7 | *
8 | * http://www.apache.org/licenses/LICENSE-2.0
9 | *
10 | * Unless required by applicable law or agreed to in writing, software
11 | * distributed under the License is distributed on an "AS IS" BASIS,
12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | * See the License for the specific language governing permissions and
14 | * limitations under the License.
15 | */
16 |
17 | #include
18 |
19 | #define PERIOD 4000 // loop period in micros
20 | #define PRINT_PERIOD 100000 // print period in micros
21 |
22 |
23 | //////////////// MOTORS ////////////////////////////
24 |
25 | #define MOT_R_ENB 32
26 | #define MOT_R_STP 33
27 | #define MOT_R_DIR 25
28 | #define MOT_R_CHANNEL 1 // for the ledc library
29 |
30 | #define MOT_L_ENB 26
31 | #define MOT_L_STP 14
32 | #define MOT_L_DIR 27
33 | #define MOT_L_CHANNEL 2 // for the ledc library
34 |
35 | #define MAX_SPEED 20000
36 |
37 | uint32_t prevSpeedStart;
38 | int16_t prevSpeed;
39 | int32_t currentPos = 0;
40 |
41 |
42 | void setup_motors() {
43 | ledcAttachPin(MOT_L_STP, MOT_L_CHANNEL);
44 | ledcSetup(MOT_L_CHANNEL, 0, 10); // these will be updated later by the ledcWriteNote()
45 | ledcAttachPin(MOT_R_STP, MOT_R_CHANNEL);
46 | ledcSetup(MOT_R_CHANNEL, 0, 10); // these will be updated later by the ledcWriteNote()
47 |
48 | pinMode(MOT_L_ENB, OUTPUT);
49 | pinMode(MOT_L_DIR, OUTPUT);
50 | disableL(true);
51 |
52 | pinMode(MOT_R_ENB, OUTPUT);
53 | pinMode(MOT_R_DIR, OUTPUT);
54 | disableR(true);
55 | }
56 |
57 | void disableL(bool orEnable) {
58 | digitalWrite(MOT_L_ENB, orEnable);
59 | }
60 |
61 | void disableR(bool orEnable) {
62 | digitalWrite(MOT_R_ENB, orEnable);
63 | }
64 |
65 | void forwardL(bool orBack) {
66 | digitalWrite(MOT_L_DIR, orBack);
67 | }
68 | void forwardR(bool orBack) {
69 | digitalWrite(MOT_R_DIR, orBack);
70 | }
71 |
72 | void setSpeed(int16_t s, int16_t rotation) {
73 | int16_t sL = s - rotation;
74 | int16_t sR = s + rotation;
75 | boolean backwardL = sL < 0;
76 | boolean backwardR = sR < 0;
77 |
78 | if(backwardL){
79 | forwardL(false);
80 | sL = -sL;
81 | }else{
82 | forwardL(true);
83 | }
84 |
85 | if(backwardR){
86 | forwardR(false);
87 | sR = -sR;
88 | }else{
89 | forwardR(true);
90 | }
91 |
92 |
93 | disableL(sL < MAX_SPEED / 100);
94 | disableR(sR < MAX_SPEED / 100);
95 |
96 | if(sL > MAX_SPEED) sL = MAX_SPEED;
97 | if(sR > MAX_SPEED) sR = MAX_SPEED;
98 |
99 | // keep track of the position (in steps forward or backward)
100 | currentPos += ((micros() - prevSpeedStart) / (float)1000000) * prevSpeed;
101 | prevSpeed = s;
102 | prevSpeedStart = micros();
103 |
104 | // set the new speed
105 | ledcWriteTone(MOT_L_CHANNEL, sL);
106 | ledcWriteTone(MOT_R_CHANNEL, sR);
107 | }
108 |
109 |
110 |
111 |
112 |
113 | ///////////////// MPU-6050 //////////////////////////
114 |
115 |
116 | static int MPU_ADDR = 0x69; //AD0 is HIGH
117 |
118 | // MPU6050 specific
119 | #define MPU6050_FS_SEL0 3
120 | #define MPU6050_FS_SEL1 4
121 | #define MPU6050_AFS_SEL0 3
122 | #define MPU6050_AFS_SEL1 4
123 |
124 | // Combined definitions for the FS_SEL values.eg. ±250 degrees/second
125 | #define MPU6050_FS_SEL_250 (0)
126 | #define MPU6050_FS_SEL_500 (bit(MPU6050_FS_SEL0))
127 | #define MPU6050_FS_SEL_1000 (bit(MPU6050_FS_SEL1))
128 | #define MPU6050_FS_SEL_2000 (bit(MPU6050_FS_SEL1) | bit(MPU6050_FS_SEL0))
129 |
130 | // Combined definitions for the AFS_SEL values
131 | #define MPU6050_AFS_SEL_2G (0)
132 | #define MPU6050_AFS_SEL_4G (bit(MPU6050_AFS_SEL0))
133 | #define MPU6050_AFS_SEL_8G (bit(MPU6050_AFS_SEL1))
134 | #define MPU6050_AFS_SEL_16G (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0))
135 |
136 | // See page 12 & 13 of MPU-6050 datasheet for sensitivities config and corresponding output
137 | #define GYRO_FULL_SCALE_RANGE MPU6050_FS_SEL_250
138 | #define GYRO_SCALE_FACTOR 131 // LSB / (degs per seconds)
139 | #define ACC_FULL_SCALE_RANGE MPU6050_AFS_SEL_4G
140 | #define ACC_SCALE_FACTOR 8192 // LSB / g
141 |
142 |
143 |
144 | static float GYRO_RAW_TO_DEGS = 1.0 / (1000000.0 / PERIOD) / GYRO_SCALE_FACTOR;
145 |
146 | int16_t accX, accY, accZ;
147 | int16_t gyroX, gyroY, gyroZ;
148 | int16_t gyroX_calibration, gyroY_calibration, gyroZ_calibration;
149 |
150 |
151 | void setup_mpu() {
152 | Wire.begin();
153 | Wire.setClock(400000L);
154 |
155 | //By default the MPU-6050 sleeps. So we have to wake it up.
156 | Wire.beginTransmission(MPU_ADDR);
157 | Wire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex)
158 | Wire.write(0x00); //Set the register bits as 00000000 to activate the gyro
159 | Wire.endTransmission();
160 |
161 | Wire.beginTransmission(MPU_ADDR);
162 | Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)
163 | Wire.write(GYRO_FULL_SCALE_RANGE);
164 | Wire.endTransmission();
165 |
166 | Wire.beginTransmission(MPU_ADDR);
167 | Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex)
168 | Wire.write(ACC_FULL_SCALE_RANGE);
169 | Wire.endTransmission();
170 | //Set some filtering to improve the raw data.
171 | Wire.beginTransmission(MPU_ADDR);
172 | Wire.write(0x1A); //We want to write to the CONFIG register (1A hex)
173 | Wire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
174 | Wire.endTransmission();
175 |
176 |
177 | calibrateGyro();
178 | }
179 |
180 |
181 | #define ACCEL_XOUT_H 0x3B
182 | #define ACCEL_XOUT_L 0x3C
183 | #define ACCEL_YOUT_H 0x3D
184 | #define ACCEL_YOUT_L 0x3E
185 | #define ACCEL_ZOUT_H 0x3F
186 | #define ACCEL_ZOUT_L 0x40
187 |
188 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
189 | Wire.beginTransmission(MPU_ADDR);
190 | Wire.write(ACCEL_XOUT_H);
191 | Wire.endTransmission();
192 | Wire.requestFrom(MPU_ADDR, 6);
193 | *x = constr((((int16_t)Wire.read()) << 8) | Wire.read(), -ACC_SCALE_FACTOR, ACC_SCALE_FACTOR);
194 | *y = constr((((int16_t)Wire.read()) << 8) | Wire.read(), -ACC_SCALE_FACTOR, ACC_SCALE_FACTOR);
195 | *z = constr((((int16_t)Wire.read()) << 8) | Wire.read(), -ACC_SCALE_FACTOR, ACC_SCALE_FACTOR);
196 | }
197 |
198 | #define GYRO_XOUT_H 0x43
199 | #define GYRO_XOUT_L 0x44
200 | #define GYRO_YOUT_H 0x45
201 | #define GYRO_YOUT_L 0x46
202 | #define GYRO_ZOUT_H 0x47
203 | #define GYRO_ZOUT_L 0x48
204 |
205 | void getRotation(int16_t* x, int16_t* y, int16_t* z) {
206 | Wire.beginTransmission(MPU_ADDR);
207 | Wire.write(GYRO_XOUT_H);
208 | Wire.endTransmission();
209 | Wire.requestFrom(MPU_ADDR, 6);
210 | *x = ((((int16_t)Wire.read()) << 8) | Wire.read()) - gyroX_calibration;
211 | *y = ((((int16_t)Wire.read()) << 8) | Wire.read()) - gyroY_calibration;
212 | *z = ((((int16_t)Wire.read()) << 8) | Wire.read()) - gyroZ_calibration;
213 | }
214 |
215 |
216 | void calibrateGyro() {
217 | int32_t x, y, z;
218 |
219 | for(int i=0; i<500; i++){
220 | getRotation(&gyroX, &gyroY, &gyroZ);
221 | x += gyroX;
222 | y += gyroY;
223 | z += gyroZ;
224 |
225 | delayMicroseconds(PERIOD); // simulate the main program loop time ??
226 | }
227 |
228 | gyroX_calibration = x / 500;
229 | gyroY_calibration = y / 500;
230 | gyroZ_calibration = z / 500;
231 | }
232 |
233 |
234 | // on ESP32 Arduino constrain doesn't work
235 | int16_t constr(int16_t value, int16_t mini, int16_t maxi) {
236 | if(value < mini) return mini;
237 | else if(value > maxi) return maxi;
238 | return value;
239 | }
240 | float constrf(float value, float mini, float maxi) {
241 | if(value < mini) return mini;
242 | else if(value > maxi) return maxi;
243 | return value;
244 | }
245 |
246 |
247 | //////// PID //////////
248 |
249 | #define MAX_PID_OUTPUT 500
250 |
251 | float BASE_Kp = 100.0, BASE_Ki = 5.0, BASE_Kd = 130.0;
252 | float Kp = BASE_Kp, Ki = BASE_Ki, Kd = BASE_Kd;
253 | float angleSetpoint = 0, selfBalanceAngleSetpoint = 0;
254 | float pidOutput, pidError, pidLastError, integralErr, positionErr, serialControlErr, prevSerialControlErr, errorDerivative;
255 |
256 | float MAX_CONTROL_OR_POSITION_ERR = MAX_PID_OUTPUT / Kp;
257 | float MAX_CONTROL_ERR_INCREMENT = MAX_CONTROL_OR_POSITION_ERR / 400;
258 | #define MIN_CONTROL_ERR 1
259 |
260 |
261 | //////// MAIN //////////
262 |
263 | // populated in the SerialControl part
264 | uint8_t joystickX;
265 | uint8_t joystickY;
266 |
267 | uint32_t loop_timer;
268 | uint32_t print_timer;
269 |
270 | float roll, pitch, rollAcc, pitchAcc;
271 | float speeed;
272 |
273 |
274 | void setup() {
275 | Serial.begin(500000);
276 |
277 | setup_mpu();
278 | setup_motors();
279 | setup_serial_control();
280 | setup_wifi();
281 |
282 |
283 | loop_timer = micros() + PERIOD;
284 | print_timer = micros() + PRINT_PERIOD;
285 | }
286 |
287 |
288 |
289 | void loop() {
290 | getAcceleration(&accX, &accY, &accZ);
291 | rollAcc = asin((float)accX / ACC_SCALE_FACTOR) * RAD_TO_DEG;
292 | pitchAcc = asin((float)accY / ACC_SCALE_FACTOR) * RAD_TO_DEG;
293 |
294 | getRotation(&gyroX, &gyroY, &gyroZ);
295 | // roll vs pitch depends on how the MPU is installed in the robot
296 | roll -= gyroY * GYRO_RAW_TO_DEGS;
297 | pitch += gyroX * GYRO_RAW_TO_DEGS;
298 | // sin() has to be applied on radians
299 | // roll += pitch * sin((float)gyroZ * GYRO_RAW_TO_DEGS * DEG_TO_RAD);
300 | // pitch -= roll * sin((float)gyroZ * GYRO_RAW_TO_DEGS * DEG_TO_RAD);
301 |
302 |
303 | roll = roll * 0.999 + rollAcc * 0.001;
304 | pitch = pitch * 0.999 + pitchAcc * 0.001;
305 |
306 |
307 | // apply PID algo
308 |
309 | // The selfBalanceAngleSetpoint variable is automatically changed to make sure that the robot stays balanced all the time.
310 | positionErr = constrf(currentPos / (float)1000, -MAX_CONTROL_OR_POSITION_ERR, MAX_CONTROL_OR_POSITION_ERR);
311 | serialControlErr = 0;
312 | if(isValidJoystickValue(joystickY)) {
313 | serialControlErr = constrf((joystickY - 130) / (float)15, -MAX_CONTROL_OR_POSITION_ERR, MAX_CONTROL_OR_POSITION_ERR);
314 | // this control has to change slowly/gradually to avoid shaking the robot
315 | if(serialControlErr < prevSerialControlErr) {
316 | serialControlErr = max(serialControlErr, prevSerialControlErr - MAX_CONTROL_ERR_INCREMENT);
317 | } else {
318 | serialControlErr = min(serialControlErr, prevSerialControlErr + MAX_CONTROL_ERR_INCREMENT);
319 | }
320 |
321 | prevSerialControlErr = serialControlErr;
322 | }
323 |
324 |
325 | pidError = pitch - angleSetpoint - selfBalanceAngleSetpoint;
326 | // either no manual / serial control -> try to keep the position or follow manual control
327 | if(abs(serialControlErr) > MIN_CONTROL_ERR) {
328 | pidError += serialControlErr > 0 ? serialControlErr - MIN_CONTROL_ERR : serialControlErr + MIN_CONTROL_ERR;
329 | // re-init position so it doesn't try to go back when getting out of manual control mode
330 | currentPos = 0;
331 | } else {
332 | pidError += positionErr;
333 | }
334 |
335 |
336 | integralErr = constrf(integralErr + Ki * pidError, -MAX_PID_OUTPUT, MAX_PID_OUTPUT);
337 | errorDerivative = pidError - pidLastError;
338 |
339 | pidOutput = Kp * pidError + integralErr + Kd * errorDerivative;
340 |
341 | if(pidOutput < 5 && pidOutput > -5) pidOutput = 0; //Create a dead-band to stop the motors when the robot is balanced
342 |
343 | if(pitch > 30 || pitch < -30) { //If the robot tips over
344 | pidOutput = 0;
345 | integralErr = 0;
346 | selfBalanceAngleSetpoint = 0;
347 | }
348 |
349 | // store error for next loop
350 | pidLastError = pidError;
351 |
352 | int16_t rotation = 0;
353 | if(isValidJoystickValue(joystickX)) {
354 | rotation = constrf((float)(joystickX - 130), -MAX_PID_OUTPUT, MAX_PID_OUTPUT) * (MAX_SPEED / MAX_PID_OUTPUT);
355 | }
356 |
357 | if(micros() >= print_timer) {
358 | Serial.print(positionErr);Serial.print(" - ");Serial.print(rotation); Serial.print(" / ");Serial.println(serialControlErr);
359 | //Serial.print(pitch); Serial.print(" / ");Serial.print(errorDerivative);Serial.print(" - ");Serial.println(selfBalanceAngleSetpoint);
360 | //Serial.print(accX); Serial.print(" / ");Serial.print(accY); Serial.print(" / ");Serial.print(accZ); Serial.print(" / ");Serial.print(gyroX); Serial.print(" / ");Serial.print(gyroY);Serial.print(" / ");Serial.println(gyroZ);
361 | print_timer += PRINT_PERIOD;
362 | }
363 |
364 |
365 | //The self balancing point is adjusted when there is not forward or backwards movement from the transmitter. This way the robot will always find it's balancing point
366 | // if(angleSetpoint == 0) { //If the setpoint is zero degrees
367 | // if(pidOutput < 0) selfBalanceAngleSetpoint -= 0.0015; //Increase the self_balance_pid_setpoint if the robot is still moving forward
368 | // if(pidOutput > 0) selfBalanceAngleSetpoint += 0.0015; //Decrease the self_balance_pid_setpoint if the robot is still moving backward
369 | // }
370 |
371 |
372 | setSpeed(constrf(pidOutput, -MAX_PID_OUTPUT, MAX_PID_OUTPUT) * (MAX_SPEED / MAX_PID_OUTPUT), rotation);
373 |
374 | // The angle calculations are tuned for a loop time of PERIOD milliseconds.
375 | // To make sure every loop is exactly that, a wait loop is created by setting the loop_timer
376 | if(loop_timer <= micros()) Serial.println("ERROR loop too short !");
377 | while(loop_timer > micros());
378 | loop_timer += PERIOD;
379 | }
380 |
381 |
--------------------------------------------------------------------------------
/arduino/SelfBalancingRobot4_ESP32/SerialControl.ino:
--------------------------------------------------------------------------------
1 | /**
2 | * Copyright 2017 Dan Oprescu
3 | *
4 | * Licensed under the Apache License, Version 2.0 (the "License");
5 | * you may not use this file except in compliance with the License.
6 | * You may obtain a copy of the License at
7 | *
8 | * http://www.apache.org/licenses/LICENSE-2.0
9 | *
10 | * Unless required by applicable law or agreed to in writing, software
11 | * distributed under the License is distributed on an "AS IS" BASIS,
12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | * See the License for the specific language governing permissions and
14 | * limitations under the License.
15 | */
16 |
17 |
18 | HardwareSerial SerialControl(1);
19 |
20 | /////////////// AUREL Wireless Commands /////////////////////////////////////
21 |
22 | uint8_t _prevChar;
23 | boolean _readingMsg = false;
24 | uint8_t _msg[6];
25 | uint8_t _msgPos;
26 | boolean _validData = false;
27 |
28 |
29 | boolean startNewMsg(uint8_t c) {
30 | boolean res = (_prevChar == 0) && (c == 255);
31 | _prevChar = c;
32 | return res;
33 | }
34 |
35 |
36 | void serialControlLoop(void *params) {
37 | Serial.println("\nStarting thread dealing with Serial Control requests...");
38 | uint8_t currChar;
39 |
40 | while(true) {
41 | while (SerialControl.available()) {
42 | currChar = SerialControl.read();
43 |
44 | if (startNewMsg(currChar)) {
45 | _readingMsg = true;
46 | _msgPos = 0;
47 | } else if (_readingMsg) {
48 | if (_msgPos >= 6) {
49 | // data finished, last byte is the CRC
50 | uint8_t crc = 0;
51 | for (uint8_t i = 0; i < 6; i++)
52 | crc += _msg[i];
53 |
54 | if (crc == currChar) {
55 | joystickX = _msg[0];
56 | joystickY = _msg[1];
57 | _validData = true;
58 | } else {
59 | _validData = false;
60 | Serial.print("Wrong CRC: ");Serial.print(currChar);Serial.print(" Expected: ");Serial.println(crc);
61 | }
62 |
63 | _readingMsg = false;
64 | } else {
65 | // normal data, add it to the message
66 | _msg[_msgPos++] = currChar;
67 | }
68 | }
69 | }
70 |
71 | delay(1);
72 | }
73 | }
74 |
75 | void setup_serial_control() {
76 | SerialControl.begin(9600, SERIAL_8N1, 17, 16);
77 |
78 | // deal with control requests in a separate thread, to avoid impacting the real time balancing
79 | // lower number means lower priority. 1 is just above tskIDLE_PRIORITY == 0 which is the lowest priority
80 | // use same core as rest of Arduino code as the other one is for system tasks
81 | xTaskCreatePinnedToCore(serialControlLoop, "serialControlLoop", 4096, NULL, 2, NULL, xPortGetCoreID());
82 | }
83 |
84 |
85 | boolean isValidJoystickValue(uint8_t joystick) {
86 | return joystick > 20 && joystick < 230;
87 | }
88 |
89 |
--------------------------------------------------------------------------------
/arduino/SelfBalancingRobot4_ESP32/Wifi.ino:
--------------------------------------------------------------------------------
1 | /**
2 | * Copyright 2017 Dan Oprescu
3 | *
4 | * Licensed under the Apache License, Version 2.0 (the "License");
5 | * you may not use this file except in compliance with the License.
6 | * You may obtain a copy of the License at
7 | *
8 | * http://www.apache.org/licenses/LICENSE-2.0
9 | *
10 | * Unless required by applicable law or agreed to in writing, software
11 | * distributed under the License is distributed on an "AS IS" BASIS,
12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | * See the License for the specific language governing permissions and
14 | * limitations under the License.
15 | */
16 |
17 |
18 | #include
19 | #include
20 |
21 | ESP32WebServer server(80);
22 |
23 | void displayInfo() {
24 | Serial.println("Handle Display Info...");
25 | String message = "
Self balancing robot\n
";
26 | message += "
Position=" + String(currentPos) + "
";
27 | message += "
Kp=" + String(Kp) + "
";
28 | message += "
Ki=" + String(Ki) + "
";
29 | message += "
Kd=" + String(Kd) + "
";
30 | message += "
SP=" + String(angleSetpoint) + "
";
31 | server.send(200, "text/html", message);
32 | }
33 |
34 |
35 | void handleNotFound() {
36 | String message = "File Not Found\n\n";
37 | message += "URI: " + server.uri();
38 | message += "\nMethod: " + (server.method() == HTTP_GET)?" GET":" POST";
39 | message += "\nArguments: " + server.args();
40 | message += "\n";
41 | for (uint8_t i=0; i use ENABLE, as I don't care about the current stepping tables, etc. ...
5 | # 250kHz MAX
6 | ## Experimental
7 | # in Micro stepping mode (1/32 step) analogWrite(B14, 0.1, {freq: 65000})
8 | # in normal stepping mode (1/1 step) analogWrite(B14, 0.5, {freq: 1500})
9 |
10 | #### MPU6050
11 | ### http://www.brokking.net/yabr_main.html
12 | ### https://github.com/espruino/EspruinoDocs/commits/master/devices/MPU6050.js
13 | ### DLPF
14 | ## https://ulrichbuschbaum.wordpress.com/2015/01/18/using-the-mpu6050s-dlpf/
15 | ##
16 |
17 |
18 |
19 | Gyro with 500degs / sec full scale => output = 65.5 when 1deg/sec
20 | Integrating every 20ms (50HZ)
21 | Gyro out / (1000ms / 20) / 65.5 = Gyro out * 0.000305344 = degrees each iteration
22 |
23 | Angle += Gyro* 0.000305344
24 |
25 | Accelerometre with +- 8G full scale => output = 4096 for 1G
26 |
27 |
--------------------------------------------------------------------------------
/docs/SelfBalancingV4_Fritzing.fzz:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/trandi/esp32-self_balancing_robot/fe7b3ab58ac5dde099d6193eef92db85730c0417/docs/SelfBalancingV4_Fritzing.fzz
--------------------------------------------------------------------------------
/docs/SelfBalancingV4_Fritzing_schem.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/trandi/esp32-self_balancing_robot/fe7b3ab58ac5dde099d6193eef92db85730c0417/docs/SelfBalancingV4_Fritzing_schem.png
--------------------------------------------------------------------------------
/javascript_espruino/SelfBalancing_V4_CODE.js:
--------------------------------------------------------------------------------
1 | var MR_ENB = B10;
2 | var MR_STP = A7;
3 | var MR_DIR = B1;
4 | var ML_ENB = B5;
5 | var ML_STP = B4;
6 | var ML_DIR = B3;
7 |
8 |
9 | var KP = 150000;
10 | var KI = 0;
11 | var KD = 10;
12 | var horizontalCoeff = -0.01; //adjust for the fact that the robot is not completely aligned with the sensor
13 |
14 |
15 |
16 | var MPU;
17 | var DMP;
18 |
19 |
20 | // empirical max frequency/speed when microstepping (1/32 step) with current limited @ 300mA for each motor ?
21 | var MAX_SPEED = 20000;
22 | var MAX_INTEGRAL_ERR = 5000;
23 |
24 |
25 | function onInit() {
26 | console.log("START");
27 | disable(true);
28 |
29 | I2C1.setup({scl:B6, sda:B7, bitrate:100000});
30 | // "true" because I have AD0 connected to VCC
31 | MPU = require("MPU6050").connect(I2C1, true);
32 | // 200Hz / (1 + 4) = 40Hz
33 | DMP = require("MPU6050_DMP-trandi").create(MPU, 4);
34 |
35 | // the MPU notifies us everytime there's available data (50Hz as per the configuration of DMP module)
36 | setWatch(pidLoop, A8, { repeat:true, edge:'falling' }); // should it be 'falling' ?
37 |
38 | console.log("DONE");
39 | }
40 |
41 |
42 |
43 | function forward(orBack) {
44 | digitalWrite(MR_DIR, orBack);
45 | digitalWrite(ML_DIR, orBack);
46 | }
47 |
48 |
49 | function disable(orEnable) {
50 | digitalWrite(MR_ENB, orEnable);
51 | digitalWrite(ML_ENB, orEnable);
52 | }
53 |
54 | function setSpeed(s) {
55 | if(s < 0){
56 | forward(false);
57 | s = -s;
58 | }else{
59 | forward(true);
60 | }
61 |
62 | disable(s < 10);
63 |
64 | if(s > MAX_SPEED) s = MAX_SPEED;
65 | else if(s === 0) s = 1; // "freq: 0" won't work, whereas 1Hz is pretty much stopped
66 |
67 | analogWrite(MR_STP, 0.5, {freq: s});
68 | analogWrite(ML_STP, 0.5, {freq: s});
69 | }
70 |
71 |
72 | // frequency at which we'll toggle the step pin, proportional with the resulting RPM
73 | var speed = 0;
74 | var propErr = 0;
75 | var intErr = 0;
76 | var setPoint = 0;
77 |
78 |
79 |
80 | function pidLoop() {
81 | var data = DMP.getData();
82 |
83 | if(data !== undefined) {
84 | var roll = DMP.getYawPitchRoll(data).roll + horizontalCoeff;
85 | var gyrox = data.gyrox;
86 |
87 | propErr = roll - setPoint;
88 |
89 | intErr = KI * intErr;
90 | if(intErr > MAX_INTEGRAL_ERR) intErr = MAX_INTEGRAL_ERR;
91 | else if(intErr < -MAX_INTEGRAL_ERR) intErr = -MAX_INTEGRAL_ERR;
92 |
93 | if(Math.abs(propErr) > 0.3) {
94 | speed = 0;
95 | } else {
96 | speed = KP * propErr + intErr + KD * gyrox;
97 | intErr = intErr + propErr;
98 | }
99 |
100 | console.log(roll + " - " + gyrox + " / " + speed);
101 | setSpeed(speed);
102 | }
103 | }
104 |
105 |
106 |
107 |
--------------------------------------------------------------------------------
/javascript_espruino/SelfBalancing_V4_ManualIntegration.js:
--------------------------------------------------------------------------------
1 | var MR_ENB = B10;
2 | var MR_STP = A7;
3 | var MR_DIR = B1;
4 | var ML_ENB = B5;
5 | var ML_STP = B4;
6 | var ML_DIR = B3;
7 |
8 |
9 | var KP = 2500;
10 | var KI = 0;
11 | var KD = 0;
12 |
13 |
14 |
15 | var MPU;
16 | var gyroOffsetX = 0;
17 | var gyroOffsetY = 0;
18 | var gyroOffsetZ = 0;
19 | var calibCount = 0;
20 | var calibIntId;
21 |
22 |
23 | // empirical max frequency/speed when microstepping (1/32 step) with current limited @ 300mA for each motor ?
24 | var MAX_SPEED = 20000;
25 | var MAX_INTEGRAL_ERR = 5000;
26 |
27 |
28 | function onInit() {
29 | console.log("START");
30 | disable(true);
31 |
32 | I2C1.setup({scl:B6, sda:B7, bitrate:100000});
33 | // "true" because I have AD0 connected to VCC
34 | MPU = require("MPU6050").connect(I2C1, true);
35 |
36 | MPU.setFullScaleAccelRange(0x02); // +- 8G range
37 | MPU.setFullScaleGyroRange(0x01); // 500degs / sec
38 |
39 | // calibration
40 | calibCount = 0;
41 | calibIntId = setInterval(calibrate, 1);
42 | }
43 |
44 | function calibrate() {
45 | if(calibCount++ === 2000) {
46 | clearInterval(calibIntId);
47 | gyroOffsetX = gyroOffsetX / 2000;
48 | gyroOffsetY = gyroOffsetY / 2000;
49 | gyroOffsetZ = gyroOffsetZ / 2000;
50 |
51 | console.log("GYRO offsets [" + gyroOffsetX + ", " + gyroOffsetY + ", " + gyroOffsetZ + "]");
52 |
53 | // now go ahead and run
54 | setInterval(pidLoop, 20);
55 | }
56 | else{
57 | var rot = MPU.getRotation();
58 | gyroOffsetX += rot[0];
59 | gyroOffsetY += rot[1];
60 | gyroOffsetZ += rot[2];
61 | }
62 | }
63 |
64 |
65 |
66 |
67 |
68 | function forward(orBack) {
69 | digitalWrite(MR_DIR, orBack);
70 | digitalWrite(ML_DIR, orBack);
71 | }
72 |
73 |
74 | function disable(orEnable) {
75 | digitalWrite(MR_ENB, orEnable);
76 | digitalWrite(ML_ENB, orEnable);
77 | }
78 |
79 | function setSpeed(s) {
80 | if(s < 0){
81 | forward(false);
82 | s = -s;
83 | }else{
84 | forward(true);
85 | }
86 |
87 | disable(s < 10);
88 |
89 | if(s > MAX_SPEED) s = MAX_SPEED;
90 | else if(s === 0) s = 1; // "freq: 0" won't work, whereas 1Hz is pretty much stopped
91 |
92 | analogWrite(MR_STP, 0.5, {freq: s});
93 | analogWrite(ML_STP, 0.5, {freq: s});
94 | }
95 |
96 |
97 | // frequency at which we'll toggle the step pin, proportional with the resulting RPM
98 | var speed = 0;
99 | var propErr = 0;
100 | var intErr = 0;
101 | var setPoint = 0;
102 |
103 | // 0.000305344 = 1 / (1000ms / 20ms interval) / 65.5
104 | var GYRO_RAW_DEGS = 1 / (1000 / 20) / 65.5;
105 | // PI radians / 180 degrees
106 | var DEGS_RAD = 3.14159 / 180;
107 |
108 | var rollAccOffset = -1.65;
109 | var pitchAccOffset = -1.7;
110 |
111 | // angles in degrees
112 | var roll = 0;
113 | var pitch = 0;
114 | var lastRoll = roll; // to calculate the deriative term
115 | function pidLoop() {
116 | lastRoll = roll;
117 |
118 | var accs = MPU.getAcceleration();
119 | var accX = accs[0];
120 | var accY = accs[1];
121 | var accZ = accs[2];
122 |
123 | var accTotalVector = Math.sqrt(accX*accX + accY*accY + accZ*accZ); // this actually should be constant = whatever raw value corresponds to 1G
124 | var rollAcc = Math.asin(accY / accTotalVector) / DEGS_RAD - rollAccOffset; // asin() gives radians, result has to be in degrees
125 | var pitchAcc = Math.asin(accX / accTotalVector) / DEGS_RAD - pitchAccOffset;
126 |
127 |
128 | var gyros = MPU.getRotation();
129 | var gyroX = gyros[0] - gyroOffsetX;
130 | var gyroY = gyros[1] - gyroOffsetY;
131 | var gyroZ = gyros[2] - gyroOffsetZ;
132 |
133 |
134 | roll += gyroX * GYRO_RAW_DEGS;
135 | pitch += gyroY * GYRO_RAW_DEGS;
136 | // sin() has to be applied on radians
137 | roll -= pitch * Math.sin(gyroZ * GYRO_RAW_DEGS * DEGS_RAD);
138 | pitch += roll * Math.sin(gyroZ * GYRO_RAW_DEGS * DEGS_RAD);
139 |
140 |
141 | // integrate accelerometre & gyro. Tiny weightto the noisy accelerometre, just enough to stop the gyro from drifting
142 | roll = 0.99 * roll + 0.01 * rollAcc;
143 | pitch = 0.99 * pitch + 0.01 * pitchAcc;
144 |
145 | propErr = roll - setPoint;
146 |
147 | intErr = KI * intErr;
148 | if(intErr > MAX_INTEGRAL_ERR) intErr = MAX_INTEGRAL_ERR;
149 | else if(intErr < -MAX_INTEGRAL_ERR) intErr = -MAX_INTEGRAL_ERR;
150 |
151 | if(Math.abs(propErr) > 30) {
152 | speed = 0;
153 | } else {
154 | speed = KP * propErr + intErr + KD * (roll - lastRoll);
155 | intErr = intErr + propErr;
156 | }
157 |
158 |
159 | console.log(roll + " - " + rollAcc + " / " + pitchAcc);
160 | //setSpeed(speed);
161 | }
162 |
163 |
164 |
165 |
--------------------------------------------------------------------------------