Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#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
You can’t perform that action at this time.