碼源轉載自：http://www.zg4o1577.cn/bbs/dpj-156513-1.html，只是用于學習，若有侵犯原作者權益請告知，我會及時刪除的
附：代碼
- #include <Servo.h>
- Servo servo_0; //定義4個舵機
- Servo servo_1;
- Servo servo_2;
- Servo servo_3;
- int sensorPin0 = A0; //電位器阻值輸入口
- int sensorPin1 = A1;
- int sensorPin2 = A2;
- int sensorPin3 = A3;
- int SensVal_0;
- int SensVal_1;
- int SensVal_2;
- int SensVal_3;
- int led1 = 1;
- int led2 = 2;
- boolean playmode = false; //設置運行模式，初始化為學習
- float angle0[30];// 存儲底座舵機動作的數組，最多存30個
- float angle1[30];//手臂舵機角度數組
- float angle2[30];//手掌舵機角度數組
- float angle3[30];//夾子舵機角度數組
- float dif[4],temp[4];// 差分值與中間數組
- int i,j,k,stepsMax,num=0,key_value,num1=0,del;
- void setup() {
- // put your setup code here, to run once:
- pinMode(4, INPUT); // 鍵盤輸入口
- pinMode(sensorPin0, INPUT);
- pinMode(sensorPin1, INPUT);
- pinMode(sensorPin2, INPUT);
- pinMode(sensorPin3, INPUT);
- pinMode(led1, OUTPUT);
- pinMode(led2, OUTPUT);
- servo_0.attach(6); // 連接舵機
- servo_1.attach(9);
- servo_2.attach(10);
- servo_3.attach(11);
- Serial.begin(9600);
- }
- void loop()
- {
- // 主函數，loop循環
- if(playmode == false) //學習模式
- {
- SensVal_1 = analogRead(sensorPin1); //讀取電位器阻值
- Serial.println(SensVal_1);
- SensVal_1 = map(SensVal_1, 0, 1024, 500, 2500);//將電位器阻值與舵機角度映射，實現兩個機械臂的同步
- SensVal_0 = analogRead(sensorPin0);
- Serial.println(SensVal_0);
- SensVal_0 = map(SensVal_0, 0, 1024, 500, 2500);
- SensVal_2 = analogRead(sensorPin2);
- Serial.println(SensVal_2);
- SensVal_2 = map(SensVal_2, 0, 1024, 500, 2500);
- SensVal_3 = analogRead(sensorPin3);
- Serial.println(SensVal_3);
- SensVal_3 = map(SensVal_3, 0, 1024, 500, 2500);
- servo_0.writeMicroseconds(SensVal_0); //控制主機械臂跟隨從機械臂運行
- servo_1.writeMicroseconds(SensVal_1);
- servo_2.writeMicroseconds(SensVal_2);
- servo_3.writeMicroseconds(SensVal_3);
- digitalWrite(led1,HIGH);
- digitalWrite(led2,LOW);
- }
- else //運行模式
- {
- for(i=0;i<num;i++)
- {
- temp[0]=angle0[i]; //暫存當前動作角度值
- temp[1]=angle1[i];
- temp[2]=angle2[i];
- temp[3]=angle3[i];
- j=i+1; //指向下一個動作
- if(j==num) j=0;
- dif[0] = abs(angle0[j]-angle0[i]); //下一動作與當前動作做差分
- dif[1] = abs(angle1[j]-angle1[i]);
- dif[2] = abs(angle2[j]-angle2[i]);
- dif[3] = abs(angle3[j]-angle3[i]);
- stepsMax = max(dif[0],dif[1]); //找出最大差分值
- stepsMax = max(stepsMax,dif[2]);
- stepsMax = max(stepsMax,dif[3]);
- //將兩個動作分為最大差分值個小動作，即用每個舵機的差分值除以最大差分值
- if (angle0[j] < angle0[i]) dif[0] = 0-dif[0]/stepsMax; else dif[0] = dif[0]/stepsMax;
- if (angle1[j] < angle1[i]) dif[1] = 0-dif[1]/stepsMax; else dif[1] = dif[1]/stepsMax;
- if (angle2[j] < angle2[i]) dif[2] = 0-dif[2]/stepsMax; else dif[2] = dif[2]/stepsMax;
- if (angle3[j] < angle3[i]) dif[3] = 0-dif[3]/stepsMax; else dif[3] = dif[3]/stepsMax;
- for(k=0;k<stepsMax;k++) // 運行至下一大動作
- {
- Serial.println("stepin");
- angle0[i] += dif[0]; //運行到下一小動作
- angle1[i] += dif[1];
- angle2[i] += dif[2];
- angle3[i] += dif[3];
- servo_0.writeMicroseconds(angle0);
- servo_1.writeMicroseconds(angle1);
- servo_2.writeMicroseconds(angle2);
- servo_3.writeMicroseconds(angle3);
- delay(1);
- }
- Serial.println("stepout");
- angle0[i] = temp[0]; //返回暫存的動作值
- angle1[i] = temp[1];
- angle2[i] = temp[2];
- angle3[i] = temp[3];
- }
- }
- Serial.println(digitalRead(4));
- Button(); //按鍵檢測
- }
- void Button() //按鍵檢測
- {
- if (digitalRead(4) == true)
- {
- delay(20); //消抖
- if (digitalRead(4) == true)
- {
- key_value = 1;
- delay(1000);
- if(digitalRead(4) == true) key_value = 2; //長按進入運行模式
- }
- }
- if ((key_value == 1)) // 記錄舵機當前位置
- {
- angle0[num1]=SensVal_0;
- angle1[num1]=SensVal_1;
- angle2[num1]=SensVal_2;
- angle3[num1]=SensVal_3;
- num1++; //指針加1
- key_value = 0;
- playmode = false;
- }
- else if (key_value == 2)
- {
- num = num1; //取出動作的總數
- num1=0; //為下一次學習做準備
- playmode = true; //運行模式
- key_value = 0;
- digitalWrite(led2,HIGH);
- digitalWrite(led1,LOW);
- }
- }
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
