Roboterarm/Gripper/Gripper.ino

#include "Arduino.h"
#include "AX12A.h"

#define DirectionPin   (10u)
#define BaudRate      (1000000ul)
#define ID1       (4u)
#define ID2       (5u)

#include <XYZrobotServo.h>

#ifdef SERIAL_PORT_HARDWARE_OPEN
#define servoSerial SERIAL_PORT_HARDWARE_OPEN
#else
#include <SoftwareSerial.h>
SoftwareSerial servoSerial(10, 11);
#endif

int input = 0;
XYZrobotServo servo1(servoSerial, 2);

const uint8_t playtime = 100;

int button_oben = 2;
int button_unten = 3;
int ain1 = 4;
int ain2 = 5;
int pwma = 8;

int x ,position;
int val[10] = {0,0,0,0,0};
int choice;

void ax12_1(int position){
     if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;}
  ax12a.moveSpeed(ID1,position, playtime);
}

void ax12_2(int position){
     if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;}
  ax12a.moveSpeed(ID2,position, playtime);
}

void a116(int position){
    if((position<300) || (position>700)){
    Serial.print("invalide position. Try again");
    return;
  }
  servo1.setPosition(position, playtime);
}

void motor(int speed, int dir){
  if (dir == 1){
    digitalWrite(ain1,HIGH);
    digitalWrite(ain2,LOW);
    }
    else{
    digitalWrite(ain1,LOW);
    digitalWrite(ain2,HIGH);
    }
    analogWrite(pwma,speed);
  }

void setup() {
  Serial.begin(9600);
  Serial.setTimeout(1);

  ax12a.begin(BaudRate, DirectionPin, &Serial3);
  ax12a.setEndless(ID1, OFF);
  ax12a.setEndless(ID2, OFF);

  servoSerial.begin(115200);

  ax12a.move(ID1,512);
  ax12a.move(ID2,512);
  servo1.setPosition(512, playtime);

  pinMode(button_oben,INPUT_PULLUP);
  pinMode(button_unten,INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(button_oben),interrupt1, LOW);
  attachInterrupt(digitalPinToInterrupt(button_unten),interrupt2,LOW);
}
void loop() {
 if (Serial.available()){
   //Serial.print(test);
   for(int n = 0; n < 10 ; n++){
    val[n] = Serial.readString().toInt();

   }
   position = 0 ;
   for(int i = 2; i < 5;i++){
     position = position * 10 + val[i];
   }
   choice = val[0];
   Serial.print("Motor: ");
   Serial.print(choice);
   Serial.print(" position: ");
   Serial.print(position);
  switch(val[0]){
    case 1:
    ax12_1(position);
    Serial.print("Ready");
    break;
    case 2:
    a116(position);
    Serial.print("Ready");
    break;
    case 3:
    ax12_2(position);
    Serial.print("Ready");
    break;
    case 4:
    motor(position,val[1]);
    Serial.print("Ready");
    break;
  }
 }
}

void interrupt1(){
    Serial.println("interrupt button oben");
    motor(254,' ');
    delay(200);
    motor(0,' ');
  }
void interrupt2(){
    Serial.println("interrupt button unten");
    motor(254,'a');
    delay(200);
    motor(0,'a');
    }