emc2_PCA9685_mechaSkull.ino (16631B)
1 #include <Wire.h> 2 #include <Adafruit_PWMServoDriver.h> 3 #include <CAN.h> 4 5 Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 6 7 #define SERVOMIN 125 // minimum pulse length count (out of 4096) 8 #define SERVOMAX 600 // maximum pulse length count (out of 4096) 9 10 #define CAN_TX_PIN 4 // Any GPIO pin for CAN TX 11 #define CAN_RX_PIN 5 // Any GPIO pin for CAN RX 12 #define CAN_SPEED 500E3 // Baud rate for CAN bus communication 13 14 char boardAddress = '1'; 15 16 uint8_t sg90_1 = 0; // EyeR Y 17 uint8_t sg90_2 = 1; // EyeL Y 18 uint8_t sg90_3 = 2; // EyeR X 19 uint8_t sg90_4 = 3; // EyeL X 20 21 uint8_t sg90_1b = 4; 22 uint8_t sg90_2b = 5; 23 uint8_t sg90_3b = 6; 24 uint8_t sg90_4b = 7; 25 26 uint8_t sg90_brow1 = 8; 27 uint8_t sg90_brow2 = 9; 28 29 uint8_t sg90_lip = 10; 30 uint8_t sg90_corner1 = 11; 31 uint8_t sg90_corner2 = 15; 32 33 uint8_t sg90_jaw = 13; 34 uint8_t sg90_jaw2 = 14; 35 36 int offset_lid_top_right = -3; 37 int offset_lid_top_left = 38; 38 39 int offset_lid_bottom_right = -10; 40 int offset_lid_bottom_left = 35; 41 42 int offset_right_y = 5; 43 int offset_right_x = 25; 44 45 int offset_left_y = 17; 46 int offset_left_x = -30; 47 48 int offset_brow1 = -20; 49 int offset_brow2 = 20; 50 51 int offset_lip = -22; 52 int offset_corner1 = -14; 53 int offset_corner2 = 14; 54 55 int offset_jaw = 15; 56 int offset_jaw2 = 15; 57 58 unsigned long previousTime = 0; // Previous time the eyes blinked 59 unsigned long interval = 10000; // Time interval between blinking 60 61 long servoState = 0; 62 int mouthState = 0; 63 64 int angleToPulse(int ang) { 65 int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX); // map angle of 0 to 180 to Servo min and Servo max 66 return pulse; 67 } 68 69 void move_eyes (int eye_ry, int eye_ly, int eye_rx, int eye_lx) { 70 pwm.setPWM(sg90_1, 0, angleToPulse(eye_ry + offset_right_y)); 71 pwm.setPWM(sg90_2, 0, angleToPulse(eye_ly + offset_left_y)); 72 73 pwm.setPWM(sg90_3, 0, angleToPulse(eye_rx + offset_right_x)); 74 pwm.setPWM(sg90_4, 0, angleToPulse(eye_lx + offset_left_x)); 75 } 76 77 void move_lids (int lid_tr, int lid_tl, int lid_br, int lid_bl) { 78 pwm.setPWM(sg90_1b, 0, angleToPulse(lid_tr + offset_lid_top_right)); 79 pwm.setPWM(sg90_2b, 0, angleToPulse(lid_tl + offset_lid_top_left)); 80 81 pwm.setPWM(sg90_3b, 0, angleToPulse(lid_br + offset_lid_bottom_right)); 82 pwm.setPWM(sg90_4b, 0, angleToPulse(lid_bl + offset_lid_bottom_left)); 83 } 84 85 void move_face (int brow1, int brow2, int lip, int corner1, int corner2, int jaw, int jaw2) { 86 pwm.setPWM(sg90_brow1, 0, angleToPulse(brow1 + offset_brow1)); 87 pwm.setPWM(sg90_brow2, 0, angleToPulse(brow2 + offset_brow2)); 88 89 pwm.setPWM(sg90_lip, 0, angleToPulse(lip + offset_lip)); 90 pwm.setPWM(sg90_corner1, 0, angleToPulse(corner1 + offset_corner1)); 91 pwm.setPWM(sg90_corner2, 0, angleToPulse(corner2 + offset_corner2)); 92 93 pwm.setPWM(sg90_jaw, 0, angleToPulse(jaw + offset_jaw)); 94 pwm.setPWM(sg90_jaw2, 0, angleToPulse(jaw2 + offset_jaw2)); 95 } 96 97 void setup() { 98 pwm.begin(); 99 pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates 100 101 move_eyes(90, 90, 90, 90); 102 move_lids(90, 90, 90, 90); 103 move_face(90, 90, 90, 90, 90, 90, 90); 104 105 Serial.begin(115200); 106 Serial.setTimeout(10); 107 108 CAN.setPins(CAN_RX_PIN, CAN_TX_PIN); 109 110 // Initialize CAN bus at 500 kbps 111 if (!CAN.begin(CAN_SPEED)) { 112 Serial.println("Failed to initialize CAN bus!"); 113 while (1); // Halt if initialization fails 114 } else { 115 Serial.println("CAN bus initialized!"); 116 } 117 118 // Start the I2C Bus as Master 119 Wire.begin(); 120 } 121 122 /////////////////////////////////// 123 // Facial animation sets (Start) // 124 /////////////////////////////////// 125 126 void neutral_reset () { 127 move_eyes(90, 90, 90, 90); 128 move_lids(90, 90, 90, 90); 129 move_face(90, 90, 90, 90, 90, 90, 90); 130 131 if (mouthState == 1 || mouthState == 2) { 132 move_face(90, 90, 90, 90, 90, 100, 80); 133 delay(30); 134 move_face(90, 90, 90, 90, 90, 95, 85); 135 delay(30); 136 move_face(90, 90, 90, 90, 90, 92, 88); 137 delay(30); 138 move_face(90, 90, 90, 90, 90, 88, 92); 139 delay(30); 140 } 141 move_face(90, 90, 90, 90, 90, 90, 90); 142 143 mouthState = 0; 144 } 145 146 void blink_eyes() { 147 move_lids(57, 123, 100, 80); 148 delay(100); 149 move_lids(90, 90, 90, 90); 150 move_eyes(90, 90, 90, 90); 151 } 152 153 void chatter(int rounds1=2, int rounds2=4, bool to_smile=true) { 154 for (int i=0; i<rounds1; i++) { 155 Wire.beginTransmission(0x10); 156 Wire.write(50); 157 Wire.endTransmission(); 158 159 move_eyes(95, 85, 90, 90); 160 move_lids(93, 87, 95, 85); 161 162 for (int j=0; j<rounds2; j++) { 163 164 delay(50); 165 move_face(90,90,90,90,90,97,83); 166 delay(50); 167 move_face(90,90,90,90,90,100,80); 168 delay(50); 169 move_face(90,90,90,90,90,105,75); 170 delay(50); 171 move_face(90,90,90,80,100,105,75); 172 delay(50); 173 move_face(100,80,90,80,100,105,75); 174 delay(50); 175 move_face(100,80,95,80,100,105,75); 176 delay(20); 177 move_face(100,80,105,80,100,105,75); 178 179 delay(100); 180 181 move_face(90,90,90,90,90,100,80); 182 delay(50); 183 move_face(90,90,90,90,90,95,85); 184 delay(30); 185 move_face(90,90,90,90,90,92,88); 186 delay(30); 187 move_face(90,90,90,90,90,88,92); 188 delay(30); 189 move_face(90,90,90,90,90,90,90); 190 191 delay(50); 192 } 193 Wire.beginTransmission(0x10); 194 Wire.write(51); 195 Wire.endTransmission(); 196 197 move_eyes(90,90,90,90); 198 move_lids(90,90,90,90); 199 delay(350); 200 } 201 move_lids(60,120,100,80); 202 delay(100); 203 move_lids(90,90,90,90); 204 205 int randexpress = random(2); 206 207 if (randexpress == 0){ 208 if (to_smile==true){ 209 smile(); 210 }else{ 211 move_eyes(90,90,75,110); 212 delay(1000); 213 } 214 } else if (randexpress == 1) { 215 move_eyes(75,100,110,75); 216 move_lids(80,100,75,105); 217 delay(1000); 218 } else if (randexpress == 2) { 219 move_eyes(75,100,75,110); 220 move_lids(80,100,75,105); 221 delay(1000); 222 } 223 } 224 225 void nod_head() { 226 move_lids(60,120,100,80); 227 delay(100); 228 Wire.beginTransmission(0x10); 229 Wire.write(54); 230 Wire.endTransmission(); 231 delay(300); 232 Wire.beginTransmission(0x10); 233 Wire.write(55); 234 Wire.endTransmission(); 235 delay(300); 236 move_lids(90,90,90,90); 237 move_eyes(90,90,90,90); 238 delay(1000); 239 } 240 241 void smile() { 242 move_face(90, 90, 90, 110, 70, 90, 90); 243 delay(30); 244 move_face(90, 90, 90, 120, 60, 95, 85); 245 delay(30); 246 move_face(90, 90, 90, 130, 50, 97, 83); 247 delay(30); 248 move_face(100, 80, 90, 150, 30, 97, 83); 249 move_lids(87, 93, 103, 77); 250 move_face(100, 80, 100, 150, 30, 100, 80); 251 delay(20); 252 move_face(95, 85, 100, 150, 30, 100, 80); 253 delay(2000); 254 move_face(95, 85, 100, 150, 30, 100, 80); 255 delay(30); 256 move_face(95, 85, 100, 130, 50, 97, 83); 257 delay(30); 258 move_face(95, 85, 100, 120, 60, 95, 85); 259 delay(30); 260 move_face(95, 85, 100, 110, 70, 90, 90); 261 move_face(95, 85, 100, 80, 100, 90, 90); 262 263 neutral_reset(); 264 } 265 266 void thinking_eyes() { 267 blink_eyes(); 268 delay(300); 269 move_eyes(100,80,90,90); 270 move_lids(95,85,100,80); 271 delay(200); 272 move_eyes(100,80,75,110); 273 delay(1000); 274 } 275 276 void expression_misc_a() { 277 pwm.setPWM(sg90_corner1, 0, angleToPulse(60 + offset_corner1)); 278 pwm.setPWM(sg90_corner2, 0, angleToPulse(120 + offset_corner2)); 279 pwm.setPWM(sg90_lip, 0, angleToPulse(90 + offset_lip)); 280 pwm.setPWM(sg90_1b, 0, angleToPulse(90 + offset_lid_top_right)); 281 pwm.setPWM(sg90_2b, 0, angleToPulse(90 + offset_lid_top_left)); 282 pwm.setPWM(sg90_3b, 0, angleToPulse(90 + offset_lid_bottom_right)); 283 pwm.setPWM(sg90_4b, 0, angleToPulse(90 + offset_lid_bottom_left)); 284 pwm.setPWM(sg90_brow1, 0, angleToPulse(80 + offset_brow1)); 285 pwm.setPWM(sg90_brow2, 0, angleToPulse(100 + offset_brow2)); 286 pwm.setPWM(sg90_lip, 0, angleToPulse(90 + offset_lip)); 287 pwm.setPWM(sg90_jaw, 0, angleToPulse(95 + offset_jaw)); 288 pwm.setPWM(sg90_jaw2, 0, angleToPulse(85 + offset_jaw2)); 289 } 290 291 void expression_misc_b() { 292 pwm.setPWM(sg90_corner1, 0, angleToPulse(110 + offset_corner1)); 293 pwm.setPWM(sg90_corner2, 0, angleToPulse(70 + offset_corner2)); 294 delay(30); 295 pwm.setPWM(sg90_corner1, 0, angleToPulse(130 + offset_corner1)); 296 pwm.setPWM(sg90_corner2, 0, angleToPulse(50 + offset_corner2)); 297 delay(30); 298 pwm.setPWM(sg90_corner1, 0, angleToPulse(150 + offset_corner1)); 299 pwm.setPWM(sg90_corner2, 0, angleToPulse(30 + offset_corner2)); 300 pwm.setPWM(sg90_1b, 0, angleToPulse(87 + offset_lid_top_right)); 301 pwm.setPWM(sg90_2b, 0, angleToPulse(93 + offset_lid_top_left)); 302 pwm.setPWM(sg90_3b, 0, angleToPulse(100 + offset_lid_bottom_right)); 303 pwm.setPWM(sg90_4b, 0, angleToPulse(82 + offset_lid_bottom_left)); 304 pwm.setPWM(sg90_brow1, 0, angleToPulse(100 + offset_brow1)); 305 pwm.setPWM(sg90_brow2, 0, angleToPulse(80 + offset_brow2)); 306 pwm.setPWM(sg90_lip, 0, angleToPulse(97 + offset_lip)); 307 pwm.setPWM(sg90_jaw, 0, angleToPulse(100 + offset_jaw)); 308 pwm.setPWM(sg90_jaw2, 0, angleToPulse(80 + offset_jaw2)); 309 } 310 311 void expression_misc_c() { 312 pwm.setPWM(sg90_jaw, 0, angleToPulse(100 + offset_jaw)); 313 pwm.setPWM(sg90_jaw2, 0, angleToPulse(80 + offset_jaw2)); 314 delay(30); 315 pwm.setPWM(sg90_jaw, 0, angleToPulse(105 + offset_jaw)); 316 pwm.setPWM(sg90_jaw2, 0, angleToPulse(75 + offset_jaw2)); 317 delay(30); 318 pwm.setPWM(sg90_jaw, 0, angleToPulse(110 + offset_jaw)); 319 pwm.setPWM(sg90_jaw2, 0, angleToPulse(70 + offset_jaw2)); 320 delay(50); 321 pwm.setPWM(sg90_corner1, 0, angleToPulse(80 + offset_corner1)); 322 pwm.setPWM(sg90_corner2, 0, angleToPulse(100 + offset_corner2)); 323 delay(50); 324 pwm.setPWM(sg90_brow1, 0, angleToPulse(100 + offset_brow1)); 325 pwm.setPWM(sg90_brow2, 0, angleToPulse(80 + offset_brow2)); 326 delay(50); 327 pwm.setPWM(sg90_lip, 0, angleToPulse(95 + offset_lip)); 328 mouthState = 2; 329 } 330 331 void expression_misc_d() { 332 if (mouthState == 2) { 333 pwm.setPWM(sg90_jaw, 0, angleToPulse(108 + offset_jaw)); 334 pwm.setPWM(sg90_jaw2, 0, angleToPulse(72 + offset_jaw2)); 335 delay(30); 336 pwm.setPWM(sg90_jaw, 0, angleToPulse(105 + offset_jaw)); 337 pwm.setPWM(sg90_jaw2, 0, angleToPulse(75 + offset_jaw2)); 338 delay(30); 339 pwm.setPWM(sg90_jaw, 0, angleToPulse(100 + offset_jaw)); 340 pwm.setPWM(sg90_jaw2, 0, angleToPulse(80 + offset_jaw2)); 341 } else { 342 pwm.setPWM(sg90_jaw, 0, angleToPulse(95 + offset_jaw)); 343 pwm.setPWM(sg90_jaw2, 0, angleToPulse(85 + offset_jaw2)); 344 delay(30); 345 pwm.setPWM(sg90_jaw, 0, angleToPulse(100 + offset_jaw)); 346 pwm.setPWM(sg90_jaw2, 0, angleToPulse(80 + offset_jaw2)); 347 } 348 pwm.setPWM(sg90_corner1, 0, angleToPulse(90 + offset_corner1)); 349 pwm.setPWM(sg90_corner2, 0, angleToPulse(90 + offset_corner2)); 350 pwm.setPWM(sg90_brow1, 0, angleToPulse(90 + offset_brow1)); 351 pwm.setPWM(sg90_brow2, 0, angleToPulse(90 + offset_brow2)); 352 pwm.setPWM(sg90_lip, 0, angleToPulse(90 + offset_lip)); 353 mouthState = 1; 354 } 355 356 ///////////////////////////////// 357 // Facial animation sets (End) // 358 ///////////////////////////////// 359 360 361 void loop() { 362 unsigned long currentTime = millis(); // Get the current time 363 364 // Check if the time interval has elapsed 365 if (currentTime - previousTime >= interval) { 366 previousTime = currentTime; // Update the previous time 367 368 // Generate a random variation of 5 seconds (5000 milliseconds) 369 unsigned long variation = random(5000); 370 371 // Calculate the next interval between LED state changes (10-15 seconds + random variation) 372 interval = 10000 + variation; 373 374 // Toggle the blink state 375 blink_eyes(); 376 } 377 378 if (Serial.available() > 0) { 379 // Read the message into a string 380 String receivedMessage = Serial.readStringUntil('\n'); 381 382 // Print the received message for debugging 383 Serial.println("Received message: "); 384 Serial.println(receivedMessage); 385 386 if (receivedMessage[0] == boardAddress) { 387 switch (receivedMessage[1]) { 388 case 'm': 389 390 // Handle motor commands... 391 switch (receivedMessage[2]) { 392 case '1': 393 Wire.beginTransmission(0x10); 394 Wire.write(54); 395 Wire.endTransmission(); 396 break; 397 case '2': 398 Wire.beginTransmission(0x10); 399 Wire.write(55); 400 Wire.endTransmission(); 401 break; 402 case '3': 403 Wire.beginTransmission(0x20); 404 Wire.write(50); 405 Wire.endTransmission(); 406 break; 407 case '4': 408 Wire.beginTransmission(0x20); 409 Wire.write(51); 410 Wire.endTransmission(); 411 break; 412 case '5': 413 Wire.beginTransmission(0x10); 414 Wire.write(50); 415 Wire.endTransmission(); 416 break; 417 case '6': 418 Wire.beginTransmission(0x10); 419 Wire.write(51); 420 Wire.endTransmission(); 421 break; 422 // Add more motor commands as needed... 423 default: 424 // Handle default case if needed 425 break; 426 } 427 break; 428 case 's': 429 430 // Handle servo commands... 431 switch (receivedMessage[2]) { 432 case '1': 433 move_lids(57, 123, 100, 80); 434 break; 435 case '2': 436 move_lids(90, 90, 90, 90); 437 break; 438 case '3': 439 move_lids(80, 100, 100, 80); 440 break; 441 case '4': 442 move_lids(90, 90, 90, 90); 443 break; 444 case '5': 445 move_eyes(100, 80, 90, 90); 446 move_lids(95, 85, 100, 80); 447 break; 448 case '6': 449 move_eyes(75, 100, 90, 90); 450 move_lids(80, 100, 75, 105); 451 break; 452 case '7': 453 pwm.setPWM(sg90_3, 0, angleToPulse(75 + offset_right_x)); 454 pwm.setPWM(sg90_4, 0, angleToPulse(110 + offset_left_x)); 455 break; 456 case '8': 457 pwm.setPWM(sg90_3, 0, angleToPulse(110 + offset_right_x)); 458 pwm.setPWM(sg90_4, 0, angleToPulse(75 + offset_left_x)); 459 break; 460 case '9': 461 neutral_reset(); 462 break; 463 case 'a': 464 move_lids(60, 90, 100, 90); 465 break; 466 case 'b': 467 move_lids(90, 120, 90, 80); 468 break; 469 case 'c': 470 pwm.setPWM(sg90_brow1, 0, angleToPulse(70 + offset_brow1)); 471 pwm.setPWM(sg90_brow2, 0, angleToPulse(105 + offset_brow2)); 472 break; 473 case 'd': 474 pwm.setPWM(sg90_brow1, 0, angleToPulse(105 + offset_brow1)); 475 pwm.setPWM(sg90_brow2, 0, angleToPulse(75 + offset_brow2)); 476 break; 477 case 'e': 478 pwm.setPWM(sg90_lip, 0, angleToPulse(110 + offset_lip)); 479 break; 480 case 'f': 481 pwm.setPWM(sg90_lip, 0, angleToPulse(90 + offset_lip)); 482 break; 483 case 'g': 484 expression_misc_a(); 485 break; 486 case 'h': 487 expression_misc_b(); 488 break; 489 case 'i': 490 expression_misc_c(); 491 break; 492 case 'j': 493 expression_misc_d(); 494 break;1 495 case 'k': 496 chatter(1); 497 break; 498 case 'l': 499 blink_eyes(); 500 break; 501 case 'm': 502 chatter(2); 503 break; 504 case 'n': 505 chatter(1, 2); 506 break; 507 case 'o': 508 chatter(2, 2); 509 break; 510 case 'p': 511 smile(); 512 break; 513 case 'q': 514 chatter(1, 4, false); 515 break; 516 case 'r': 517 nod_head(); 518 break; 519 case 's': 520 thinking_eyes(); 521 break; 522 default: 523 // Handle default case if needed 524 break; 525 } 526 break; 527 default: 528 Serial.println("Unknown command"); 529 break; 530 } 531 } else if (receivedMessage.startsWith("@")) { 532 // Extract the message without the '@' character 533 String messageString = receivedMessage.substring(1); 534 535 Serial.println("Sending command: "); 536 Serial.println(messageString); 537 // Convert string to byte array 538 uint8_t message[messageString.length()]; 539 messageString.getBytes(message, messageString.length() + 1); 540 // Send the message over CAN bus 541 CAN.beginPacket(0x123); // Assuming 0x123 is the ID for user commands 542 CAN.write(message, messageString.length()); 543 CAN.endPacket(); 544 } 545 } 546 }