├── LICENSE ├── .gitignore ├── README.md └── main.ino /LICENSE: -------------------------------------------------------------------------------- 1 | This work is licensed under the Creative Commons Attribution-NonCommercial 4.0 International License. 2 | To view a copy of this license, visit http://creativecommons.org/licenses/by-nc/4.0/ 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Arduino compiled files 2 | *.cpp.o 3 | *.ino.cpp 4 | *.applet/ 5 | *.hex 6 | *.elf 7 | 8 | # Build folder 9 | build/ 10 | 11 | # Object files 12 | *.o 13 | *.obj 14 | 15 | # Precompiled files 16 | *.so 17 | *.dll 18 | *.dylib 19 | 20 | # Dependency files 21 | *.d 22 | 23 | # Executables 24 | *.exe 25 | *.out 26 | *.app 27 | 28 | # Logs 29 | *.log 30 | 31 | # EEPROM 32 | *.eep 33 | 34 | # Sketch backups 35 | *.bak 36 | *.tmp 37 | 38 | # DS_Store 39 | .DS_Store 40 | 41 | # VSCode settings (if you're using it) 42 | .vscode/ 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | work in progress 2 | 3 | # BABOT 4 | The open-source ball-balancing robot you build, program, and bring to life. 5 | 6 | 7 | # 🔧 Features 8 | 1. Ball-Balancing: Utilizes a PID controller to maintain balance a ball on its plate. 9 | 2. Arduino-Based: Compatible with Arduino IDE for easy customization and programming. 10 | 3. Open-Source: Fully documented hardware and software for community collaboration. 11 | 12 | # 🚀 Getting Started 13 | Clone the Repository 14 | 15 | # 🤝 Contributing 16 | We welcome contributions! Please fork the repository, make your changes, and submit a pull request. For major changes, open an issue first to discuss what you would like to change. 17 | 18 | # 🧾 License 19 | This project is licensed under the Creative Commons Attribution-NonCommercial 4.0 International License. 20 | 21 | # 📞 Contact 22 | For inquiries or support, please contact: info@ba-bot.com 23 | -------------------------------------------------------------------------------- /main.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // ---- PID Coefficients ---- 6 | const float P_GAIN = 3; 7 | const float I_GAIN = 0.1; 8 | const float D_GAIN = 40; 9 | 10 | 11 | // ---- Mechanical Constants ---- 12 | const float DEG2RAD = M_PI / 180.0; 13 | const float RAD2DEG = 180.0 / M_PI; 14 | const float R1 = 50.0; // Servo arm length [mm] 15 | const float R2 = 39.2; // Passive link length [mm] 16 | const float BASE_R = 32.9 / sqrt(3.0); // Base triangle radius [mm] 17 | const float PLAT_R = 107.9 / sqrt(3.0); // Platform triangle radius [mm] 18 | const float PLATE_HEIGHT = 60; // TOP PCB height [mm] 19 | 20 | // ---- Pin Assignments ---- 21 | // Control 22 | const int BUTTON_PIN = A1; 23 | const int LED_PIN = 8; 24 | 25 | // IR Sensor 26 | const int IR_LED_PIN = 7; 27 | const int IR_RECEIVER_PIN = A0; 28 | 29 | // Digital Potentiometer (MCP42xx) 30 | const int DIGIPOT_CS = 4; 31 | const int DIGIPOT_DIN = 1; 32 | const int DIGIPOT_SCLK = 0; 33 | 34 | // Servo pins 35 | const int SERVO_PIN_A = 10; 36 | const int SERVO_PIN_B = 9; 37 | const int SERVO_PIN_C = 11; 38 | 39 | // ---- Smoothing Factors ---- 40 | const float IR_ALPHA = 0.8; // IR signal low-pass filter 41 | const float alpha = 0.7; // Smoothing factor 42 | 43 | // ---- Globals ---- 44 | // IR measurements and center tracking 45 | int ambientLight[16] = { 0 }; 46 | int irLight[16] = { 0 }; 47 | float irSignal[16] = { 0.0 }; 48 | 49 | float centerX = 0.0; 50 | float centerY = 0.0; 51 | float setpointX = 0.0; 52 | float setpointY = 0.0; 53 | 54 | // PID state 55 | float lastErrorX = 0.0; 56 | float lastErrorY = 0.0; 57 | float integralX = 0.0; 58 | float integralY = 0.0; 59 | 60 | float outputX = 0, outputY = 0; // OUTPUTS OF PID 61 | 62 | // Ball tracking 63 | bool ballWasOnPlate = false; 64 | unsigned long ballLostTime = 0; 65 | unsigned long ballFoundTime = 0; 66 | 67 | // BaBot Modes 68 | enum BaBotMode { 69 | ON, 70 | OFF, 71 | ASSEMBLY 72 | }; 73 | BaBotMode mode = ON; // Initial state 74 | 75 | // === ENUM FOR PRESS TYPES === 76 | enum ButtonPress { 77 | NO_PRESS, 78 | SINGLE_PRESS, 79 | DOUBLE_PRESS, 80 | LONG_PRESS 81 | }; 82 | 83 | 84 | // === TIMING CONFIG === 85 | const unsigned long DEBOUNCE_TIME = 50; 86 | const unsigned long DOUBLE_PRESS_GAP = 500; 87 | const unsigned long LONG_PRESS_TIME = 1000; 88 | 89 | // === STATE VARIABLES === 90 | bool buttonWasPressed = false; 91 | unsigned long buttonDownTime = 0; 92 | unsigned long lastPressTime = 0; 93 | int pressCount = 0; 94 | 95 | // OptimalPot 96 | int optimalPot; 97 | 98 | // ---- Objects ---- 99 | CD74HC4067 mux(5, 13, 6, 12); // S0,S1 -> 13, S2->6, S3->12 100 | Servo servoA, servoB, servoC; 101 | 102 | 103 | // ---- Arduino Setup ---- 104 | void setup() { 105 | pinMode(BUTTON_PIN, INPUT); 106 | pinMode(LED_PIN, OUTPUT); 107 | pinMode(IR_LED_PIN, OUTPUT); 108 | pinMode(DIGIPOT_CS, OUTPUT); 109 | pinMode(DIGIPOT_DIN, OUTPUT); 110 | pinMode(DIGIPOT_SCLK, OUTPUT); 111 | digitalWrite(DIGIPOT_CS, HIGH); 112 | 113 | servoA.attach(SERVO_PIN_A); 114 | servoB.attach(SERVO_PIN_B); 115 | servoC.attach(SERVO_PIN_C); 116 | 117 | // Initialize platform to neutral 118 | movePlatform(0, 0, PLATE_HEIGHT); 119 | 120 | Serial.begin(115200); 121 | delay(1000); 122 | } 123 | 124 | int skipCounter = 0; 125 | // ---- Main Loop ---- 126 | void loop() { 127 | static unsigned long lastTime = 0; 128 | unsigned long now = millis(); 129 | 130 | measureIR(); 131 | 132 | setDigitalPot(235); //255=0kOhm 0=100kOhm 133 | 134 | float rawX, rawY; 135 | calculateWeightedCenter(irSignal, rawX, rawY); 136 | // Apply Exponential Moving Average 137 | centerX = alpha * rawX + (1 - alpha) * centerX; 138 | centerY = alpha * rawY + (1 - alpha) * centerY; 139 | 140 | // sendSerialData(); 141 | 142 | ButtonPress result = checkButton(); 143 | 144 | switch (result) { 145 | case SINGLE_PRESS: 146 | // Serial.println("Single Press"); 147 | if (mode == OFF) { 148 | mode = ON; 149 | } else if (mode == ON) { 150 | mode = OFF; 151 | } 152 | break; 153 | case DOUBLE_PRESS: 154 | // Serial.println("Double Press"); 155 | break; 156 | case LONG_PRESS: 157 | if (mode != ASSEMBLY) { 158 | mode = ASSEMBLY; 159 | } else if (mode == ASSEMBLY) { 160 | mode = OFF; 161 | } 162 | // Serial.println("Long Press"); 163 | break; 164 | default: 165 | break; 166 | } 167 | 168 | switch (mode) { 169 | case ON: 170 | blinkLED(300); 171 | if (ballOnPlate()) { 172 | digitalWrite(LED_PIN, HIGH); //Turn on light 173 | if (ballWasOnPlate == false) { 174 | ballFoundTime = millis(); 175 | skipCounter = 2; // Ignore next 10 cycles 176 | lastErrorX = setpointX - centerX; 177 | lastErrorY = setpointY - centerY; 178 | } 179 | ballWasOnPlate = true; 180 | ballLostTime = millis(); // Reset the timer since the ball is detected 181 | if (skipCounter > 0) { 182 | skipCounter--; 183 | return; // Skip rest of loop 184 | } 185 | 186 | // PID 187 | pidControl(centerX, setpointX, lastErrorX, integralX, outputX); 188 | pidControl(centerY, setpointY, lastErrorY, integralY, outputY); 189 | 190 | movePlatform(outputX, outputY, PLATE_HEIGHT); 191 | 192 | } else { 193 | if (ballWasOnPlate && millis() - ballLostTime < 1000) { 194 | // hold last 195 | movePlatform(outputX, outputY, PLATE_HEIGHT); 196 | } 197 | else { 198 | ballWasOnPlate = false; 199 | integralX = integralY = 0; 200 | lastErrorX = lastErrorY = 0; 201 | setpointX = setpointY = 0; 202 | movePlatform(0, 0, PLATE_HEIGHT); 203 | } 204 | } 205 | break; 206 | 207 | case OFF: 208 | digitalWrite(LED_PIN, LOW); //Turn off light 209 | movePlatform(0, 0, PLATE_HEIGHT); 210 | break; 211 | 212 | case ASSEMBLY: 213 | blinkLED(50); 214 | moveServos(0, 0, 0); 215 | break; 216 | 217 | default: 218 | break; 219 | } 220 | } 221 | 222 | // ---- Utility Functions ---- 223 | 224 | void blinkLED(unsigned long interval) { 225 | static unsigned long lastToggle = 0; 226 | if (millis() - lastToggle >= interval) { 227 | digitalWrite(LED_PIN, !digitalRead(LED_PIN)); 228 | lastToggle = millis(); 229 | } 230 | } 231 | 232 | void setDigitalPot(byte val) { 233 | digitalWrite(DIGIPOT_CS, LOW); 234 | for (int i = 7; i >= 0; --i) { 235 | digitalWrite(DIGIPOT_DIN, (val & (1 << i)) ? HIGH : LOW); 236 | digitalWrite(DIGIPOT_SCLK, LOW); 237 | delayMicroseconds(10); 238 | digitalWrite(DIGIPOT_SCLK, HIGH); 239 | delayMicroseconds(10); 240 | } 241 | digitalWrite(DIGIPOT_CS, HIGH); 242 | } 243 | 244 | void measureIR() { 245 | // Ambient 246 | digitalWrite(IR_LED_PIN, LOW); 247 | delay(1); 248 | for (int i = 0; i < 16; i++) { 249 | mux.channel(i); 250 | delayMicroseconds(400); 251 | ambientLight[i] = analogRead(IR_RECEIVER_PIN); 252 | } 253 | // IR On 254 | digitalWrite(IR_LED_PIN, HIGH); 255 | delay(4); 256 | for (int i = 0; i < 16; i++) { 257 | mux.channel(i); 258 | delayMicroseconds(400); 259 | irLight[i] = analogRead(IR_RECEIVER_PIN); 260 | } 261 | // Compute signal 262 | for (int i = 0; i < 16; i++) { 263 | float delta = irLight[i] - ambientLight[i]; 264 | irSignal[i] = IR_ALPHA * delta + (1 - IR_ALPHA) * irSignal[i]; 265 | } 266 | 267 | Serial.print("irLight "); 268 | Serial.println(irLight[0]); 269 | Serial.print("ambientLight "); 270 | Serial.println(ambientLight[0]); 271 | } 272 | 273 | const float BALL_THRESHOLD = 200.0; 274 | bool ballOnPlate() { 275 | float minV = irSignal[0], maxV = irSignal[0]; 276 | for (int i = 1; i < 16; i++) { 277 | minV = min(minV, irSignal[i]); 278 | maxV = max(maxV, irSignal[i]); 279 | } 280 | return (maxV - minV >= BALL_THRESHOLD); 281 | } 282 | 283 | void pidControl(float input, float target, float &lastErr, float &integ, float &out) { 284 | float error = target - input; 285 | integ += I_GAIN * error; 286 | float deriv = D_GAIN * (error - lastErr); 287 | out = P_GAIN * error + integ + deriv; 288 | lastErr = error; 289 | } 290 | 291 | void movePlatform(float rollDeg, float pitchDeg, float height) { 292 | float roll = -rollDeg * DEG2RAD; 293 | float pitch = -pitchDeg * DEG2RAD; 294 | float baseAngle[3] = { 0, 120 * DEG2RAD, 240 * DEG2RAD }; 295 | float platX[3], platY[3], platZ[3], angles[3]; 296 | 297 | // Transform platform points 298 | for (int i = 0; i < 3; i++) { 299 | float a = baseAngle[i]; 300 | float px = PLAT_R * cos(a); 301 | float py = PLAT_R * sin(a); 302 | float pz = height; 303 | 304 | // Pitch 305 | float x1 = px * cos(pitch) + pz * sin(pitch); 306 | float z1 = -px * sin(pitch) + pz * cos(pitch); 307 | // Roll 308 | float y1 = py * cos(roll) - z1 * sin(roll); 309 | float z2 = py * sin(roll) + z1 * cos(roll); 310 | 311 | platX[i] = x1; 312 | platY[i] = y1; 313 | platZ[i] = z2; 314 | } 315 | // Calculate servo angles 316 | for (int i = 0; i < 3; i++) { 317 | float a = baseAngle[i]; 318 | float bx = BASE_R * cos(a); 319 | float by = BASE_R * sin(a); 320 | float dx = platX[i] - bx; 321 | float dy = platY[i] - by; 322 | float dz = platZ[i]; 323 | float dxl = dx * cos(a) + dy * sin(a); 324 | float dyl = dz; 325 | float d = sqrt(dxl * dxl + dyl * dyl); 326 | float theta = atan2(dyl, dxl) - acos(constrain((R1 * R1 + d * d - R2 * R2) / (2 * R1 * d), -1, 1)); 327 | angles[i] = theta * RAD2DEG; 328 | } 329 | moveServos(angles[0], angles[1], angles[2]); // ordre a changer pour respecter pcb 330 | } 331 | 332 | void moveServos(float a, float b, float c) { 333 | a = constrain(a, -10, 65); 334 | b = constrain(b, -10, 65); 335 | c = constrain(c, -10, 65); 336 | servoA.write(100 - a); // BABOT blanc PCBWAY 337 | servoB.write(100 - b); 338 | servoC.write(100 - c); 339 | } 340 | 341 | void calculateWeightedCenter(const float arr[], float &x, float &y) { 342 | // If insufficient contrast, return (0,0) 343 | float minV = arr[0], maxV = arr[0]; 344 | for (int i = 1; i < 16; i++) { 345 | minV = min(minV, arr[i]); 346 | maxV = max(maxV, arr[i]); 347 | } 348 | 349 | if(!ballOnPlate()){ 350 | x = y = 0; 351 | return; 352 | } 353 | 354 | const float coordsX[16] = { 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3 }; 355 | const float coordsY[16] = { 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3 }; 356 | float sumW = 0, wx = 0, wy = 0; 357 | for (int i = 0; i < 16; i++) { 358 | float norm = pow((arr[i] - minV) / (maxV - minV), 4); 359 | wx += coordsX[i] * norm; 360 | wy += coordsY[i] * norm; 361 | sumW += norm; 362 | } 363 | x = wx / sumW - 1.5; 364 | y = wy / sumW - 1.5; 365 | } 366 | 367 | 368 | void sendSerialData() { 369 | for (int i = 0; i < 16; i++) { 370 | Serial.print(irSignal[i]); 371 | Serial.print(','); 372 | } 373 | Serial.print(centerX); 374 | Serial.print(','); 375 | Serial.print(centerY); 376 | Serial.print(','); 377 | Serial.print(setpointX); 378 | Serial.print(','); 379 | Serial.println(setpointY); 380 | } 381 | 382 | 383 | // === BUTTON CHECK FUNCTION === 384 | ButtonPress checkButton() { 385 | static bool lastButtonState = LOW; 386 | bool currentButtonState = digitalRead(BUTTON_PIN); 387 | unsigned long now = millis(); 388 | 389 | ButtonPress result = NO_PRESS; 390 | 391 | // === DETECT PRESS === 392 | if (currentButtonState == HIGH && lastButtonState == LOW) { 393 | // Button just pressed 394 | buttonDownTime = now; 395 | buttonWasPressed = true; 396 | } 397 | 398 | // === DETECT RELEASE === 399 | if (currentButtonState == LOW && lastButtonState == HIGH) { 400 | unsigned long pressDuration = now - buttonDownTime; 401 | buttonWasPressed = false; 402 | 403 | if (pressDuration >= LONG_PRESS_TIME) { 404 | result = LONG_PRESS; 405 | pressCount = 0; 406 | } else { 407 | pressCount++; 408 | lastPressTime = now; 409 | } 410 | } 411 | 412 | // === HANDLE SINGLE/DOUBLE === 413 | if (pressCount > 0 && (now - lastPressTime > DOUBLE_PRESS_GAP)) { 414 | if (pressCount == 1) { 415 | result = SINGLE_PRESS; 416 | } else if (pressCount == 2) { 417 | result = DOUBLE_PRESS; 418 | } 419 | pressCount = 0; 420 | } 421 | 422 | lastButtonState = currentButtonState; 423 | return result; 424 | } 425 | --------------------------------------------------------------------------------