├── .gitattributes ├── schematic.pdf ├── pictures ├── nidec.jpg ├── stick1.jpg ├── stick2.jpg └── schematic.png ├── README.md └── two_axis_reaction_wheel_stick ├── two_axis_reaction_wheel_stick.ino └── functions.ino /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /schematic.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Two-Axis-Reaction-Wheel-Stick/HEAD/schematic.pdf -------------------------------------------------------------------------------- /pictures/nidec.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Two-Axis-Reaction-Wheel-Stick/HEAD/pictures/nidec.jpg -------------------------------------------------------------------------------- /pictures/stick1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Two-Axis-Reaction-Wheel-Stick/HEAD/pictures/stick1.jpg -------------------------------------------------------------------------------- /pictures/stick2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Two-Axis-Reaction-Wheel-Stick/HEAD/pictures/stick2.jpg -------------------------------------------------------------------------------- /pictures/schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/Two-Axis-Reaction-Wheel-Stick/HEAD/pictures/schematic.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Two-Axis-Reaction-Wheel-Stick 2 | 3 | Arduino mini, Nidec 24H motors, MPU6050, 3S 500 mAh LiPo battery. 4 | 5 | Balancing controllers can be tuned remotely over bluetooth. 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 | Send i, s if you need to change K2, K3. 14 | 15 | Send c+ from serial monitor for calibrating procedure. Place the stick at the balancing point (as accurately as possible). Send c- from serial monitor. The stick will start balancing and write the offsets values ​​into the EEPROM. 16 | 17 | Balancing stick pic 18 | Balancing stick pic 19 | Schematic 20 | 21 | About schematic: 22 | 23 | Battery: 3S1P LiPo (11.1V). 24 | 25 | Buzzer: any 5V active buzzer. 26 | 27 | More about this: 28 | 29 | https://youtu.be/vXHlW2S24GQ 30 | 31 | https://youtu.be/4gS2i5fecFE 32 | 33 | This video may also help: 34 | 35 | https://youtu.be/Nkm9PoihZOI -------------------------------------------------------------------------------- /two_axis_reaction_wheel_stick/two_axis_reaction_wheel_stick.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define MPU6050 0x68 // Device address 6 | #define ACCEL_CONFIG 0x1C // Accelerometer configuration address 7 | #define GYRO_CONFIG 0x1B // Gyro configuration address 8 | #define PWR_MGMT_1 0x6B 9 | #define PWR_MGMT_2 0x6C 10 | 11 | #define BRAKE 8 12 | #define PWM_X 9 13 | #define PWM_Y 10 14 | #define DIRECTION_X 4 15 | #define DIRECTION_Y 3 16 | 17 | #define BUZZER 12 18 | #define VBAT A7 19 | 20 | float K1 = 70; 21 | float K2 = 5.15; 22 | float K3 = 0.035; 23 | float loop_time = 10; 24 | 25 | float alpha = 0.4; 26 | 27 | struct OffsetsObj { 28 | int ID; 29 | float X; 30 | float Y; 31 | }; 32 | 33 | OffsetsObj offsets; 34 | 35 | int pwm_X, pwm_Y = 0; 36 | byte brake_t = 1; // stabdis - '0 stop' 37 | 38 | int32_t motor_speed_pwmX; 39 | int32_t motor_speed_pwmY; 40 | 41 | uint32_t timer; 42 | long currentT, previousT_1, previousT_2 = 0; // laiko periodai 43 | 44 | /* IMU Data */ 45 | int16_t AcX, AcY, AcZ; 46 | int32_t GyZ, GyX, GyY, gyroZ, gyroY; 47 | 48 | //Sensor output scaling 49 | #define accSens 0 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g 50 | #define gyroSens 1 // 0 = 250rad/s, 1 = 500rad/s, 2 1000rad/s, 3 = 2000rad/s 51 | #define Gyro_amount 0.996 //percent of gyro in complementary filter 52 | 53 | //IMU offset values 54 | int16_t AcX_offset = -750; 55 | int16_t AcY_offset = 280; 56 | int16_t AcZ_offset = 100; 57 | int16_t GyZ_offset = 0; 58 | int16_t GyY_offset = 0; 59 | int32_t GyZ_offset_sum = 0; 60 | int32_t GyY_offset_sum = 0; 61 | 62 | float robot_angleX, robot_angleY; 63 | float angleX, angleY; 64 | float Acc_angleX, Acc_angleY; 65 | float gyroZfilt, gyroYfilt; 66 | 67 | bool vertical = false; 68 | bool calibrating = false; 69 | bool calibrated = false; 70 | 71 | uint8_t i2cData[14]; // Buffer for I2C data 72 | 73 | void battVoltage(double voltage) { 74 | //Serial.print("batt: "); Serial.println(voltage); 75 | if (voltage > 8 && voltage <= 9.5) { 76 | digitalWrite(BUZZER, HIGH); 77 | } else { 78 | digitalWrite(BUZZER, LOW); 79 | } 80 | } 81 | 82 | void setup() { 83 | Serial.begin(115200); 84 | pinMode(BUZZER, OUTPUT); 85 | digitalWrite(BUZZER, LOW); 86 | pinMode(BRAKE, OUTPUT); 87 | pinMode(DIRECTION_X, OUTPUT); 88 | pinMode(DIRECTION_Y, OUTPUT); 89 | InitTimersSafe(); 90 | SetPinFrequencySafe(PWM_X, 20000); 91 | SetPinFrequencySafe(PWM_Y, 20000); 92 | pwmWrite(PWM_X, 255); 93 | pwmWrite(PWM_Y, 255); 94 | digitalWrite(BRAKE, HIGH); 95 | delay(1000); 96 | EEPROM.get(0, offsets); 97 | if (offsets.ID == 11) calibrated = true; 98 | else calibrated = false; 99 | Serial.println("Calibrating gyroscope..."); 100 | angle_setup(); 101 | } 102 | 103 | void loop() { 104 | 105 | currentT = millis(); 106 | 107 | if (currentT - previousT_1 >= loop_time) { 108 | 109 | Tuning(); 110 | angle_calc(); 111 | 112 | if (vertical && calibrated) { 113 | digitalWrite(BRAKE, HIGH); 114 | gyroZ = GyZ / 131.0; // Convert to deg/s 115 | gyroY = GyY / 131.0; // Convert to deg/s 116 | 117 | gyroZfilt = alpha * gyroZ + (1 - alpha) * gyroZfilt; 118 | gyroYfilt = alpha * gyroY + (1 - alpha) * gyroYfilt; 119 | 120 | pwm_X = constrain(K1 * angleX + K2 * gyroZfilt + K3 * motor_speed_pwmX, -255, 255); 121 | pwm_Y = constrain(K1 * angleY + K2 * gyroYfilt + K3 * motor_speed_pwmY, -255, 255); 122 | 123 | if (!calibrating) { 124 | Motor_controlX(pwm_X); 125 | motor_speed_pwmX += pwm_X; 126 | Motor_controlY(pwm_Y); 127 | motor_speed_pwmY += pwm_Y; 128 | } else { 129 | Motor_controlX(0); 130 | Motor_controlY(0); 131 | } 132 | previousT_1 = currentT; 133 | } else { 134 | Motor_controlX(0); 135 | Motor_controlY(0); 136 | digitalWrite(BRAKE, LOW); 137 | motor_speed_pwmX = 0; 138 | motor_speed_pwmY = 0; 139 | } 140 | } 141 | if (currentT - previousT_2 >= 2000) { 142 | battVoltage((double)analogRead(VBAT) / 38.4); // This is then connected to a 47k-12k voltage divider 143 | if (!calibrated && !calibrating) { 144 | Serial.println("first you need to calibrate the balancing point..."); 145 | } 146 | previousT_2 = currentT; 147 | } 148 | } 149 | 150 | -------------------------------------------------------------------------------- /two_axis_reaction_wheel_stick/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 | //setup MPU6050 9 | void angle_setup() { 10 | Wire.begin(); 11 | delay (100); 12 | writeTo(MPU6050, PWR_MGMT_1, 0); 13 | writeTo(MPU6050, ACCEL_CONFIG, accSens << 3); // Specifying output scaling of accelerometer 14 | writeTo(MPU6050, GYRO_CONFIG, gyroSens << 3); // Specifying output scaling of gyroscope 15 | delay (100); 16 | 17 | // calc Y gyro offset by averaging 1024 values 18 | for (int i = 0; i < 1024; i++) { 19 | angle_calc(); 20 | GyZ_offset_sum += GyZ; 21 | delay(5); 22 | } 23 | GyZ_offset = GyZ_offset_sum >> 10; 24 | Serial.print("GyZ offset value = "); Serial.println(GyZ_offset); 25 | 26 | digitalWrite(BUZZER, HIGH); 27 | delay(70); 28 | digitalWrite(BUZZER, LOW); 29 | 30 | for (int i = 0; i < 1024; i++) { 31 | angle_calc(); 32 | GyY_offset_sum += GyY; 33 | delay(5); 34 | } 35 | GyY_offset = GyY_offset_sum >> 10; 36 | Serial.print("GyY offset value = "); Serial.println(GyY_offset); 37 | 38 | digitalWrite(BUZZER, HIGH); 39 | delay(70); 40 | digitalWrite(BUZZER, LOW); 41 | delay(80); 42 | digitalWrite(BUZZER, HIGH); 43 | delay(70); 44 | digitalWrite(BUZZER, LOW); 45 | } 46 | 47 | void angle_calc() { 48 | 49 | Wire.beginTransmission(MPU6050); 50 | Wire.write(0x43); 51 | Wire.endTransmission(false); 52 | Wire.requestFrom(MPU6050, 6, true); 53 | GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) 54 | GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) 55 | GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 56 | 57 | Wire.beginTransmission(MPU6050); 58 | Wire.write(0x3B); 59 | Wire.endTransmission(false); 60 | Wire.requestFrom(MPU6050, 6, true); 61 | AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 62 | AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 63 | AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) 64 | 65 | AcX += AcX_offset; 66 | AcY += AcY_offset; 67 | AcZ += AcZ_offset; 68 | GyZ -= GyZ_offset; 69 | GyY -= GyY_offset; 70 | 71 | robot_angleX += GyZ * loop_time / 1000 / 65.536; 72 | Acc_angleX = atan2(AcY, -AcX) * 57.2958; // angle from acc. values * 57.2958 (deg/rad) 73 | robot_angleX = robot_angleX * Gyro_amount + Acc_angleX * (1.0 - Gyro_amount); 74 | 75 | robot_angleY += GyY * loop_time / 1000 / 65.536; 76 | Acc_angleY = -atan2(AcZ, -AcX) * 57.2958; //angle from acc. values * 57.2958 (deg/rad) 77 | robot_angleY = robot_angleY * Gyro_amount + Acc_angleY * (1.0 - Gyro_amount); 78 | 79 | angleX = robot_angleX - offsets.X; 80 | angleY = robot_angleY - offsets.Y; 81 | 82 | if (abs(angleX) > 6 || abs(angleY) > 6) vertical = false; 83 | if (abs(angleX) < 0.3 && abs(angleY) < 0.3) vertical = true; 84 | 85 | //Serial.print("AngleX: "); Serial.print(angleX); Serial.print(" AngleY: "); Serial.println(angleY); 86 | } 87 | 88 | void Motor_controlX(int pwm) { 89 | if (pwm < 0) { 90 | digitalWrite(DIRECTION_X, LOW); 91 | pwm = -pwm; 92 | } else { 93 | digitalWrite(DIRECTION_X, HIGH); 94 | } 95 | pwmWrite(PWM_X, pwm > 255 ? 255 : 255 - pwm); 96 | } 97 | 98 | void Motor_controlY(int pwm) { 99 | if (pwm < 0) { 100 | digitalWrite(DIRECTION_Y, LOW); 101 | pwm = -pwm; 102 | } else { 103 | digitalWrite(DIRECTION_Y, HIGH); 104 | } 105 | pwmWrite(PWM_Y, pwm > 255 ? 255 : 255 - pwm); 106 | } 107 | 108 | int Tuning() { 109 | if (!Serial.available()) return 0; 110 | delay(2); 111 | char param = Serial.read(); // get parameter byte 112 | if (!Serial.available()) return 0; 113 | char cmd = Serial.read(); // get command byte 114 | Serial.flush(); 115 | switch (param) { 116 | case 'p': 117 | if (cmd == '+') K1 += 1; 118 | if (cmd == '-') K1 -= 1; 119 | printValues(); 120 | break; 121 | case 'i': 122 | if (cmd == '+') K2 += 0.01; 123 | if (cmd == '-') K2 -= 0.01; 124 | printValues(); 125 | break; 126 | case 's': 127 | if (cmd == '+') K3 += 0.005; 128 | if (cmd == '-') K3 -= 0.005; 129 | printValues(); 130 | break; 131 | case 'c': 132 | if (cmd == '+' && !calibrating) { 133 | calibrating = true; 134 | Serial.println("calibrating on"); 135 | } 136 | if (cmd == '-' && calibrating) { 137 | calibrated = true; 138 | calibrating = false; 139 | Serial.println("calibrating off"); 140 | Serial.print("X: "); Serial.print(robot_angleX); Serial.print(" Y: "); Serial.println(robot_angleY); 141 | if (abs(robot_angleX) < 15 && abs(robot_angleY) < 15) { 142 | offsets.ID = 11; 143 | offsets.X = robot_angleX; 144 | offsets.Y = robot_angleY; 145 | EEPROM.put(0, offsets); 146 | digitalWrite(BUZZER, HIGH); 147 | delay(70); 148 | digitalWrite(BUZZER, LOW); 149 | } else { 150 | Serial.println("The angles are wrong!!!"); 151 | digitalWrite(BUZZER, HIGH); 152 | delay(50); 153 | digitalWrite(BUZZER, LOW); 154 | delay(70); 155 | digitalWrite(BUZZER, HIGH); 156 | delay(50); 157 | digitalWrite(BUZZER, LOW); 158 | } 159 | } 160 | break; 161 | } 162 | } 163 | 164 | void printValues() { 165 | Serial.print("K1: "); Serial.print(K1); 166 | Serial.print(" K2: "); Serial.print(K2); 167 | Serial.print(" K3: "); Serial.println(K3,3); 168 | } 169 | --------------------------------------------------------------------------------