├── .gitattributes ├── pictures ├── triangle1.jpg └── triangle2.jpg ├── README.md └── ESP32_triangle ├── functions.ino └── ESP32_triangle.ino /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /pictures/triangle1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Self-Balancing-Triangle/HEAD/pictures/triangle1.jpg -------------------------------------------------------------------------------- /pictures/triangle2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Self-Balancing-Triangle/HEAD/pictures/triangle2.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Self-Balancing-Triangle 2 | 3 | ESP32 D2 controller, Simple FOC Mini BLDC driver, MT6701 magnetic encoder, EMAX 2806-100KV motor, MPU6050, 3 WS2812B leds, 3S 500 mAh LiPo battery. 4 | 5 | Balancing controller can be tuned remotely over bluetooth (but do this only if you know what you are doing). 6 | 7 | Example (change K1): 8 | 9 | Send p+ (or p+p+p+p+p+p+p+) for increase K1. 10 | 11 | Send p- (or p-p-p-p-p-p-p-) for decrease K1. 12 | 13 | The same for K2, K3, K4. Send "d", "s", "a", . 14 | 15 | Self-Balancing-Triangle 16 | Self-Balancing-Triangle 17 | 18 | 19 | If all leds are flashing red, the triangle needs to be calibrated. Connect via bluetooth to the controller. You will see a message that you need to calibrate the balancing points. 20 | Send c+ from serial monitor. This activate calibrating procedure. Set the triangle to first balancing point (green led on top). 21 | Hold still when the triangle does not fall to either side. Send c- from serial monitor. Set the triangle on second edge (green led on top). 22 | Send c- from serial monitor. Do the same with third edge. After calibrating, the triangle will begin to balance. 23 | 24 | More about this: 25 | 26 | https://youtu.be/dTYiqNcXw68 27 | 28 | You will not find all functions in this code. Triangle can balance on any edge, but cannot stand up and can't roll over to the other edge. 29 | In my video you can see that it is possible. But these functions are very difficult to tuning... Also many possible solutions how to do it. Maybe you can do this better? -------------------------------------------------------------------------------- /ESP32_triangle/functions.ino: -------------------------------------------------------------------------------- 1 | void writeTo(byte device, byte address, byte value) { 2 | Wire.beginTransmission(device); 3 | Wire.write(address); 4 | Wire.write(value); 5 | Wire.endTransmission(true); 6 | } 7 | 8 | void angle_setup() { 9 | Wire.begin(); 10 | delay (100); 11 | writeTo(MPU6050, PWR_MGMT_1, 0); 12 | writeTo(MPU6050, ACCEL_CONFIG, accSens << 3); // Specifying output scaling of accelerometer 13 | writeTo(MPU6050, GYRO_CONFIG, gyroSens << 3); // Specifying output scaling of gyroscope 14 | delay (100); 15 | 16 | for (int i = 0; i < 512; i++) { 17 | angle_calc(); 18 | GyZ_offset_sum += GyZ; 19 | delay (3); 20 | } 21 | GyZ_offset = GyZ_offset_sum >> 9; 22 | Serial.print("GyZ offset value = "); Serial.println(GyZ_offset); 23 | } 24 | 25 | void angle_calc() { 26 | 27 | Wire.beginTransmission(MPU6050); 28 | Wire.write(0x3B); 29 | Wire.endTransmission(false); 30 | Wire.requestFrom(MPU6050, 4, true); // request a total of 4 registers 31 | AcX = Wire.read() << 8 | Wire.read(); 32 | AcY = Wire.read() << 8 | Wire.read(); 33 | 34 | Wire.beginTransmission(MPU6050); 35 | Wire.write(0x47); 36 | Wire.endTransmission(false); 37 | Wire.requestFrom(MPU6050, 2, true); // request a total of 2 registers 38 | GyZ = Wire.read() << 8 | Wire.read(); 39 | 40 | AcXc = AcX - offsets.X; 41 | AcYc = AcY - offsets.Y; 42 | GyZ -= GyZ_offset; 43 | 44 | robot_angle += GyZ * loop_time / 1000.0 / 65.536; // integrate gyroscope to get angle / 65.536 (bits / (deg/sec)) 45 | Acc_angle = atan2(AcYc, -AcXc) * 57.2958; // angle from acc. values * 57.2958 (deg/rad) 46 | robot_angle = robot_angle * Gyro_amount + Acc_angle * (1.0 - Gyro_amount); 47 | } 48 | 49 | int sign(int x) { 50 | if (x > 0) return 1; 51 | if (x < 0) return -1; 52 | if (x = 0) return 0; 53 | } 54 | 55 | float controller(float p_angle, float p_vel, float s_vel, float a_vel) { 56 | float u = K1Gain * p_angle + K2Gain * p_vel + K3Gain * s_vel + K4Gain * a_vel; 57 | if (abs(u) > 70) u = sign(u) * 70; 58 | return u; 59 | } 60 | 61 | double battVoltage(double voltage) { 62 | if (voltage > 8 && voltage <= 9.5) { 63 | digitalWrite(BUZZER, HIGH); 64 | digitalWrite(INT_LED, HIGH); 65 | } else { 66 | digitalWrite(BUZZER, LOW); 67 | digitalWrite(INT_LED, LOW); 68 | } 69 | return voltage; 70 | } 71 | 72 | #if (USE_BT) 73 | int Tuning() { 74 | if (!SerialBT.available()) return 0; 75 | char param = SerialBT.read(); // get parameter byte 76 | if (!SerialBT.available()) return 0; 77 | char cmd = SerialBT.read(); // get command byte 78 | switch (param) { 79 | case 'p': 80 | if (cmd == '+') K1Gain += 0.5; 81 | if (cmd == '-') K1Gain -= 0.5; 82 | printValues(); 83 | break; 84 | case 'd': 85 | if (cmd == '+') K2Gain += 0.5; 86 | if (cmd == '-') K2Gain -= 0.5; 87 | printValues(); 88 | break; 89 | case 's': 90 | if (cmd == '+') K3Gain += 0.1; 91 | if (cmd == '-') K3Gain -= 0.1; 92 | printValues(); 93 | break; 94 | case 'a': 95 | if (cmd == '+') K4Gain += 0.01; 96 | if (cmd == '-') K4Gain -= 0.01; 97 | printValues(); 98 | break; 99 | case 'b': 100 | if (cmd == '+') bat_divider += 1; 101 | if (cmd == '-') bat_divider -= 1; 102 | printValues(); 103 | break; 104 | case 'c': 105 | if (cmd == '+' && !calibrating) { 106 | calibrating = true; 107 | SerialBT.println("Calibrating on"); 108 | SerialBT.println("set robot on first edge (greed led on top)"); 109 | } 110 | if (cmd == '-' && calibrating) { 111 | if (calibrating_step == 1) { 112 | SerialBT.print("X: "); SerialBT.print(AcX + 16384); SerialBT.print(" Y: "); SerialBT.println(AcY); 113 | if (abs(AcY) < 5000) { 114 | offsets.ID = 24; 115 | offsets.X = AcX + 16384; 116 | offsets.Y = AcY; 117 | digitalWrite(BUZZER, HIGH); 118 | delay(70); 119 | digitalWrite(BUZZER, LOW); 120 | calibrating_step = 2; 121 | SerialBT.println("set robot on left edge (greed led on top)"); 122 | } else { 123 | calibrating = false; 124 | SerialBT.println("The angle are wrong!!!"); 125 | SerialBT.println("calibrating off..."); 126 | digitalWrite(BUZZER, HIGH); 127 | delay(50); 128 | digitalWrite(BUZZER, LOW); 129 | delay(70); 130 | digitalWrite(BUZZER, HIGH); 131 | delay(50); 132 | digitalWrite(BUZZER, LOW); 133 | } 134 | } else if (calibrating_step == 2 && robot_angle < 140 && robot_angle > 100) { 135 | offsets.off1 = robot_angle; 136 | digitalWrite(BUZZER, HIGH); 137 | delay(70); 138 | digitalWrite(BUZZER, LOW); 139 | calibrating_step = 3; 140 | SerialBT.println("set robot on right edge (greed led on top)"); 141 | } else if (calibrating_step == 3 && robot_angle < -100 && robot_angle > -140) { 142 | SerialBT.println("calibrating complete."); 143 | offsets.off2 = robot_angle; 144 | EEPROM.put(0, offsets); 145 | EEPROM.commit(); 146 | calibrating = false; 147 | calibrated = true; 148 | digitalWrite(BUZZER, HIGH); 149 | delay(70); 150 | digitalWrite(BUZZER, LOW); 151 | } 152 | } 153 | break; 154 | } 155 | return 1; 156 | } 157 | 158 | void printValues() { 159 | SerialBT.print("K1: "); SerialBT.print(K1Gain); 160 | SerialBT.print(" K2: "); SerialBT.print(K2Gain); 161 | SerialBT.print(" K3: "); SerialBT.print(K3Gain); 162 | SerialBT.print(" K4: "); SerialBT.println(K4Gain); 163 | SerialBT.print("Bat_divider: "); SerialBT.println(bat_divider); 164 | } 165 | #endif 166 | -------------------------------------------------------------------------------- /ESP32_triangle/ESP32_triangle.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "BluetoothSerial.h" 4 | #include 5 | 6 | #define USE_BT 1 7 | 8 | #define BUZZER 4 9 | #define VBAT 32 10 | #define INT_LED 2 11 | 12 | #define MPU6050 0x68 // Device address 13 | #define ACCEL_CONFIG 0x1C // Accelerometer configuration address 14 | #define GYRO_CONFIG 0x1B // Gyro configuration address 15 | 16 | #define PWR_MGMT_1 0x6B 17 | #define PWR_MGMT_2 0x6C 18 | 19 | #define accSens 0 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g 20 | #define gyroSens 1 // 0 = 250rad/s, 1 = 500rad/s, 2 1000rad/s, 3 = 2000rad/s 21 | float Gyro_amount = 0.996; // percent of gyro in complementary filter 22 | float alpha = 0.3; 23 | 24 | #define EEPROM_SIZE 64 25 | 26 | #define LED_PIN 18 // ESP32 pin that connects to WS2812B 27 | #define NUM_PIXELS 3 // The number of LEDs (pixels) on WS2812B 28 | 29 | CRGB leds[NUM_PIXELS]; 30 | 31 | // encoder instance 32 | Encoder sensor = Encoder(16, 17, 1024); 33 | 34 | // Interrupt routine intialisation channel A and B callbacks 35 | void doA() { 36 | sensor.handleA(); 37 | } 38 | void doB() { 39 | sensor.handleB(); 40 | } 41 | 42 | BLDCMotor motor = BLDCMotor(7); 43 | BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 23); 44 | 45 | //float target_velocity = 0; // Enter "T+speed" in the serial monitor to make the two motors rotate in closed loop 46 | // include commander interface 47 | //Commander command = Commander(Serial); 48 | //void doMotor(char* cmd) { 49 | // command.scalar(&target_velocity, cmd); 50 | //} 51 | 52 | #if (USE_BT) 53 | BluetoothSerial SerialBT; 54 | #endif 55 | 56 | long dt, currentT, previousT_1, previousT_2, previousT_3; 57 | int bat_divider = 198; // must be tuned to measure battery voltage correctly 58 | 59 | int16_t AcX, AcY, AcZ, AcXc, AcYc, GyZ, gyroZ; 60 | 61 | struct AccOffsetsObj { 62 | int ID; 63 | int16_t X; 64 | int16_t Y; 65 | float off1; 66 | float off2; 67 | }; 68 | AccOffsetsObj offsets; 69 | 70 | int16_t GyZ_offset = 0; 71 | int32_t GyZ_offset_sum = 0; 72 | 73 | float robot_angle, angle_offset, gyroZfilt; 74 | float Acc_angle; 75 | 76 | bool vertical = false; 77 | bool calibrating = false; 78 | bool calibrated = false; 79 | int calibrating_step = 1; 80 | 81 | float robot_speed = 0, robot_position = 0; 82 | float target_voltage = 0; 83 | 84 | float K1Gain = 21.0; 85 | float K2Gain = 6.5; 86 | float K3Gain = 2.1; 87 | float K4Gain = 0.15; 88 | int loop_time = 8; 89 | 90 | int up_led, up_led_dir; 91 | int led_calibrating_step = 0; 92 | 93 | void setup() { 94 | 95 | Serial.begin(115200); 96 | #if (USE_BT) 97 | SerialBT.begin("ESP32Triangle"); // Bluetooth device name 98 | #endif 99 | 100 | EEPROM.begin(EEPROM_SIZE); 101 | EEPROM.get(0, offsets); 102 | 103 | if (offsets.ID == 24) calibrated = true; 104 | else calibrated = false; 105 | 106 | pinMode(BUZZER, OUTPUT); 107 | pinMode(INT_LED, OUTPUT); 108 | 109 | delay(500); 110 | 111 | FastLED.addLeds(leds, NUM_PIXELS); 112 | 113 | for (int i=0;i<=255;i++) { 114 | leds[0] = CRGB(i, 0, 0); 115 | leds[1] = CRGB(i, 0, 0); 116 | leds[2] = CRGB(i, 0, 0); 117 | FastLED.show(); 118 | delay(5); 119 | } 120 | delay(500); 121 | for (int i=0;i<=255;i++) { 122 | leds[0] = CRGB(0, i, 0); 123 | leds[1] = CRGB(0, i, 0); 124 | leds[2] = CRGB(0, i, 0); 125 | FastLED.show(); 126 | delay(5); 127 | } 128 | delay(500); 129 | for (int i=0;i<=255;i++) { 130 | leds[0] = CRGB(0, 0, i); 131 | leds[1] = CRGB(0, 0, i); 132 | leds[2] = CRGB(0, 0, i); 133 | FastLED.show(); 134 | delay(5); 135 | } 136 | delay(500); 137 | leds[0] = CRGB::Black; 138 | leds[1] = CRGB::Black; 139 | leds[2] = CRGB::Black; 140 | FastLED.show(); 141 | 142 | delay(500); 143 | 144 | sensor.quadrature = Quadrature::ON; 145 | sensor.init(); 146 | sensor.enableInterrupts(doA, doB); 147 | motor.linkSensor(&sensor); 148 | 149 | driver.voltage_power_supply = 12; 150 | driver.init(); 151 | 152 | motor.linkDriver(&driver); 153 | 154 | motor.foc_modulation = FOCModulationType::SpaceVectorPWM; 155 | motor.controller = MotionControlType::velocity; 156 | 157 | //PID Settings 158 | motor.PID_velocity.P = 0.3f; 159 | motor.PID_velocity.I = 1; 160 | motor.PID_velocity.D = 0; 161 | 162 | motor.voltage_limit = 12; 163 | 164 | // jerk control using voltage voltage ramp 165 | // default value is 300 volts per sec ~ 0.3V per millisecond 166 | motor.PID_velocity.output_ramp = 1000; 167 | 168 | //Speed Low-pass Filter Time Constant 169 | motor.LPF_velocity.Tf = 0.06f; 170 | 171 | //Maximum Speed Limit Settings 172 | motor.velocity_limit = 70; 173 | 174 | motor.useMonitoring(Serial); 175 | 176 | //Initialize the Motor 177 | motor.init(); 178 | 179 | //Initialize FOC 180 | motor.initFOC(); 181 | 182 | digitalWrite(INT_LED, HIGH); 183 | delay(200); 184 | digitalWrite(INT_LED, LOW); 185 | delay(500); 186 | digitalWrite(INT_LED, HIGH); 187 | delay(500); 188 | digitalWrite(INT_LED, LOW); 189 | 190 | digitalWrite(BUZZER, HIGH); 191 | delay(70); 192 | digitalWrite(BUZZER, LOW); 193 | angle_setup(); 194 | digitalWrite(BUZZER, HIGH); 195 | delay(70); 196 | digitalWrite(BUZZER, LOW); 197 | delay(70); 198 | digitalWrite(BUZZER, HIGH); 199 | delay(70); 200 | digitalWrite(BUZZER, LOW); 201 | } 202 | 203 | void loop() { 204 | 205 | currentT = millis(); 206 | 207 | if (currentT - previousT_1 >= loop_time) { 208 | angle_calc(); 209 | if (robot_angle < -90 && robot_angle > -140) 210 | angle_offset = offsets.off1; 211 | else if (robot_angle > -35 && robot_angle < 35) 212 | angle_offset = 0; 213 | else if (robot_angle > 90 && robot_angle < 140) 214 | angle_offset = offsets.off2; 215 | if (abs(robot_angle + angle_offset) > 8) vertical = false; 216 | if (abs(robot_angle + angle_offset) < 1) vertical = true; 217 | gyroZ = GyZ / 131.0; 218 | gyroZfilt = alpha * gyroZ + (1 - alpha) * gyroZfilt; 219 | #if (USE_BT) 220 | Tuning(); 221 | #endif 222 | 223 | if (vertical && calibrated && !calibrating) { 224 | target_voltage = controller(robot_angle + angle_offset, gyroZfilt, robot_speed, robot_position); 225 | robot_speed = motor.shaftVelocity(); 226 | robot_position += robot_speed / 30; 227 | robot_position = constrain(robot_position, -210, 210); 228 | motor.move(target_voltage); 229 | } else { 230 | if (abs(target_voltage) <= 0.3) target_voltage = 0; 231 | if (target_voltage > 0) target_voltage -= 0.3; 232 | if (target_voltage < 0) target_voltage += 0.3; 233 | motor.move(target_voltage); 234 | robot_position = 0; 235 | robot_speed = 0; 236 | } 237 | previousT_1 = currentT; 238 | } 239 | 240 | motor.loopFOC(); 241 | 242 | if (currentT - previousT_3 >= 20) { 243 | if (vertical && calibrated && !calibrating) { 244 | if (up_led >= 255) 245 | up_led_dir = 0; 246 | else if (up_led <= 0) 247 | up_led_dir = 1; 248 | if (up_led_dir == 1) 249 | up_led += 5; 250 | else 251 | up_led -= 5; 252 | if (abs(robot_angle) >= 0 && abs(robot_angle) <= 1) 253 | leds[1] = CRGB(up_led, 0, 0); 254 | else if (abs(robot_angle) > 1 && abs(robot_angle) <= 2.5) 255 | leds[1] = CRGB(up_led, up_led, 0); 256 | else if (abs(robot_angle) > 2.5 && abs(robot_angle) <= 8) 257 | leds[1] = CRGB(0, up_led, 0); 258 | else if (abs(robot_angle + offsets.off2) >= 0 && abs(robot_angle + offsets.off2) <= 1) 259 | leds[2] = CRGB(up_led, 0, 0); 260 | else if (abs(robot_angle + offsets.off2) > 1 && abs(robot_angle + offsets.off2) <= 2.5) 261 | leds[2] = CRGB(up_led, up_led, 0); 262 | else if (abs(robot_angle + offsets.off2) > 2.5 && abs(robot_angle + offsets.off2) <= 8) 263 | leds[2] = CRGB(0, up_led, 0); 264 | else if (abs(robot_angle + offsets.off1) >= 0 && abs(robot_angle + offsets.off1) <= 1) 265 | leds[0] = CRGB(up_led, 0, 0); 266 | else if (abs(robot_angle + offsets.off1) > 1 && abs(robot_angle + offsets.off1) <= 2.5) 267 | leds[0] = CRGB(up_led, up_led, 0); 268 | else if (abs(robot_angle + offsets.off1) > 2.5 && abs(robot_angle + offsets.off1) <= 8) 269 | leds[0] = CRGB(0, up_led, 0); 270 | FastLED.show(); 271 | } else if (!calibrated && !calibrating) { 272 | if (led_calibrating_step >= 40) { 273 | up_led_dir = 0; 274 | leds[0] = CRGB(0, 200, 0); 275 | leds[1] = CRGB(0, 200, 0); 276 | leds[2] = CRGB(0, 200, 0); 277 | FastLED.show(); 278 | } else if (led_calibrating_step <= 0) { 279 | up_led_dir = 1; 280 | leds[0] = CRGB::Black; 281 | leds[1] = CRGB::Black; 282 | leds[2] = CRGB::Black; 283 | FastLED.show(); 284 | } 285 | if (up_led_dir == 1) 286 | led_calibrating_step++; 287 | else 288 | led_calibrating_step--; 289 | } else if (calibrating) { 290 | if (calibrating_step == 1) { 291 | leds[1] = CRGB(200, 0, 0); 292 | leds[0] = CRGB::Black; 293 | leds[2] = CRGB::Black; 294 | } else if (calibrating_step == 2) { 295 | leds[2] = CRGB(200, 0, 0); 296 | leds[1] = CRGB::Black; 297 | leds[0] = CRGB::Black; 298 | } else if (calibrating_step == 3) { 299 | leds[0] = CRGB(200, 0, 0); 300 | leds[1] = CRGB::Black; 301 | leds[2] = CRGB::Black; 302 | } 303 | FastLED.show(); 304 | } else { 305 | up_led = 0; 306 | leds[0] = CRGB::Black; 307 | leds[1] = CRGB::Black; 308 | leds[2] = CRGB::Black; 309 | FastLED.show(); 310 | } 311 | previousT_3 = currentT; 312 | } 313 | 314 | if (currentT - previousT_2 >= 1500) { 315 | if (!calibrated && !calibrating) { 316 | #if (USE_BT) 317 | SerialBT.println("first you need to calibrate the balancing point..."); 318 | #endif 319 | Serial.println("first you need to calibrate the balancing point..."); 320 | } 321 | battVoltage((double)analogRead(VBAT) / bat_divider); 322 | previousT_2 = currentT; 323 | } 324 | } 325 | --------------------------------------------------------------------------------