├── .gitattributes ├── schematic.pdf ├── pictures ├── nidec.jpg ├── stick1.jpg └── schematic.png ├── README.md ├── functions.ino └── one_axis_reaction_wheel_stick.ino /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /schematic.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/One-Axis-Reaction-Wheel-Stick/HEAD/schematic.pdf -------------------------------------------------------------------------------- /pictures/nidec.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/One-Axis-Reaction-Wheel-Stick/HEAD/pictures/nidec.jpg -------------------------------------------------------------------------------- /pictures/stick1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/One-Axis-Reaction-Wheel-Stick/HEAD/pictures/stick1.jpg -------------------------------------------------------------------------------- /pictures/schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/remrc/One-Axis-Reaction-Wheel-Stick/HEAD/pictures/schematic.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # One-Axis-Reaction-Wheel-Stick 2 | 3 | Arduino mini, Nidec 24H motors, MPU6050, 3S 500 mAh LiPo battery. 4 | 5 | Balancing controller 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 | Balancing stick pic 14 | Schematic 15 | 16 | About schematic: 17 | 18 | Battery: 3S1P LiPo (11.1V). 19 | 20 | Buzzer: any 5V active buzzer. 21 | 22 | More about this: 23 | 24 | https://youtu.be/qGbiZseBd_o 25 | 26 | This video may also help: 27 | 28 | https://youtu.be/Nkm9PoihZOI -------------------------------------------------------------------------------- /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 < 1024; i++) { 17 | angle_calc(); 18 | GyZ_offset_sum += GyZ; 19 | delay (5); 20 | } 21 | GyZ_offset = GyZ_offset_sum >> 10; 22 | digitalWrite(BUZZER, HIGH); 23 | delay(70); 24 | digitalWrite(BUZZER, LOW); 25 | delay(80); 26 | digitalWrite(BUZZER, HIGH); 27 | delay(70); 28 | digitalWrite(BUZZER, LOW); 29 | Serial.print("GyZ offset value = "); Serial.println(GyZ_offset); 30 | } 31 | 32 | void angle_calc() { 33 | 34 | Wire.beginTransmission(MPU6050); 35 | Wire.write(0x3B); 36 | Wire.endTransmission(false); 37 | Wire.requestFrom(MPU6050, 4, true); 38 | AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 39 | AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 40 | 41 | Wire.beginTransmission(MPU6050); 42 | Wire.write(0x47); 43 | Wire.endTransmission(false); 44 | Wire.requestFrom(MPU6050, 2, true); 45 | GyZ = Wire.read() << 8 | Wire.read(); 46 | 47 | AcX += AcX_offset; 48 | AcY += AcY_offset; 49 | AcZ += AcZ_offset; 50 | GyZ -= GyZ_offset; 51 | 52 | robot_angle += GyZ * loop_time / 1000 / 65.536; 53 | Acc_angle = atan2(AcY, -AcX) * 57.2958; // angle from acc. values * 57.2958 (deg/rad) 54 | robot_angle = robot_angle * Gyro_amount + Acc_angle * (1.0 - Gyro_amount); 55 | 56 | if (abs(robot_angle) > 9) vertical = false; 57 | if (abs(robot_angle) < 0.3) vertical = true; 58 | 59 | //Serial.print("Angle: "); Serial.println(robot_angle); 60 | } 61 | 62 | void battVoltage(double voltage) { 63 | //Serial.print("batt: "); Serial.println(voltage); //debug 64 | if (voltage > 8 && voltage <= 9.5) { 65 | digitalWrite(BUZZER, HIGH); 66 | } else { 67 | digitalWrite(BUZZER, LOW); 68 | } 69 | } 70 | 71 | void setPWM(uint16_t dutyCycle) { // dutyCycle is a value between 0-ICR1 72 | OCR1A = dutyCycle; 73 | } 74 | 75 | void Motor_control(int pwm) { 76 | if (pwm <= 0) { 77 | digitalWrite(DIRECTION, LOW); 78 | pwm = -pwm; 79 | } else { 80 | digitalWrite(DIRECTION, HIGH); 81 | } 82 | setPWM(map(pwm, 0, 255, PWMVALUE, 0)); 83 | } 84 | 85 | int Tuning() { 86 | if (!Serial.available()) return 0; 87 | delay(2); 88 | char param = Serial.read(); // get parameter byte 89 | if (!Serial.available()) return 0; 90 | char cmd = Serial.read(); // get command byte 91 | Serial.flush(); 92 | switch (param) { 93 | case 'p': 94 | if (cmd == '+') X1 += 1; 95 | if (cmd == '-') X1 -= 1; 96 | printValues(); 97 | break; 98 | case 'i': 99 | if (cmd == '+') X2 += 0.01; 100 | if (cmd == '-') X2 -= 0.01; 101 | printValues(); 102 | break; 103 | case 's': 104 | if (cmd == '+') X3 += 0.005; 105 | if (cmd == '-') X3 -= 0.005; 106 | printValues(); 107 | break; 108 | } 109 | } 110 | 111 | void printValues() { 112 | Serial.print("X1: "); Serial.print(X1); 113 | Serial.print(" X2: "); Serial.print(X2); 114 | Serial.print(" X3: "); Serial.println(X3, 3); 115 | } 116 | 117 | 118 | -------------------------------------------------------------------------------- /one_axis_reaction_wheel_stick.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MPU6050 0x68 // Device address 4 | #define ACCEL_CONFIG 0x1C // Accelerometer configuration address 5 | #define GYRO_CONFIG 0x1B // Gyro configuration address 6 | #define PWR_MGMT_1 0x6B 7 | #define PWR_MGMT_2 0x6C 8 | 9 | #define BRAKE 8 10 | #define PWM 9 11 | #define DIRECTION 4 12 | 13 | #define BUZZER 12 14 | #define VBAT A7 15 | 16 | const uint16_t PWM_FREQUENCY = 20000; // The motor driver can handle a PWM frequency up to 20kHz 17 | const uint16_t PWMVALUE = F_CPU / PWM_FREQUENCY / 2; // The frequency is given by F_CPU/(2*N*ICR) - where N is the prescaler, prescaling is used so the frequency is given by F_CPU/(2*ICR) - ICR = F_CPU/PWM_FREQUENCY/2 18 | 19 | float X1 = 75.0; 20 | float X2 = 5.25; 21 | float X3 = 0.04; 22 | float loop_time = 10; 23 | 24 | int pwm_s = 0; 25 | byte dir; 26 | int32_t motor_speed; 27 | uint32_t timer; 28 | long currentT, previousT_1, previousT_2 = 0; 29 | int16_t AcX, AcY, AcZ, GyZ, gyroZ; 30 | 31 | //Sensor output scaling 32 | #define accSens 0 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g 33 | #define gyroSens 1 // 0 = 250rad/s, 1 = 500rad/s, 2 1000rad/s, 3 = 2000rad/s 34 | #define Gyro_amount 0.996 // percent of gyro in complementary filter 35 | 36 | //IMU offset values 37 | int16_t AcX_offset = -750; 38 | int16_t AcY_offset = 360; 39 | int16_t AcZ_offset = 0; 40 | int16_t GyZ_offset = 0; 41 | int32_t GyZ_offset_sum = 0; 42 | 43 | float alpha = 0.40; 44 | float gyroZfilt; 45 | 46 | float robot_angle; 47 | float Acc_angle; 48 | 49 | bool vertical = false; 50 | 51 | uint8_t i2cData[14]; // Buffer for I2C data 52 | 53 | void setup() { 54 | Serial.begin(115200); 55 | 56 | // Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/doc8272.pdf page 128-135 57 | // Set up PWM, Phase and Frequency Correct on pin 9 (OC1A) & pin 10 (OC1B) with ICR1 as TOP using Timer1 58 | TCCR1B = (1 << WGM13) | (1 << CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling 59 | ICR1 = PWMVALUE; // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz 60 | 61 | // Clear OC1A/OC1B on compare match when up-counting - Set OC1A/OC1B on compare match when downcounting 62 | TCCR1A = (1 << COM1A1) | (1 << COM1B1); 63 | setPWM(400); 64 | 65 | Serial.print("PWM: "); Serial.println(PWMVALUE); Serial.print("CPU_FREQ: "); Serial.println(F_CPU); 66 | 67 | pinMode(PWM, OUTPUT); 68 | pinMode(BUZZER, OUTPUT); 69 | digitalWrite(BUZZER, LOW); 70 | pinMode(BRAKE, OUTPUT); 71 | pinMode(DIRECTION, OUTPUT); 72 | digitalWrite(BRAKE, HIGH); 73 | delay(1000); 74 | angle_setup(); 75 | } 76 | 77 | void loop() { 78 | currentT = millis(); 79 | if (currentT - previousT_1 >= loop_time) { 80 | Tuning(); 81 | angle_calc(); 82 | if (vertical) { 83 | digitalWrite(BRAKE, HIGH); 84 | gyroZ = GyZ / 131.0; // Convert to deg/s 85 | 86 | gyroZfilt = alpha * gyroZ + (1 - alpha) * gyroZfilt; 87 | pwm_s = -constrain(X1 * robot_angle + X2 * gyroZfilt + X3 * -motor_speed, -255, 255); 88 | 89 | Motor_control(pwm_s); 90 | motor_speed += pwm_s; 91 | } else { 92 | Motor_control(0); 93 | digitalWrite(BRAKE, LOW); 94 | motor_speed = 0; 95 | } 96 | previousT_1 = currentT; 97 | } 98 | if (currentT - previousT_2 >= 500) { 99 | battVoltage((double)analogRead(VBAT) / 53.7); // This is then connected to a 47k-12k voltage divider 100 | previousT_2 = currentT; 101 | } 102 | } 103 | 104 | --------------------------------------------------------------------------------