#include #define HOLE_MAX 3 #define STEP_MAX 72 Servo servo_0; // hand Servo servo_1; // arm1 Servo servo_2; // arm2 Servo servo_3; // pan int timeset = 50; // deley time int motion_table[HOLE_MAX][STEP_MAX][4] = { { // Hole 1 motion data { 180, 133, 38, 116 }, { 180, 133, 40, 110 }, { 180, 134, 42, 100 }, { 180, 134, 44, 90 }, { 180, 136, 46, 80 }, { 180, 138, 48, 75 }, { 180, 140, 50, 75 }, { 180, 141, 52, 75 }, { 180, 142, 54, 74 }, { 180, 143, 56, 74 }, { 180, 144, 58, 74 }, { 180, 145, 60, 74 }, { 180, 146, 62, 74 }, { 180, 147, 64, 74 }, { 180, 148, 68, 74 }, { 180, 149, 70, 74 }, { 180, 150, 72, 74 }, { 180, 151, 74, 74 }, { 180, 152, 76, 74 }, { 180, 153, 78, 74 }, { 180, 154, 80, 74 }, { 180, 155, 82, 74 }, { 180, 156, 84, 74 }, { 180, 157, 86, 76 }, { 180, 159, 88, 78 }, { 180, 160, 90, 80 }, { 180, 162, 92, 82 }, { 180, 164, 94, 84 }, { 180, 166, 96, 86 }, { 180, 168, 98, 88 }, { 180, 170, 100, 90 }, { 180, 171, 102, 90 }, { 180, 172, 104, 92 }, { 180, 173, 104, 92 }, { 145, 174, 104, 92 }, { 145, 172, 104, 92 }, { 145, 171, 104, 92 }, { 145, 170, 104, 90 }, { 145, 170, 102, 90 }, { 145, 168, 100, 90 }, { 145, 166, 98, 88 }, { 145, 164, 96, 86 }, { 145, 162, 94, 84 }, { 145, 161, 94, 82 }, { 145, 160, 92, 80 }, { 145, 159, 90, 78 }, { 145, 158, 88, 76 }, { 145, 157, 86, 74 }, { 145, 156, 84, 74 }, { 145, 155, 82, 74 }, { 145, 154, 80, 74 }, { 145, 153, 78, 74 }, { 145, 152, 76, 74 }, { 145, 151, 74, 74 }, { 145, 150, 72, 74 }, { 145, 149, 70, 74 }, { 145, 148, 68, 74 }, { 145, 147, 66, 74 }, { 145, 146, 64, 74 }, { 145, 145, 62, 74 }, { 145, 144, 60, 74 }, { 145, 143, 58, 74 }, { 145, 142, 56, 74 }, { 145, 142, 54, 75 }, { 145, 141, 52, 75 }, { 145, 140, 50, 75 }, { 145, 138, 48, 80 }, { 145, 136, 46, 90 }, { 145, 134, 44, 100 }, { 145, 134, 42, 110 }, { 180, 133, 40, 116 }, { 180, 133, 38, 116 }, }, { // Hole 2 motion data { 180, 133, 38, 116 }, { 180, 133, 40, 110 }, { 180, 134, 42, 100 }, { 180, 134, 44, 90 }, { 180, 136, 46, 80 }, { 180, 138, 48, 75 }, { 180, 140, 50, 75 }, { 180, 142, 52, 75 }, { 180, 144, 54, 75 }, { 180, 145, 56, 74 }, { 180, 146, 58, 73 }, { 180, 147, 60, 72 }, { 180, 148, 62, 71 }, { 180, 149, 64, 70 }, { 180, 150, 66, 60 }, { 180, 151, 68, 59 }, { 180, 152, 70, 58 }, { 180, 153, 72, 57 }, { 180, 154, 74, 55 }, { 180, 155, 76, 54 }, { 180, 156, 78, 53 }, { 180, 157, 80, 53 }, { 180, 157, 82, 52 }, { 180, 157, 84, 52 }, { 180, 158, 86, 51 }, { 180, 158, 88, 51 }, { 180, 158, 90, 50 }, { 180, 158, 92 , 50 }, { 180, 158, 94, 49 }, { 180, 158, 96, 49 }, { 180, 158, 100, 48 }, { 180, 158, 105, 48 }, { 180, 159, 108, 47 }, { 180, 159, 108, 47 }, { 145, 159, 108, 47 }, { 145, 159, 108, 47 }, { 145, 158, 108, 47 }, { 145, 158, 106, 48 }, { 145, 158, 104, 49 }, { 145, 158, 102, 50 }, { 145, 157, 100, 51 }, { 145, 157, 98, 52 }, { 145, 157, 96, 53 }, { 145, 157, 94, 54 }, { 145, 157, 92, 55 }, { 145, 157, 90, 56 }, { 145, 156, 88, 57 }, { 145, 156, 86, 58 }, { 145, 155, 84, 59 }, { 145, 155, 82, 60 }, { 145, 154, 80, 62 }, { 145, 153, 78, 63 }, { 145, 152, 76, 64 }, { 145, 151, 74, 65 }, { 145, 150, 72, 66 }, { 145, 149, 70, 67 }, { 145, 148, 68, 68 }, { 145, 147, 66, 69 }, { 145, 146, 64, 70 }, { 145, 145, 62, 71 }, { 145, 144, 60, 72 }, { 145, 143, 58, 73 }, { 145, 142, 56, 74 }, { 145, 142, 54, 75 }, { 145, 141, 52, 75 }, { 145, 140, 50, 75 }, { 145, 138, 48, 85 }, { 145, 136, 46, 90 }, { 145, 134, 44, 100 }, { 145, 134, 42, 110 }, { 180, 133, 40, 116 }, { 180, 133, 38, 116 }, }, { // Hole 3 motion data { 180, 133, 38, 116 }, { 180, 133, 40, 110 }, { 180, 132, 42, 100}, { 180, 132, 44, 90 }, { 180, 130, 46, 80 }, { 180, 128, 46, 75 }, { 180, 126, 48, 75 }, { 180, 125, 51, 70 }, { 180, 124, 54, 70 }, { 180, 123, 57, 65 }, { 180, 122, 60, 65 }, { 180, 121, 63, 60 }, { 180, 120, 71, 55 }, { 180, 119, 74, 50 }, { 180, 119, 77, 49 }, { 180, 118, 80, 48 }, { 180, 118, 83, 47 }, { 180, 117, 86, 46 }, { 180, 117, 89, 45 }, { 180, 116, 92, 44 }, { 180, 116, 95, 43 }, { 180, 116, 98, 42 }, { 180, 115, 101, 41 }, { 180, 115, 104, 40 }, { 180, 115, 107, 39 }, { 180, 114, 110, 38 }, { 180, 114, 113, 36 }, { 180, 114, 116, 34 }, { 180, 113, 119, 32 }, { 180, 113, 122, 32 }, { 180, 113, 128, 30 }, { 180, 112, 135, 30 }, { 180, 112, 135, 29 }, { 180, 112, 135, 29 }, { 145, 112, 130, 29}, { 145, 112, 135, 29 }, { 145, 113, 134, 29 }, { 145, 113, 131, 30 }, { 145, 114, 128, 30 }, { 145, 114, 125, 32 }, { 145, 114, 122, 32 }, { 145, 115, 119, 34 }, { 145, 115, 116, 36 }, { 145, 115, 116, 37 }, { 145, 116, 110, 39 }, { 145, 116, 107, 40 }, { 145, 116, 104, 41 }, { 145, 117, 101, 42 }, { 145, 117, 98, 43 }, { 145, 117, 95, 44 }, { 145, 118, 92, 45 }, { 145, 118, 89, 46 }, { 145, 118, 86, 47 }, { 145, 119, 83, 48 }, { 145, 119, 80, 49 }, { 145, 119, 77, 50 }, { 145, 120, 73, 55 }, { 145, 120, 70, 60 }, { 145, 120, 67, 65 }, { 145, 121, 64, 70 }, { 145, 121, 61, 70 }, { 145, 123, 58, 71 }, { 145, 125, 56, 72 }, { 145, 126, 54, 74 }, { 145, 127, 52, 75 }, { 145, 128, 50, 75 }, { 145, 129, 48, 80 }, { 145, 130, 46, 90 }, { 145, 132, 44, 100 }, { 145, 132, 42, 110 }, { 180, 133, 40, 116 }, { 180, 133, 38, 116 } } }; void setup() { Serial.begin(115200); pinMode(PB0, INPUT); // Hole Sensor1 pinMode(PB1, INPUT); // Hole Sensor2 pinMode(PA4, INPUT); // Hole Sensor3 servo_0.attach(PB9, 500, 2500); // hand servo_1.attach(PB8, 500, 2500); // arm1 servo_2.attach(PB7, 500, 2500); // arm2 servo_3.attach(PB6, 500, 2500); // pan // initial write value servo_0.write(180), servo_1.write(133), servo_2.write(38), servo_3.write(116); } void loop() { if (digitalRead(PB0) == LOW) { // Hole 1 Motion_dat(1); } if (digitalRead(PB1) == LOW) { // Hole 2 Motion_dat(2); } if (digitalRead(PA4) == LOW) { // Hole 3 Motion_dat(3); } } void Motion_dat(int hole) { for (int step = 0; step < STEP_MAX; step++){ servo_0.write(motion_table[hole-1][step][0]); // hand servo_1.write(motion_table[hole-1][step][1]); // arm1 servo_2.write(motion_table[hole-1][step][2]); // arm2 servo_3.write(motion_table[hole-1][step][3]); // pan delay(timeset); } }