#include <Servo.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);// 0x27のアドレス,16列2行のLCDを使用,A0A1A2が解放状態で1+2+4=7で0x27,A0が短絡で0+2+4=6で0x26となる。
//LiquidCrystal lcd(4, 2, 7, 8, 12, 13); //本PRGではPin7はSWに使用している。

Servo servoA;  //base 0～180deg.
Servo servoB;  //shoulder 15～165deg.
Servo servoC;  //elbow 0～180deg.
Servo servoD;  //wrist_ver 0～180deg.
Servo servoE;  //wrist_rot 0～180deg.
Servo servoF;  //gripper 10～73deg. 10deg=open, 73deg=close.

//Informationsite: https://www.thingbits.in/products/arduino-braccio-robotic-arm
//Motorspec1
//<SpringRC SR431> - Dual Output Servo: for base,shoulder,elbow,wrist_ver.
//Control Signal: PWM Analog
//Torque: @ 4.8V: 169.5 oz-in (12.2 kg-cm), @ 6.0V: 201.4 oz-in (14.5 kg-cm)
//Weight: 2.19 oz (62.0 g)
//Dimentions: 1.65×0.81×1.56 in (42.0×20.5×39.5 mm)
//Speed: @ 4.8V: 0.20 sec/60°, @ 6.0V: 0.18 sec/60°
//Rotation Support: Dual Bearings
//Gear Material: Metal
//Rotation Range: 180°
//Connector Type: J (aka Futaba)
//@4.8V:300deg/sec= 0.83 rps=5.236rad/s, 1.196Nm ⇒ 6.262W

//Motorspec2
//<SpringRC SR311>: for wrist rot,glipper.
//Control Signal: PWM Analog
//Torque: @ 4.8V: 43.13 oz-in (3.1 kg-cm), @ 6.0V: 52.86 oz-in (3.8 kg-cm)
//Weight: 0.95 oz (27.0 g)
//Dimentions: 1.23×0.65×1.13 in (31.3×16.5×28.6 mm)
//Speed: @ 4.8V: 0.14 sec/60°, @ 6.0V: 0.12 sec/60°
//Rotation Support: Dual Bearings
//Gear Material: Metal
//Rotation Range: 180°
//Connector Type: J (aka Futaba)
//@4.8V: 1.19rps=7.477rad/s, 0.304Nm ⇒ 2.273W
const int STP = 2;//割り込み停止入力
const int SW = 7;//スイッチ入力
const int EMG = 8;//停止スイッチ
const int BRK = 12;//解除スイッチ
int val = 1;
int val2 = 0;
int val3 = 0;
int oldval = 0;
int state = 0;

void setup() {
  attachInterrupt(0,stop,CHANGE);
  lcd.init();                    // LCDの初期化
  lcd.backlight();               // LCDバックライトの点灯
  lcd.setCursor(0, 0);           // カーソルの位置を指定
  lcd.print("RobotArm PRG.");      // 文字の表示
  lcd.setCursor(0, 1);           // カーソルの位置を指定
  lcd.print("  Ver.0  ");  // 文字の表示
  //pinMode(3, INPUT_PULLUP);   //Prevent the arm from returning to  initial position.
  //pinMode(5, INPUT_PULLUP);   //Prevent the arm from returning to  initial position.
  //pinMode(6, INPUT_PULLUP);   //Prevent the arm from returning to  initial position.
  //pinMode(9, INPUT_PULLUP);   //Prevent the arm from returning to  initial position.
  //pinMode(10, INPUT_PULLUP);  //Prevent the arm from returning to  initial position.
  //pinMode(11, INPUT_PULLUP);  //Prevent the arm from returning to  initial position.
  pinMode(2, INPUT);//割り込み停止入力ピン設定
  pinMode(7,INPUT_PULLUP); //スイッチOFFのときにHIGH設定。スイッチONしたときにLOWになるように接続のこと。
  pinMode(8, INPUT);//停止スイッチ、HIGH有効の場合はプルダウンでLOW入力
  pinMode(12, INPUT);//解除スイッチ、HIGH有効の場合はプルダウンでLOW入力
  pinMode(13, OUTPUT);//停止ランプ
  delay(1000);
  pinMode(3,OUTPUT);
  servoA.attach(3,500,2400);//500～2500の幅で0～180deg。ここで幅を制限することも可能だが、例えばMax2300にすると180degまでは動かない。
  pinMode(5,OUTPUT); 
  servoB.attach(5,500,2400);
  pinMode(6,OUTPUT); 
  servoC.attach(6,500,2400);
  pinMode(9,OUTPUT); 
  servoD.attach(9,500,2400);
  pinMode(10,OUTPUT); 
  servoE.attach(10,500,2400);
  pinMode(11,OUTPUT); 
  servoF.attach(11,500,2400);
}

void stop(){
  //↓非常停止スイッチのテストプログラム
  val2 = digitalRead(8);
  val3 = digitalRead(12);
  if((val2 == HIGH)&&(state == 0)){
    state = 1;
  }
  if((val3 == HIGH)&&(state == 1)){
    state = 0;
  }
  //oldval;
  if((state == 1)&&(val3 == LOW)){
    digitalWrite(13, HIGH);
  }
  if((state == 0)&&(val2 == LOW)){
    digitalWrite(13,LOW);
  }
  //↑ここまでが停止スイッチのstateテストプログラム
servoA.detach();
servoB.detach();
servoC.detach();
servoD.detach();
servoE.detach();
servoF.detach();
}

void loop() {
  val = digitalRead(SW);
  //↓非常停止スイッチのテストプログラム
  val2 = digitalRead(8);
  val3 = digitalRead(12);
  if((val2 == HIGH)&&(state == 0)){
    state = 1;
  }
  if((val3 == HIGH)&&(state == 1)){
    state = 0;
  }
  //oldval;
  if((state == 1)&&(val3 == LOW)){
    digitalWrite(13, HIGH);
  }
  if((state == 0)&&(val2 == LOW)){
    digitalWrite(13,LOW);
  }
  //↑ここまでが停止スイッチのstateテストプログラム
  if (val == LOW){
  lcd.setCursor(0, 0);           // カーソルの位置を指定
  lcd.print("RobotArm PRG.");      // 文字の表示
  lcd.setCursor(0, 1);           // カーソルの位置を指定
  lcd.print("Automode");  // 文字の表示
  servoA.write(90);//safemode and grease up mode. base 0～180deg.
  delay(5);
  servoB.write(45);//safemode and grease up mode. shoulder 15～165deg.
  delay(5);
  servoC.write(180);//safemode and grease up mode. elbow 0～180deg.
  delay(5);
  servoD.write(180);//safemode and grease up mode. wrist_ver 0～180deg.
  delay(5);
  servoE.write(90);//safemode and grease up mode. wrist_rot 0～180deg.
  delay(5);
  servoF.write(10);//safemode and grease up mode. gripper 10～73deg. 10deg=open, 73deg=close.
  delay(1000);
  //サーボモータは1周期20ms指令なので、20msで1deg動く指令の場合、90deg移動で1.8秒を要する。delayで20ms区切り実施。
  for (int  a = 90; a < 180; a++) {//base角度を90～180°迄1°づつ増加させる。(glipper開き状態で旋回)
    servoA.write(a);                   //ピンにサーボ制御信号を出力
    delay(20);                    //サーボモータは指令1周期20msのため、20msを超えるdelayは動作に支障をきたすので注意。
  }
  for (int b = 45; b < 65; b++){//shoulder角度を45degから65degまで1degづつ増加。(glipper開き状態で降下)
    servoB.write(b);
    delay(20);
  }
  for (int f = 10; f < 73; f++){//glipper角度を10degから73degまで1degづつ増加。(glipper閉じ動作)
    servoF.write(f);
    delay(20);
  }
  //↓ここから戻り運転。
  for (int b = 65; b >= 45; b--){//shoulder角度を65degから45degまで1degづつ減少。(glipper閉じ状態で上昇)
    servoB.write(b);
    delay(20);
  }
  for (int a = 180; a >= 90; a--){//base角度を180～90°迄1°づつ減少させる。(glipper閉じ状態で旋回戻り)
    servoA.write(a);                   //ピンにサーボ制御信号を出力  
    delay(20);                    //20ms待つ
  } 
  for (int b = 45; b < 65; b++){//shoulder角度を45degから65degまで1degづつ増加。(glipper閉じ状態で降下)
    servoB.write(b);
    delay(20);
  }
  for (int f = 73; f >= 10; f--){//glipper角度を73degから10degまで1degづつ減少。(glipper開き動作)
    servoF.write(f);
    delay(20);
  }
  for (int b = 65; b >= 45; b--){//shoulder角度を65degから45degまで1degづつ減少。(glipper開き状態で上昇し、初期位置戻り)
    servoB.write(b);
    delay(20);
  }
  }
  else if (val == HIGH){ //スイッチSW 7ピンがHighにONされたとき。内部プルアップで操作無しのときはこの状態で待機。
  lcd.setCursor(0, 0);
  lcd.print("RobotArm PRG.");
  lcd.setCursor(0, 1);
  lcd.print("Standbymode");
  servoA.write(90);//safemode and grease up mode. base 0～180deg.
  delay(5);
  servoB.write(45);//safemode and grease up mode. shoulder 15～165deg.
  delay(5);
  servoC.write(180);//safemode and grease up mode. elbow 0～180deg.
  delay(5);
  servoD.write(180);//safemode and grease up mode. wrist_ver 0～180deg.
  delay(5);
  servoE.write(90);//safemode and grease up mode. wrist_rot 0～180deg.
  delay(5);
  servoF.write(73);//safemode and grease up mode. gripper 10～73deg. 10deg=open, 73deg=close.
  //↓アナログ指令、受け取った瞬間に抵抗電圧値を読み取って急旋回してしまうので要修正。
  //int degreeA = analogRead(1); //A0 base-stick
  //int degreeB = analogRead(4); //A4 shoulder-stick
  //int degreeC = analogRead(2); //A2 elbow-stick
  //int degreeD = analogRead(5); //A5 wrist_ver-stick
  //int degreeE = analogRead(3); //A3 wrist_rot-stick
  //int degreeF = analogRead(0); //A0 Gripper-stick
  //degreeA = map(degreeA,0,1023,0,180); //base-stick1
  //degreeB = map(degreeB,0,1023,15,165); //base-stick1
  //degreeC = map(degreeC,0,1023,0,180); //base-stick2
  //degreeD = map(degreeD,0,1023,0,180); //base-stick2
  //degreeE = map(degreeE,0,1023,0,180); //base-stick3
  //degreeF = map(degreeF,0,1023,10,73); //base-stick3
  //servoA.write(degreeA);
  //servoB.write(degreeB);
  //servoC.write(degreeC);
  //servoD.write(degreeD);
  //servoE.write(degreeE);
  //servoF.write(degreeF);
  }
}