Created
January 8, 2018 09:27
-
-
Save skynettw/7b22973aecb81da0734adcc209e21f3c to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "Servo.h" | |
#define M1 9 | |
#define M2 10 | |
#define M3 11 | |
#define TOTAL_MOTORS 2 | |
Servo motor[TOTAL_MOTORS]; | |
Servo base; | |
int mmax[TOTAL_MOTORS] = {180, 180}; | |
int mmin[TOTAL_MOTORS] = {0, 0}; | |
int current_degree[TOTAL_MOTORS] = {90, 90}; | |
void setup() { | |
Serial.begin(9600); | |
pinMode(2, INPUT); | |
pinMode(3, INPUT); | |
motor[0].attach(M2); | |
motor[1].attach(M3); | |
base.attach(M1); | |
base.write(90); | |
delay(300); | |
for(int i=0; i<TOTAL_MOTORS; i++) { | |
motor[i].write(90); delay(200); | |
current_degree[i] = 90; | |
} | |
} | |
// 此函數用來一次把3個伺服馬達調整到指定的角度(degree陣列) | |
void arm_move_all_to(int degree[]) { | |
int diff[TOTAL_MOTORS]; | |
int max_diff_degree=0; // 最大的差異角度數 | |
int max_index=0; // 最大差異角度的那個伺服馬達 | |
int remap[TOTAL_MOTORS][180]; // 重新對應角度用的陣列 | |
// 以下的迴圈用來找出哪一個伺服器的差異角度最多 | |
for(int i=0; i<TOTAL_MOTORS; i++) { | |
diff[i] = degree[i] - current_degree[i] ; | |
if(abs(diff[i])>max_diff_degree) { | |
max_index = i; | |
max_diff_degree = abs(diff[i]); | |
} | |
} | |
// 以下重新對應要調整的角度陣列 | |
for(int i=0; i<=max_diff_degree; i++) { | |
for(int motor_no=0; motor_no<TOTAL_MOTORS; motor_no++) { | |
remap[motor_no][i] = current_degree[motor_no] + | |
(int) (((float)diff[motor_no]/(float)max_diff_degree)*i); | |
//disp_var(remap[motor_no][i]); | |
} | |
} | |
// 以下開始讓伺服馬達調整角度 | |
for(int i=0; i<=max_diff_degree; i++) { | |
for(int motor_no=0; motor_no<TOTAL_MOTORS; motor_no++) { | |
current_degree[motor_no] = remap[motor_no][i]; | |
motor[motor_no].write(remap[motor_no][i]); | |
} | |
delay(20); | |
//disp_current_degree(); | |
} | |
//delay(1000); | |
} | |
int count=0; | |
void loop() { | |
int pin2, pin3; | |
int d[2]; | |
d[0]=170; d[1]=20; | |
arm_move_all_to(d); | |
d[0]=60; d[1]=180; | |
arm_move_all_to(d); | |
pin2 = digitalRead(2); | |
pin3 = digitalRead(3); | |
if(++count<5) { | |
base.write(80); | |
delay(300); | |
} else { | |
base.write(100); | |
delay(300); | |
if(count>10) count=0; | |
} | |
base.write(90); | |
delay(300); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment