Robotic Arm-Objects Collector with Finite State Machines

// inclui bibilioteca do servomotor
#include <Servo.h> 
// define pinos dos servos
#define pinServ1 11
#define pinServ2 10
#define pinServ3 9
#define pinServ4 6
//Trigger Pin
#define Trig_pin 4
//Echo Pin
#define Echo_pin 2
//echo time
short button_pin=5;

long rx_time;

static int R1_in,R2_in,R3_in,R4_in;
static int R1_out,R2_out,R3_out,R4_out;
static int RG_in,RG_out;
static int F2_out,F2_in;
static int FP1_out,FP2_out,FP3_out,FP4_out;
static int FP1_in,FP2_in,FP3_in,FP4_in;
static int S2_in,S3_in;
static int S2_out,S3_out;

Servo servo1,servo2,servo3,servo4;
// cria as variavies dos angulos de cada motor

void setup() {
 //inicia o monitor serial
  Serial.begin(9600); 
  pinMode(Trig_pin, OUTPUT);
  //Setup PING
  pinMode(Echo_pin, INPUT);
  attachInterrupt(digitalPinToInterrupt(Echo_pin),ping_rx_IR,FALLING);
  pinMode(button_pin,INPUT);
  // atribui pinos dos servos
  servo1.attach(pinServ1);
  servo2.attach(pinServ2);
  servo3.attach(pinServ3);
  servo4.attach(pinServ4);
  servo1.write(72);
  servo2.write(130);
  servo3.write(48);
  servo4.write(120);
  delay(3000);
}
void ping_rx_IR(){
   //PING without delay waits for echo
    rx_time = micros();
    //Serial.println(rx_time);
  }
  
void End_step_fsm(int trig){
static int state;
//Serial.println(RG_out);
//Serial.println(state);
switch(state){
  case 0:
  if(R1_in==1 && R2_in==1 && R3_in==1 && R4_in==1){state=1;}
  RG_out=0;
  break;
  case 1:
  if(1){state=2;}
  RG_out=1;
  break;
  case 2:
  if(trig==1){state=0;}
  RG_out=0;
  break;
  }
}
float distance_med_fsm(float distance){
  static int state;
  static float media;
  static int i;
//Serial.println(distanceIn);

  switch(state){
    case 0:
    if(i>=99){state=1;}
    //Serial.println(i);
    //dist[i]=distance;
    media=media+distance;
    i++;
    //Serial.println(dist[3]);
    //Serial.println(dist[4]);
    break;
    case 1:
    if(1){state=0;}
    //Serial.println(media);
    i=0;
    media=media/100;
    return(media);
  }
  }
  
void ctrl_braco_fsm(float distM,short button,int trig){

  static int state;
  float alcancem=14;
  float alcanceM=20;
 //Serial.println(state);
//Serial.println(RG_in);
switch(state){
  case 0:
  if(distM>=alcancem && distM<=alcanceM && trig==1){state=1;}
  FP1_out=68;
  FP2_out=130;
  FP3_out=48;
  FP4_out=120;
  F2_out=0;
  break;
  case 1:
  if(RG_in==1){state=2;}
   FP2_out=70;
   //FP3_out=88;
  break;
  case 2:
  if(RG_in==1){state=3;}
  FP3_out=88;
 //FP2_out=61;
  break;
  case 3:
  if(RG_in==1 ){state=4;}
  FP2_out=S2_in;
  FP3_out=S3_in;
 // FP2_out=92;
  //FP3_out=102;
  break;
  case 4:
  if(RG_in==1 && button==0){state=5;}
  if(RG_in==1 && button==1){state=7;}
  FP4_out=60;
  F2_out=1;
  break;
  case 5:
  if(RG_in==1){state=6;}//designar posio onde por objeto
  //FP1_out=120;
  FP2_out=129;
  FP3_out=56;
  break;
  case 6:
  if(RG_in==1){state=7;}
  FP1_out=120;
 // FP3_out=56;
  break; 
  case 7:
  if(RG_in==1){state=0;}
  FP4_out=120;
  break; 
   
}
}
float ping_dist_fsm(){
  static short int state;
  static long tx_time;
  long echo_time;
  long tt;
  //Serial.println(state);

  switch(state){
    case 0:
      if(1){state = 1;}
      digitalWrite(Trig_pin,LOW);
      delayMicroseconds(2);
      digitalWrite(Trig_pin,HIGH);
      delayMicroseconds(5);
      digitalWrite(Trig_pin,LOW);
      tx_time = micros();
      rx_time = tx_time;
      break;
    
    case 1:
      echo_time = micros();
      if(rx_time - tx_time > 0){state = 2;}
      if(echo_time - tx_time > 30000){state=0;}
      break;
    
    case 2:
      if(1){state = 0;
       tt = rx_time - tx_time - 452; //Needs Calibration
       return (((float)tt/2)*340/10000);
      }
      break;
  }
 }
 
int servo_mov_fsm1(int ps,int act_pos,int trig){
static int state; int gt; 
static int F1, F2;
int Step_max=2;
 //Serial.println(state); 
 
 // Serial.println(R1_out); 
  switch (state) {
  case 0:
    if (ps > act_pos) {state=1;} 
    if (ps < act_pos) {state=2;} 
    gt = act_pos;
    R1_out=1;// output 
  return (gt);  
  case 1:
    if (ps <= act_pos) {state=4;}
    if (ps > act_pos) {state=3;} 
    gt = act_pos + Step_max; // output
    F1=1;
    R1_out=0;
  return (gt);
  case 2:
    if(ps >= act_pos) {state=4;}
    if (ps < act_pos) {state=3;} 
    gt = act_pos - Step_max; // output
    F2=1;
    R1_out=0;
  return (gt);
  break;
  case 3:
  if(trig==1 && F1==1){state=1;}
  if(trig==1 && F2==1){state=2;}
  break;
  case 4:
    if (1) {state=0;}  
    gt = ps; // output
    F1=0;
    F2=0;
  return (ps);
  }
 }

int servo_mov_fsm2(int ps,int act_pos,int trig){
static int state; int gt; 
static int F1, F2;
int Step_max=3;
//Serial.println(R2_out); 
 //Serial.println(state); 
  //Serial.println(ps); 
  //Serial.println(act_pos); 
  switch (state) {
  case 0:
    if (ps > act_pos) {state=1;} 
    if (ps < act_pos) {state=2;} 
    gt = act_pos; // output  
    R2_out=1;
  return (gt);  
  case 1:
    if (ps <= act_pos) {state=4;}
    if (ps > act_pos) {state=3;} 
    gt = act_pos + Step_max; // output
    F1=1;
    R2_out=0;
  return (gt);
  case 2:
    if(ps >= act_pos) {state=4;}
    if (ps < act_pos) {state=3;} 
    gt = act_pos - Step_max; // output
    F2=1;
    R2_out=0;
  return (gt);
  break;
  case 3:
  if(trig==1 && F1==1){state=1;}
  if(trig==1 && F2==1){state=2;}
  break;
  case 4:
    if (1) {state=0;}  
    gt = ps; // output
    F1=0;
    F2=0;
  return (ps);
  }
  }
  
int servo_mov_fsm3(int ps,int act_pos,int trig){
static int state; int gt; 
static int F1, F2;
int Step_max=3;
 //Serial.println(state); 
  //Serial.println(ps); 
  //Serial.println(act_pos); 
  switch (state) {
  case 0:
    if (ps > act_pos) {state=1;} 
    if (ps < act_pos) {state=2;} 
    gt = act_pos; // output  
    R3_out=1;
  return (gt);  
  case 1:
    if (ps <= act_pos) {state=4;}
    if (ps > act_pos) {state=3;} 
    gt = act_pos + Step_max; // output
    F1=1;
    R3_out=0;
  return (gt);
  case 2:
    if(ps >= act_pos) {state=4;}
    if (ps < act_pos) {state=3;} 
    gt = act_pos - Step_max; // output
    F2=1;
    R3_out=0;
  return (gt);
  break;
  case 3:
  if(trig==1 && F1==1){state=1;}
  if(trig==1 && F2==1){state=2;}
  break;
  case 4:
    if (1) {state=0;}  
    gt = ps; // output
    F1=0;
    F2=0;
  return (ps);
  }
 }

int servo_mov_fsm4(int ps,int act_pos,int trig){
static int state; int gt; 
static int F1, F2;
int Step_max=3;
 //Serial.println(state); 
  //Serial.println(ps); 
  //Serial.println(act_pos); 
  switch (state) {
  case 0:
    if (ps > act_pos) {state=1;} 
    if (ps < act_pos) {state=2;} 
    gt = act_pos; // output  
    R4_out=1;
  return (gt);  
  case 1:
    if (ps <= act_pos) {state=4;}
    if (ps > act_pos) {state=3;} 
    gt = act_pos + Step_max; // output
    F1=1;
    R4_out=0;
  return (gt);
  case 2:
    if(ps >= act_pos) {state=4;}
    if (ps < act_pos) {state=3;} 
    gt = act_pos - Step_max; // output
    F2=1;
    R4_out=0;
  return (gt);
  break;
  case 3:
  if(trig==1 && F1==1){state=1;}
  if(trig==1 && F2==1){state=2;}
  break;
  case 4:
    if (1) {state=0;}  
    gt = ps; // output
    F1=0;
    F2=0;
  return (ps);
  }
  }

int trig_gen_fsm(float at, int T){
  static int state;
  static float pt;
  int trig;
//Serial.println(state); 
  switch(state){
    case 0:
    if(at-pt>=T){state=1;}
    //Serial.println(at-pt); 
    trig=0;
    return trig;
    case 1:
    if(1){state=0;}
    trig=1;
    pt=at;
    return trig;
  }
}

int trig_gen_fsm1(float at, int T){
  static int state;
  static float pt;
  int trig;
//Serial.println(state); 
  switch(state){
    case 0:
    if(at-pt>=T){state=1;}
    //Serial.println(at-pt); 
    trig=0;
    return trig;
    case 1:
    if(1){state=0;}
    trig=1;
    pt=at;
    return trig;
  }
}
int trig_gen_fsm2(float at, int T){
  static int state;
  static float pt;
  int trig;
//Serial.println(state); 
  switch(state){
    case 0:
    if(at-pt>=T){state=1;}
    //Serial.println(at-pt); 
    trig=0;
    return trig;
    case 1:
    if(1){state=0;}
    trig=1;
    pt=at;
    return trig;
  }
}


void move_obj_fsm(float distanceM,int trig){
    static int state;
     Serial.println(distanceM);
     Serial.println(state);
switch(state){
  case 0:
  if(distanceM>=14 && distanceM<15 && trig==1){state=1;}
  if(distanceM>=15 && distanceM<16 && trig==1){state=2;}
  if(distanceM>=16 && distanceM<17&& trig==1){state=3;}
  if(distanceM>=17 && distanceM<18 && trig==1){state=4;}
  if(distanceM>=18 && distanceM<19 && trig==1){state=5;}
  if(distanceM>=19 && distanceM<=20 && trig==1){state=6;}
  case 1 :
  if(F2_in==1){state=0;}
  S2_out=77;
  S3_out=96;
  break;
  case 2 :
  if(F2_in==1){state=0;}
  S2_out=78;
  S3_out=99;
  break;
  case 3 :
  if(F2_in==1){state=0;}
  S2_out=87;
  S3_out=106;
  break;
  case 4 :
  if(F2_in==1){state=0;}
  S2_out=88;
  S3_out=109;
  break;
  case 5 :
  if(F2_in==1){state=0;}
  S2_out=97;
  S3_out=113;
  break;
  case 6 :
  if(F2_in==1){state=0;}
  //S2_out=101;
  //S3_out=111;
  S2_out=112;
  S3_out=129;
  break;
  }
}


void loop() {
  static float distance_out,distance_in;
  static float distanceM_out,distanceM_in;
  static int trigger_in,trigger_out;
  static int trigger_in1,trigger_out1;
  static int trigger_in2,trigger_out2;

  
  int act_pos[] = {servo1.read(),servo2.read(),servo3.read(),servo4.read()};
  int servo_goto[4];
  int T=110,T1=800,T2=5000;
  short button;

  button=(short)digitalRead(button_pin); 
  distance_out=ping_dist_fsm();
  //Serial.println(distance_out);
  distanceM_out=distance_med_fsm(distance_in);
  //Serial.println(distanceM_out);
  //Serial.println(distanceM_in);
  //Serial.println(distance);
  //Serial.print(distance);
  ctrl_braco_fsm(distanceM_in,button,trigger_in2);
  move_obj_fsm(distanceM_in,trigger_in2);
  trigger_out=trig_gen_fsm(millis(),T);
  trigger_out2=trig_gen_fsm2(millis(),T2);
  servo_goto[0] = servo_mov_fsm1(FP1_in,act_pos[0],trigger_in);
  servo_goto[1] = servo_mov_fsm2(FP2_in,act_pos[1],trigger_in);
  servo_goto[2] = servo_mov_fsm3(FP3_in,act_pos[2],trigger_in);
  servo_goto[3] = servo_mov_fsm4(FP4_in,act_pos[3],trigger_in);
  trigger_out1=trig_gen_fsm1(millis(),T1);
  End_step_fsm(trigger_in1);
//Serial.println(RG_in);
//Serial.println(servo_goto[1]);
//Serial.println(servo_goto[2]);
servo1.write(servo_goto[0]);
servo2.write(servo_goto[1]);
servo3.write(servo_goto[2]);
servo4.write(servo_goto[3]);

distance_in=distance_out;
distanceM_in=distanceM_out;
trigger_in=trigger_out;
trigger_in1=trigger_out1;
trigger_in2=trigger_out2;
FP1_in=FP1_out;
FP2_in=FP2_out;
FP3_in=FP3_out;
FP4_in=FP4_out;
R1_in=R1_out;
R2_in=R2_out;
R3_in=R3_out;
R4_in=R4_out;
RG_in=RG_out;
F2_in=F2_out;
S2_in=S2_out;
S3_in=S3_out;
}