├── .gitattributes
├── README.md
├── bluetooth.ino
├── pictures
└── robot0.jpg
└── two_wheel_balancing_robot.ino
/.gitattributes:
--------------------------------------------------------------------------------
1 | # Auto detect text files and perform LF normalization
2 | * text=auto
3 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Two-Wheel-Balancing-Robot
2 |
3 | Gimbal controller BGC 3.1 (use as Arduino), gimbal motors GBM2804H-100T, MPU6050, magnetic encoders AS5600 connected over analog inputs.
4 |
5 | I use the Simple FOC library to control the BLDC motors.
6 |
7 | For remote control I use Joy BT Commander.
8 |
9 | Balancing controller can be tuned remotely over bluetooth.
10 |
11 | Example:
12 |
13 | Send p+ (or p+p+p+p+p+p+p+) for increase K1.
14 |
15 | Send p- (or p-p-p-p-p-p-p-) for decrease K1.
16 |
17 | The same for K2, K3, K4. Send "s", "d", "a".
18 |
19 | 
20 |
21 | More about this:
22 |
23 | https://youtu.be/p3CFaL55U08
24 |
--------------------------------------------------------------------------------
/bluetooth.ino:
--------------------------------------------------------------------------------
1 | void bluetooth() {
2 | if (Serial.available()) { // data received from smartphone
3 | delay(2);
4 | cmd[0] = Serial.read();
5 | if (cmd[0] == STX) {
6 | int i = 1;
7 | while (Serial.available()) {
8 | cmd[i] = Serial.read();
9 | if (cmd[i] > 127 || i > 7) break; // Communication error
10 | if ((cmd[i] == ETX) && (i == 2 || i == 7)) break; // Button or Joystick data
11 | i++;
12 | }
13 | if (i == 2) getButtonState(cmd[1]); // 3 Bytes ex: < STX "C" ETX >
14 | else if (i == 7) getJoystickState(cmd); // 6 Bytes ex: < STX "200" "180" ETX >
15 | }
16 | }
17 | }
18 |
19 | String getButtonStatusString() {
20 | String bStatus = "";
21 | for (int i = 0; i < 6; i++) {
22 | if (buttonStatus & (B100000 >> i)) bStatus += "1";
23 | else bStatus += "0";
24 | }
25 | return bStatus;
26 | }
27 |
28 | void getJoystickState(byte data[8]) {
29 | joyX = (data[1] - 48) * 100 + (data[2] - 48) * 10 + (data[3] - 48); // obtain the Int from the ASCII representation
30 | joyY = (data[4] - 48) * 100 + (data[5] - 48) * 10 + (data[6] - 48);
31 | joyX = joyX - 200; // Offset to avoid
32 | joyY = joyY - 200; // transmitting negative numbers
33 |
34 | if (joyX < -100 || joyX > 100 || joyY < -100 || joyY > 100) // commmunication error
35 | return;
36 | if (joyY < -10 || joyY > 10)
37 | move_Speed = 0.015 * resp_rate * joyY;
38 | else
39 | move_Speed = 0;
40 | if (joyX < -10 || joyX > 10)
41 | rot_Speed = - 0.015 * joyX;
42 | else
43 | rot_Speed = 0;
44 | }
45 |
46 | void getButtonState(int bStatus) {
47 | switch (bStatus) {
48 | // ----------------- BUTTON #1 -----------------------
49 | case 'A':
50 | buttonStatus |= B000001; // ON
51 | //power = true;
52 | break;
53 | case 'B':
54 | buttonStatus &= B111110; // OFF
55 | //power = false;
56 | break;
57 |
58 | // ----------------- BUTTON #2 -----------------------
59 | case 'C':
60 | buttonStatus |= B000010; // ON
61 | resp_rate = 2;
62 | break;
63 | case 'D':
64 | buttonStatus &= B111101; // OFF
65 | resp_rate = 1;
66 | break;
67 |
68 | }
69 | }
70 |
71 |
72 |
--------------------------------------------------------------------------------
/pictures/robot0.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/remrc/Two-Wheel-Balancing-Robot/5e058214e4ea511df3a0a64dd468a1e9ef916b93/pictures/robot0.jpg
--------------------------------------------------------------------------------
/two_wheel_balancing_robot.ino:
--------------------------------------------------------------------------------
1 | #include // ver. 1.61
2 |
3 | BLDCMotor motorL = BLDCMotor(3, 5, 6, 7);
4 | MagneticSensorAnalog sensorL = MagneticSensorAnalog(A0, 14, 1020);
5 |
6 | BLDCMotor motorR = BLDCMotor(9, 10, 11, 7);
7 | MagneticSensorAnalog sensorR = MagneticSensorAnalog(A1, 14, 1020);
8 |
9 | #define MPU6050 0x68 // Device address
10 | #define ACCEL_CONFIG 0x1C // Accelerometer configuration address
11 | #define GYRO_CONFIG 0x1B // Gyro configuration address
12 |
13 | #define PWR_MGMT_1 0x6B
14 | #define PWR_MGMT_2 0x6C
15 |
16 | //Sensor output scaling
17 | #define accSens 0 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g
18 | #define gyroSens 1 // 0 = 250rad/s, 1 = 500rad/s, 2 1000rad/s, 3 = 2000rad/s
19 |
20 | #define STX 0x02
21 | #define ETX 0x03
22 | int16_t joyY, joyX; // smartphone joystick input values
23 | uint8_t cmd[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // bytes received
24 | uint8_t buttonStatus = 0; // first Byte sent to Android device
25 | uint8_t resp_rate = 1;
26 |
27 | int16_t AcX, AcZ, GyY, gyroY;
28 |
29 | //IMU offset values
30 | int16_t AcX_offset = -60;
31 | int16_t AcZ_offset = 300;
32 | int16_t GyY_offset = 0;
33 | int32_t GyY_offset_sum = 0;
34 |
35 | float robot_angle;
36 | float Acc_angle; // angle calculated from acc. measurments
37 |
38 | bool vertical = false;
39 | bool test = true; // motor test
40 |
41 | float rot_Speed = 0;
42 | float move_Speed = 0;
43 | float robot_speed = 0, robot_position = 0;
44 |
45 | float K1 = 0.44;
46 | float K2 = 0.036;
47 | float K3 = 0.25;
48 | float K4 = 0.20;
49 |
50 | long loop_count = 0, loop2_count = 0;
51 | int loop_time = 5;
52 |
53 | #define Gyro_amount 0.996 // percent of gyro in complementary filter
54 |
55 | uint32_t timer;
56 |
57 | void setup() {
58 |
59 | Serial.begin(115200);
60 | delay(1000);
61 |
62 | sensorL.init(); // initialise magnetic sensor hardware
63 | motorL.linkSensor(&sensorL); // link the motor to the sensor
64 |
65 | sensorR.init(); // initialise magnetic sensor hardware
66 | motorR.linkSensor(&sensorR); // link the motor to the sensor
67 |
68 | Serial.println("Calibrating gyro...");
69 | angle_setup();
70 |
71 | motorL.voltage_power_supply = 12;
72 | motorL.controller = ControlType::voltage; // set control loop type to be used
73 | motorL.voltage_sensor_align = 4; // aligning voltage
74 | motorL.foc_modulation = FOCModulationType::SpaceVectorPWM; // choose FOC modulation (optional)
75 | motorL.init(); // initialize motor
76 | motorL.initFOC(); // align encoder and start FOC
77 |
78 | motorR.voltage_power_supply = 12;
79 | motorR.controller = ControlType::voltage; // set control loop type to be used
80 | motorR.voltage_sensor_align = 4; // aligning voltage
81 | motorR.foc_modulation = FOCModulationType::SpaceVectorPWM; // choose FOC modulation (optional)
82 | motorR.init(); // initialize motor
83 | motorR.initFOC(); // align encoder and start FOC
84 | }
85 |
86 | void loop() {
87 | // ~1ms
88 | motorL.loopFOC();
89 | motorR.loopFOC();
90 |
91 | if (test) {
92 | motorL.move(5 * (motorR.shaft_angle - motorL.shaft_angle));
93 | motorR.move(5 * (motorL.shaft_angle - motorR.shaft_angle));
94 | } else {
95 | //Tuning();
96 | bluetooth();
97 | angle_calc();
98 | gyroY = GyY / 131.0; // Convert to deg/s
99 |
100 | if (loop_count++ > loop_time) {
101 | float target_voltage = 0;
102 | if (vertical) {
103 | target_voltage = controlleR(robot_angle, gyroY, robot_speed, robot_position);
104 | robot_speed = (motorL.shaftVelocity() + motorR.shaftVelocity()) / 2;
105 | robot_position += (motorL.shaftVelocity() + motorR.shaftVelocity()) / 20 - move_Speed;
106 | robot_position = constrain(robot_position, -30, 30);
107 | Serial.println(robot_position);
108 | motorL.move(target_voltage + rot_Speed);
109 | motorR.move(target_voltage - rot_Speed);
110 | } else {
111 | motorL.move(target_voltage);
112 | motorR.move(target_voltage);
113 | robot_position = 0;
114 | }
115 | loop_count = 0;
116 | }
117 | }
118 | if (loop2_count++ > 8000) {
119 | test = false;
120 | loop2_count = 0;
121 | }
122 | }
123 |
124 | int sign(int x) {
125 | if (x > 0) return 1;
126 | if (x < 0) return -1;
127 | if (x = 0) return 0;
128 | }
129 |
130 | // function constraining the angle in between -pi and pi, in degrees -180 and 180
131 | float constrainAngle(float x){
132 | x = fmod(x + M_PI, _2PI);
133 | if (x < 0)
134 | x += _2PI;
135 | return x - M_PI;
136 | }
137 |
138 | float controlleR(float p_angle, float p_vel, float s_vel, float a_vel) {
139 | float u = K1 * p_angle + K2 * p_vel + K3 * s_vel + K4 * a_vel;
140 | if (abs(u) > 12 * 0.8)
141 | u = sign(u) * 12 * 0.8;
142 | return u;
143 | }
144 |
145 | void writeTo(byte device, byte address, byte value) {
146 | Wire.beginTransmission(device);
147 | Wire.write(address);
148 | Wire.write(value);
149 | Wire.endTransmission(true);
150 | }
151 |
152 | void angle_setup() {
153 | Wire.begin();
154 | delay(100);
155 | writeTo(MPU6050, PWR_MGMT_1, 0);
156 | writeTo(MPU6050, ACCEL_CONFIG, accSens << 3); // Specifying output scaling of accelerometer
157 | writeTo(MPU6050, GYRO_CONFIG, gyroSens << 3); // Specifying output scaling of gyroscope
158 | delay(100);
159 |
160 | for (int i = 0; i < 1024; i++) {
161 | angle_calc();
162 | GyY_offset_sum += GyY;
163 | delay(3);
164 | }
165 | GyY_offset = GyY_offset_sum >> 10;
166 | Serial.print("GyY offset value = "); Serial.println(GyY_offset);
167 | }
168 |
169 | void angle_calc() {
170 |
171 | // read raw accel/gyro measurements from device
172 | Wire.beginTransmission(MPU6050);
173 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
174 | Wire.endTransmission(false);
175 | Wire.requestFrom(MPU6050, 2, true); // request a total of 2 registers
176 | AcX = Wire.read() << 8 | Wire.read();
177 | //Serial.println(AcX);
178 | Wire.beginTransmission(MPU6050);
179 | Wire.write(0x3F);
180 | Wire.endTransmission(false);
181 | Wire.requestFrom(MPU6050, 2, true); // request a total of 2 registers
182 | AcZ = Wire.read() << 8 | Wire.read();
183 | //Serial.println(AcZ);
184 | Wire.beginTransmission(MPU6050);
185 | Wire.write(0x45);
186 | Wire.endTransmission(false);
187 | Wire.requestFrom(MPU6050, 2, true); // request a total of 2 registers
188 | GyY = Wire.read() << 8 | Wire.read();
189 |
190 | AcX += AcX_offset;
191 | AcZ += AcZ_offset;
192 | GyY += GyY_offset;
193 |
194 | robot_angle += GyY * 6.07968E-5;
195 | Acc_angle = atan2(-AcX, AcZ) * 57.2958;
196 | robot_angle = robot_angle * Gyro_amount + Acc_angle * (1.0 - Gyro_amount);
197 |
198 | if (abs(robot_angle) > 40) vertical = false;
199 | if (abs(robot_angle) < 1) vertical = true;
200 |
201 | //Serial.println(robot_angle);
202 | }
203 |
204 | int Tuning() {
205 | if (!Serial.available()) return 0;
206 | delay(2);
207 | char param = Serial.read(); // get parameter byte
208 | if (!Serial.available()) return 0;
209 | char cmd = Serial.read(); // get command byte
210 | Serial.flush();
211 | switch (param) {
212 | case 'p':
213 | if (cmd == '+') K1 += 0.02;
214 | if (cmd == '-') K1 -= 0.02;
215 | printValues();
216 | break;
217 | case 'd':
218 | if (cmd == '+') K2 += 0.002;
219 | if (cmd == '-') K2 -= 0.002;
220 | printValues();
221 | break;
222 | case 's':
223 | if (cmd == '+') K3 += 0.01;
224 | if (cmd == '-') K3 -= 0.01;
225 | printValues();
226 | break;
227 | case 'a':
228 | if (cmd == '+') K4 += 0.01;
229 | if (cmd == '-') K4 -= 0.01;
230 | printValues();
231 | break;
232 | }
233 | }
234 |
235 | void printValues() {
236 | Serial.print("K1: "); Serial.print(K1);
237 | Serial.print(" K2: "); Serial.print(K2,3);
238 | Serial.print(" K3: "); Serial.print(K4);
239 | Serial.print(" K4: "); Serial.println(K3);
240 | }
241 |
242 |
--------------------------------------------------------------------------------