Arduino 3D Printed self Balancing Cube

1#include "arduino_cube.h"
2#include <Wire.h>
3#include <EEPROM.h>
4
5void setup() {
6  Serial.begin(115200);
7
8  TCCR1A = 0b00000001;
9  TCCR1B = 0b00001010;
10  TCCR2B = 0b00000010;
11  TCCR2A = 0b00000011;
12
13  pinMode(DIR_1, OUTPUT);
14  pinMode(DIR_2, OUTPUT);
15  pinMode(DIR_3, OUTPUT);
16  pinMode(BRAKE, OUTPUT);
17  pinMode(BUZZER, OUTPUT);
18  
19  Motor1_control(0);
20  Motor2_control(0);
21  Motor3_control(0);
22
23  EEPROM.get(0, offsets);
24  if (offsets.ID1 == 99 && offsets.ID2 == 99 && offsets.ID3 == 99 && offsets.ID4 == 99) calibrated = true;
25    else calibrated = false;
26  delay(3000);
27  beep();
28  angle_setup();
29}
30
31void loop() {
32
33  currentT = millis();
34
35  if (currentT - previousT_1 >= loop_time) {
36    Tuning();  // derinimui
37    angle_calc();
38    
39    if (balancing_point == 1) {
40      angleX -= offsets.X1;
41      angleY -= offsets.Y1;
42      if (abs(angleX) > 8 || abs(angleY) > 8) vertical = false;
43    } else if (balancing_point == 2) {
44      angleX -= offsets.X2;
45      angleY -= offsets.Y2;
46      if (abs(angleY) > 6) vertical = false;
47    } else if (balancing_point == 3) {
48      angleX -= offsets.X3;
49      angleY -= offsets.Y3;
50      if (abs(angleY) > 6) vertical = false;
51    } else if (balancing_point == 4) {
52      angleX -= offsets.X4;
53      angleY -= offsets.Y4;
54      if (abs(angleX) > 6) vertical = false;
55    }
56 
57    if (vertical && calibrated && !calibrating) {    
58      digitalWrite(BRAKE, HIGH);
59      gyroZ = GyZ / 131.0; // Convert to deg/s
60      gyroY = GyY / 131.0; // Convert to deg/s
61
62      gyroYfilt = alpha * gyroY + (1 - alpha) * gyroYfilt;
63      gyroZfilt = alpha * gyroZ + (1 - alpha) * gyroZfilt;
64      
65      int pwm_X = constrain(pGain * angleX + iGain * gyroZfilt + sGain * motor_speed_pwmX, -255, 255); // LQR
66      int pwm_Y = constrain(pGain * angleY + iGain * gyroYfilt + sGain * motor_speed_pwmY, -255, 255); // LQR
67      motor_speed_pwmX += pwm_X; 
68      motor_speed_pwmY += pwm_Y;
69      
70      if (balancing_point == 1) {
71        XY_to_threeWay(-pwm_X, -pwm_Y);
72      } else if (balancing_point == 2) {
73        Motor1_control(-pwm_Y);
74      } else if (balancing_point == 3) {
75        Motor2_control(pwm_Y);
76      } else if (balancing_point == 4) {
77        Motor3_control(-pwm_X);
78      }
79    } else {
80      balancing_point = 0;
81      XY_to_threeWay(0, 0);
82      digitalWrite(BRAKE, LOW);
83      motor_speed_pwmX = 0;
84      motor_speed_pwmY = 0;
85    }
86    previousT_1 = currentT;
87  }
88  
89  if (currentT - previousT_2 >= 2000) {    
90    battVoltage((double)analogRead(VBAT) / bat_divider); 
91    if (!calibrated && !calibrating) {
92      Serial.println("first you need to calibrate the balancing points...");
93    }
94    previousT_2 = currentT;
95  }
96}