利用兩個搖桿控制機器手臂

利用兩個搖桿控制機器手臂

 //ping 11 伺服馬達 控制左右
 //ping 10 伺服馬達 控制前後
 //ping 9 伺服馬達 控制上下
 //ping 5 伺服馬達 控制夾子
 //ping A0 第一個搖桿的 URX 控制夾子
 //ping A1 第一個搖桿的 URY 控制上下
 //ping A2 第二個搖桿的 URX 控制左右
 //ping A3 第二個搖桿的 URY 控制前後
 //ping A4 接LCD的 SDA 
 //ping A5 接LCD的 SCL 
#include <Wire.h>
#include <motoLiquidCrystal_I2C.h>
#include <Servo.h>
LiquidCrystal_I2C mylcd(0x27,16,2);
//設定變數
int  S11;      //S11伺服馬達的起始角度 控制左右
int  S11_max;  //S11伺服馬達的最大角度 
int  S11_min;  //S11伺服馬達的最小角度 
int  S10;      //S10伺服馬達的起始角度 控制前後
int  S10_max;  //S10伺服馬達的最大角度
int  S10_min;  //S10伺服馬達的最小角度
int  S9;       //S9伺服馬達的起始角度 控制上下
int  S9_max;   //S9伺服馬達的最大角度
int  S9_min;   //S9伺服馬達的最小角度
int  S5;       //S5伺服馬達的起始角度 控制夾子
int  S5_max;   //S5伺服馬達的最大角度
int  S5_min;   //S5伺服馬達的最小角度
int  R_A0_S9;  //A0搖桿讀出的數據轉換成伺服馬達 S9 角度的變化量  控制上下
int  R_A1_S5;  //A1搖桿讀出的數據轉換成伺服馬達 S5 角度的變化量  控制夾子
int  R_A2_S10; //A2搖桿讀出的數據轉換成伺服馬達 S10 角度的變化量  控制前後
int  R_A3_S11; //A3搖桿讀出的數據轉換成伺服馬達 S11 角度的變化量  控制左右
//設定四個伺服馬達的名稱
Servo servo_11;
Servo servo_10;
Servo servo_9;
Servo servo_5;

void setup()
{
  mylcd.init();
  mylcd.backlight();
//四個伺服馬達腳位
  servo_11.attach(11);
  servo_10.attach(10);
  servo_9.attach(9);
  servo_5.attach(5);
//搖桿的四個腳位
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
//設定四個伺服馬達一、起始角度。二、最大角度。三、最小角度  
  S11 = 90;
  S11_max = 180;
  S11_min = 0;
  S10 = 90;
  S10_max = 170;
  S10_min = 50;
  S9 = 90;
  S9_max = 130;
  S9_min = 10;
  S5 = 120;
  S5_max = 150;
  S5_min = 90;
//角度變化量，初始直為0，不會動
  R_A0_S9 = 0;
  R_A1_S5 = 0;
  R_A2_S10 = 0;
  R_A3_S11 = 0;
  //寫入四個伺服馬達的起始角度
  servo_11.write(S11);
  delay(1000);
  servo_10.write(S10);
  delay(1000);
  servo_9.write(S9);
  delay(1000);
  servo_5.write(S5);
  delay(1000);

}


void loop()
{
  //偵測四個搖桿的數值，並轉換成-4~5，因為每個搖桿回傳值不同，須自行微調。調到不動搖桿時，機器手臂是靜止不動的。
    R_A0_S9 = (map(analogRead(A0), 0, 1023,4,(-5)));
    R_A1_S5 = (map(analogRead(A1), 0, 1023,(-4),4));
    R_A2_S10 = (map(analogRead(A2), 0, 1023,(-4),5));
    R_A3_S11 = (map(analogRead(A3), 0, 1023,4,(-5)));
    
 //判斷數值有無超過最大及最小角度，若無則移動伺服馬達
    if (S9 > S9_max) { S9 = S9_max;  servo_9.write(S9); } 
    else if (S9 < S9_min) { S9 = S9_min;  servo_9.write(S9); } 
    else { S9 = S9 + R_A0_S9; servo_9.write(S9); }
    
    if (S5 > S5_max) { S5 = S5_max;  servo_5.write(S5); } 
    else if (S5 < S5_min) { S5 = S5_min;  servo_5.write(S5); } 
    else { S5 = S5 + R_A1_S5; servo_5.write(S5); }
    
    if (S10 > S10_max) { S10 = S10_max;  servo_10.write(S10); } 
    else if (S10 < S10_min) { S10 = S10_min; servo_10.write(S10); } 
    else { S10 = S10 + R_A2_S10;  servo_10.write(S10); }
    
    if (S11 > S11_max) { S11 = S11_max;  servo_11.write(S11); } 
    else if (S11 < S11_min) { S11 = S11_min; servo_11.write(S11); } 
    else { S11 = S11 + R_A3_S11;  servo_11.write(S11); }
    
    mylcd.setCursor(0,0);      mylcd.print("S9=");
    mylcd.setCursor(4,0);      mylcd.print(S9);
    mylcd.setCursor(0,1);      mylcd.print("S5=");
    mylcd.setCursor(4,1);      mylcd.print(S5);
    mylcd.setCursor(8,0);      mylcd.print("S10=");
    mylcd.setCursor(12,0);     mylcd.print(S10);
    mylcd.setCursor(8,1);      mylcd.print("S11=");
    mylcd.setCursor(12,1);     mylcd.print(S11);
    delay(10);
    mylcd.clear();

}