Project showcase

Object Avoiding FSM Robot Arm © GPL3+

Obstacle avoiding robot arm implemented using finite state machines.

  • 1,744 views
  • 0 comments
  • 6 respects

Components and supplies

Ard nano
Arduino Nano R3
×1
Button
×1
6 DOF Robot Arm
×1
Mfr 25frf52 1k sml
Resistor 1k ohm
×1
51gzz5eu9pl. sx425
Ultrasonic Sensor - HC-SR04 (Generic)
×1
DC Power Supply 15A
×1
Servo MG995 or MG998
×6

About this project

Project Description

This project consists in programming a robotic arm to move a small object from a point A to a point B with the ability to detect an obstacle between these two positions and to find an alternative path to avoid it, by moving up and down along the Z axis until the object is no longer detected and then fixing the Z position and resuming movement to point B. To achieve this, an ultrasonic sensor was placed on the right side of the extremity of the arm, just below the grip.

The programming was implemented using the concept of Finite State Machines (FSM), which consist of a finite number of states in which the machine can be in at any given time. The advantage of using FSM's is that the time the main loop takes to finish is minimal (milliseconds) and each FSM evaluates the current inputs to generate the right outputs, by doing this, it allows the program to seem asynchronous as it seems to do several tasks at the same time. This allows the robot arm to do diagonal movements (by moving several servos at the same time) or to avoid obstacles while moving without the need to stop the current movement.

These states are linked to each other, which means it needs to transition through the states to do its function. Each state has a specific output and to transition to the next state it needs to verify certain conditions that are associated to that transition. There are cases where the transition to the next state has no conditions, so this transition happens in the next loop cycle. Every machine starts on an initial state and ends the cycle when it reaches that state. This project uses several FSM’s, according to the architecture of the project.

Additionally, interruptions were used for the ultrasonic sensor and for the obstacle. An interruption consists in stopping the program given certain conditions and run a particular piece of code that is associated to it. After running that code, it resumes the program where it stopped. There are two types of interruptions, internal and external. The internal interruptions are basically timers, that activates each time a defined clock cycle has passed. The external interruptions monitor a pin and wait for a certain change on that pin to activate. The change can be of four types, CHANGE(0->1 or 1->0), RISING (0->1), FALLING (1->0) and LOW (0). The FALLING configuration was used to activate the interrupt associated with the ultrasonic sensor, according to the datasheet, by doing this it will save the time of the echo, which happens when the signal of the pin changes from 1 to 0. On the Obstacle FSM the CHANGE configuration was used, due to the need to change a stimulus, which needs to be done each time the signal of the pin changes.

Of the 6 servos, only 5 were used. The servo that wasn’t used is responsible for the rotation of the grip, which wasn’t required for this project.

How to Add Servos to the Current Program (X – represents number of the servo you want to add):

1st Step:

  • Declaration of the variables of the servo
  • Declaration of an additional Global_val
  • Example:
int sX =/*servo pin*/;
intservoX_start = /*Starting position in degrees*/;
intFSM_GLOBAL_servoX_VAL;
Servo servoX; //servo X definition
byteFSM_servoX_state = 0;
intFSM_servoX_AP = servoX_start;//to avoid erratic movement
intFSM_servoX_FP = servoX_start;
intFSM_servoX_VAL = 0;
unsignedlong ts_servoX;

2nd Step (Setup):

  • Attaching servo to its corresponding pin in setup
  • Assigned starting position to the servo
  • Example:
servoX.attach(sX);
servoX.write(servoX_start);

3rd Step:

  • Add a new Servo FSM for the new servo (copy existing Servo FSM and replace all the existing variables for the ones of the new servo)
  • Call the new Servo FSM at the end of the loop
  • Example:
FSM_servoX(FSM_servoX_FP,FSM_GLOBAL_servoX_VAL);

4th Step:

  • Add new servos positions and Global_val in the states of the Global FSM(add starting position in the first state)
  • Example:
FSM_servoX_FP = servoX_start;
FSM_GLOBAL_servoX_VAL = 1;
FSM_servoX_FP= 25;
FSM_GLOBAL_servoX_VAL= 0;

Entity

As seen above the entity has two inputs, the ultrasonic sensor and the button, and six outputs that send a pulse to the electronic unit inside each servo which processes the signal and generates the movement of the motor.

Architecture

The architecture has nine FSM’s (1 ultrasonic sensor, 1 Obstacle, 1 Global Machine and 1 for each Servo) and two interrupts.

Ultrasonic FSM

The ultrasonic sensor works by emitting a wave (Trigger) and receiving its echo (Echo). Knowing that the wave travels at the speed of sound, the distance of the obstacle can be calculated by measuring the difference of time between the trigger and the echo and multiplying it by the speed of sound. The ultrasonic FSM has 3 states. The first executes the trigger, the second checks if the echo was received. If the echo was received the FSM changes to the next state, if the echo was not received and if a predefined time has passed since the trigger(timeout), the FSM returns to the initial state. The third state calculates the distance, since the trigger and the echo happened within an acceptable amount of time of each other (condition of transition from the second state to the third). The pin of the echo is connected to the interrupt pin, which means that when the sensor receives the echo, the signal of the pin changes and activates the interrupt, which in turn saves the time when the echo was received. This kind of sensor can have interferences or can easily malfunction, which means that it may not receive the echo, making the time of the trigger useless and the calculus of the distance wrong. For this reason, the program checks if a predefined time has passed since the trigger was sent(timeout), and if it has passed the whole cycle restarts. As the distance that the sensor can measure is smaller than 10meters the timeout is defined as 30000 µs, which is approximately 10 meters in distance.

Obstacle FSM

The Obstacle FSM is responsible for generating the stimulus that will activate the interrupt. This happens each time the state changes because the interrupt is on the CHANGE configuration, as stated above. The distance given by the ultrasonic FSM will influence the stimulus to the Obstacle FSM. If the distance is between 2 and 20 cm (obstacle present) it changes the stimulus to 1, which then makes the FSM to change state, changing the signal in the pin of the interrupt, activating it. When the distance is outside the defined interval (no obstacle) it changes the stimulus to 0, which instead makes the FSM to change state, changing the signal in the pin of the interrupt, activating it. Each time this interrupt is activated, it changes the stimulus that goes to the Global FSM, indicating the presence of an obstacle or not. The minimum distance is defined as 2 cm according to the minimum working distance specified on the datasheet.

Global FSM

The Global FSM is responsible for controlling the positions of the robotic arm. To do this it sends the final position(FP) to each servo (each servo has a different FP), in specific states, that represents a specific position of the arm as a whole. To control the state of the Global FSM three main inputs were implemented, the button to initiate the cycle of movement of the arm, the stimulus of the interrupt that makes the FSM exit (obstacle present) or return (no obstacle) to the normal movement, and enter (obstacle present) or exit(no obstacle) the cycle of finding a path to avoid the obstacle, respectively. The third input is a variable of validation of each servo that validates that the servo has reached the given FP (Validates when Servo Val = 1). When all the servos have reached the FP’s then the Global FSM transitions to the next state.

Servo FSM

The Servo FSM moves the servo from one side to the other according to the FP and the actual position(AP), or indicates to the Global FSM that the servo has reached the FP. Additionally, it has a delay in either movement to make sure that the servo has time to physically transition and to avoid extremely fast movements, which might damage the servo.Each servo has its own Servo FSM. When the Global FSM sends a new FP, the Servo FSM checks if AP<FP or if AP>FP and changes to the according state with the corresponding movement (AP + step or AP – step, respectively). The step corresponds to the number of degrees the servos move on each loop cycle. It is defined as 1 degree so that the movement isn’t too fast. When the servo finally reaches the FP (AP = FP), the Servo FSM changes to the state in which the ServoVal = 1. It stays in this state until the Global FSM sends a stimulus (GlobalVal = 1) that allows for the Servo FSM to return to the initial state (ServoVal = 0). This happens to all the servos at the same time. Then it stays in this state until it receives a new FP. The servos have to wait for the GlobalVal because each servo will take a different amount of loop cycles to reach their respective FP, in this way it can be guaranteed that they all have reached the FP and that they all start to move to the next position at the same time.

Code

FSM_robot_arm.inoC/C++
#include <Servo.h>

//Ultrasonic sensor without pulsein
unsigned long Ttrig; //Time of trigger
unsigned long Techo; //Time of echo
int trigger_pin = 12;
int echo_pin = 3;
int FSM_ultrasonic_state = 0;
byte echoRead = 0;
float distance;
unsigned long duration = 0;
int reset_echoRead = 0;
//Ultrasonic sensor without pulsein

//SERVO pin Config
int s0 = 4;
int s1 = 5;
int s2 = 6;
int s3 = 7;
int s4 = 8;


//SERVO START POSITION
int servo0_start = 90; //90
int servo1_start = 105;
int servo2_start = 85;
int servo3_start = 60;
int servo4_start = 100;


const byte interruptPin = 2;
const byte startButton = 13;
byte FSM_global_is_actual = 0;
byte FSM_global_is_before = 0;
int stimulus_interrupt = 1;

//FSM Obstacle
byte FSM_Obstacle_state = 0;

//NUMBER OF STEPS in each loop CYCLE
byte stepNumber = 1;

int servo_delay = 30;

//FSM GLOBAL VALs
int FSM_GLOBAL_servo0_VAL;
int FSM_GLOBAL_servo1_VAL;
int FSM_GLOBAL_servo2_VAL;
int FSM_GLOBAL_servo3_VAL;
int FSM_GLOBAL_servo4_VAL;

//FSM Servo 0
Servo servo0; //servo 0 definition
byte FSM_servo0_state = 0;
int FSM_servo0_AP = servo0_start;//to avoid erratic movement
int FSM_servo0_FP = servo0_start;
int FSM_servo0_VAL = 0;
unsigned long ts_servo0;

//FSM Servo 1
Servo servo1; //servo 1 definition
byte FSM_servo1_state = 0;
int FSM_servo1_AP = servo1_start;//to avoid erratic movement
int FSM_servo1_FP = servo1_start;
int FSM_servo1_VAL = 0;
unsigned long ts_servo1;

//FSM Servo 2
Servo servo2; //servo 2 definition
byte FSM_servo2_state = 0;
int FSM_servo2_AP = servo2_start;//to avoid erratic movement
int FSM_servo2_FP = servo2_start;
int FSM_servo2_VAL = 0;
unsigned long ts_servo2;

//FSM Servo 3
Servo servo3; //servo 3 definition
byte FSM_servo3_state = 0;
int FSM_servo3_AP = servo3_start;//to avoid erratic movement
int FSM_servo3_FP = servo3_start;
int FSM_servo3_VAL = 0;
unsigned long ts_servo3;

//FSM Servo 4
Servo servo4; //servo 4 definition
byte FSM_servo4_state = 0;
int FSM_servo4_AP = servo4_start;//to avoid erratic movement
int FSM_servo4_FP = servo4_start;
int FSM_servo4_VAL = 0;
unsigned long ts_servo4;

// FSM GLOBAL
byte FSM_global_state = 0;


////Setup
void setup() {
  Serial.begin(9600);
  pinMode(startButton, INPUT);
  pinMode(interruptPin, OUTPUT); //Interrupt associated with the Obstacle FSM
  
  servo0.attach(s0); // attach Servo 0 to pin
  servo1.attach(s1); // attach Servo 1 to pin
  servo2.attach(s2); // attach Servo 2 to pin
  servo3.attach(s3); // attach Servo 3 to pin
  servo4.attach(s4); // attach Servo 4 to pin
  servo0.write(servo0_start); //inicializing positions equal to first position in Global FSM.
  servo1.write(servo1_start); //This is to prevent erratic movement of the servos
  servo2.write(servo2_start);
  servo3.write(servo3_start);
  servo4.write(servo4_start);
                
  digitalWrite(interruptPin, LOW); // RESET Interrupt
  attachInterrupt(digitalPinToInterrupt(interruptPin), call_interrupt, CHANGE);

  //Sensor ultrasonico no pulsein
  pinMode(trigger_pin, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(echo_pin), getTime, FALLING);
  //Sensor ultrasonico no pulsein
  
}
////END setup

////loop
void loop() {
  
  duration = Techo - Ttrig;
    
  if(reset_echoRead && echoRead){
    echoRead = 0;
  }

  int stimulus = 0;
  FSM_ultrasonic(echoRead, reset_echoRead); //FSM_ultrasonic; measures distance
  if (distance<20 && distance>2){ //checks if distance is within the interval
    stimulus = 1;
  }
  FSM_Obstacle(stimulus); //FSM_Obstacle

  //Stimulus for FSM global
  if (FSM_global_is_actual == 1 && FSM_global_is_before == 0){
    stimulus_interrupt = 0;
    } else if (FSM_global_is_actual == 0 && FSM_global_is_before == 1){
    stimulus_interrupt = 1;
    }
    
  FSM_global(digitalRead(startButton), stimulus_interrupt); //FSM global
  
  FSM_servo0(FSM_servo0_FP, FSM_GLOBAL_servo0_VAL); //FSM_servo0
  FSM_servo1(FSM_servo1_FP, FSM_GLOBAL_servo1_VAL); //FSM_servo1
  FSM_servo2(FSM_servo2_FP, FSM_GLOBAL_servo2_VAL); //FSM_servo2
  FSM_servo3(FSM_servo3_FP, FSM_GLOBAL_servo3_VAL); //FSM_servo3
  FSM_servo4(FSM_servo4_FP, FSM_GLOBAL_servo4_VAL); //FSM_servo4
}
////END loop


void call_interrupt() { //Interrupt activated by FSM_obstacle
  if(FSM_global_is_actual){
    FSM_global_is_actual = 0;
    FSM_global_is_before = 1;
    } else {
      FSM_global_is_actual = 1;
      FSM_global_is_before = 0;
      }
}


///// FSM GLOBAL - Controls all servos
void FSM_global(int button, int interrupt){
  FSM_global_nextstate(button,interrupt);
  FSM_global_output();
}

void FSM_global_nextstate(int button, int interrupt){
  switch(FSM_global_state){
    case 0:
      if(button){
        FSM_global_state = 1;
      }
    break;
    case 1:
    //POS 1
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){
        FSM_global_state = 2;
      }
    break;
    case 2:
      FSM_global_state = 3;
    break;
    case 3:
    //POS 2
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL*FSM_servo4_VAL){
        FSM_global_state = 4;
      }
    break;
    case 4:
      FSM_global_state = 5;
    break;
    case 5:
    //POS 3 Hand opens
      if(FSM_servo4_VAL){
        FSM_global_state = 6;
      }
    break;
    case 6:
      FSM_global_state = 7;
    break;
    case 7:
    //POS 4 Hand closes
      if(FSM_servo4_VAL){
        FSM_global_state = 8;
      }
    break;
    case 8:
      FSM_global_state = 9;
    break;
    case 9:
    //POS 5 = POS 1 => POS A
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 10;
      }
    break;
    case 10:
      FSM_global_state = 11;
    break;
    case 11:
    //POS 6 = POS 1 with servo0 different => POS B
      if(interrupt) FSM_global_state = 20; // POS 6
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 12;
      }
    break;
    case 12:
      FSM_global_state = 13;
    break;
    case 13:
    //POS 7 = POS 2 with servo0 different
      if(FSM_servo0_VAL*FSM_servo1_VAL*FSM_servo2_VAL*FSM_servo3_VAL){
        FSM_global_state = 14;
      }
    break;
    case 14:
      FSM_global_state = 15;
    break;
    case 15:
    //POS 8 Hand opens
      if(FSM_servo4_VAL){
        FSM_global_state = 16;
      }
    break;
    case 16:
      FSM_global_state = 0;
    break;
    case 20: //state for search alternative Z position upward
      if (!interrupt) FSM_global_state = 10;
      if(FSM_servo2_VAL){
      FSM_global_state = 21;
      }
    break;
    case 21:
      FSM_global_state = 22;
    break;
    case 22: //state for search alternative Z position downward
      if (!interrupt) FSM_global_state = 10;
      if(FSM_servo2_VAL){ 
      FSM_global_state = 23;
      }
    break;
    case 23:
      FSM_global_state = 20;
    break;
  
  }
}

void FSM_global_output(){
    switch(FSM_global_state){
    case 0:
      //waiting to start
      FSM_servo0_FP = servo0_start;
      FSM_servo1_FP = servo1_start;
      FSM_servo2_FP = servo2_start;
      FSM_servo3_FP = servo3_start;
      FSM_servo4_FP = servo4_start;
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 1:
    //POS 1
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 130;
      FSM_servo2_FP = 30;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 2:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 3:
    //POS 2
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 65;
      FSM_servo2_FP = 5;
      FSM_servo3_FP = 140;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 4:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 5:
    //POS 3 Hand opens
      FSM_servo4_FP = 15;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 6:
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 7:
    //POS 4 Hand closes
      FSM_servo4_FP = 100;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 8:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 9:
    //POS 5 = POS 1 => POS A
      FSM_servo0_FP = 170;
      FSM_servo1_FP = 130;
      FSM_servo2_FP = 30;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 10:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
      FSM_servo2_FP = FSM_servo2_AP;//keeps z position after obstacle avoidance
    break;
    case 11:
    //POS 6 = POS 1 with servo0 different => POS B
      FSM_servo0_FP = 30;
      FSM_servo1_FP = 130;
      FSM_servo3_FP = 25;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 12:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 13:
    //POS 7 = POS 2 with servo0 different
      FSM_servo0_FP = 30;
      FSM_servo1_FP = 65;
      FSM_servo2_FP = 5;
      FSM_servo3_FP = 140;
      FSM_GLOBAL_servo0_VAL = 0;
      FSM_GLOBAL_servo1_VAL = 0;
      FSM_GLOBAL_servo2_VAL = 0;
      FSM_GLOBAL_servo3_VAL = 0;
    break;
    case 14:
      FSM_GLOBAL_servo0_VAL = 1;
      FSM_GLOBAL_servo1_VAL = 1;
      FSM_GLOBAL_servo2_VAL = 1;
      FSM_GLOBAL_servo3_VAL = 1;
    break;
    case 15:
    //POS 8 Hand opens
      FSM_servo4_FP = 15;
      FSM_GLOBAL_servo4_VAL = 0;
    break;
    case 16:
      FSM_GLOBAL_servo4_VAL = 1;
    break;
    case 20: //state for search alternative Z position upward
      FSM_servo2_FP = 60;
      FSM_servo0_FP = FSM_servo0_AP;
      FSM_servo1_FP = FSM_servo1_AP;
      FSM_servo3_FP = FSM_servo3_AP;
      FSM_GLOBAL_servo2_VAL = 0;
    break;
    case 21:
      FSM_GLOBAL_servo2_VAL = 1;
    break;
    case 22: //state for search alternative Z position downward
      FSM_servo2_FP = 5;
      FSM_GLOBAL_servo2_VAL = 0;
    break;
    case 23:
      FSM_GLOBAL_servo2_VAL = 1;
    break;
  }

}
// END FSM GLOBAL

///// FSM SERVO 0
void FSM_servo0(int FSM_servo0_FP, int FSM_GLOBAL_servo0_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo0) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo0_nextstate((FSM_servo0_AP < FSM_servo0_FP),(FSM_servo0_AP > FSM_servo0_FP),(FSM_servo0_AP == FSM_servo0_FP),FSM_GLOBAL_servo0_VAL, ts_stimulus);
  FSM_servo0_output();
}

void FSM_servo0_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo0_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo0_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo0_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo0_state = 1;
    }
    break;
    case 1:
      FSM_servo0_state = 4;
    break;
    case 2:
      FSM_servo0_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo0_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo0_state = 0;
    }
    break;
  }
}

void FSM_servo0_output(){
  switch(FSM_servo0_state){
    case 0:
    FSM_servo0_VAL = 0;
    break;
    case 1:
      FSM_servo0_AP = FSM_servo0_AP - stepNumber;
      servo0.write(FSM_servo0_AP);
      ts_servo0 = millis();
    break;
    case 2:
      FSM_servo0_AP = FSM_servo0_AP + stepNumber;
      servo0.write(FSM_servo0_AP);
      ts_servo0 = millis();
    break;
    case 3:
      FSM_servo0_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 0

///// FSM SERVO 1
void FSM_servo1(int FSM_servo1_FP, int FSM_GLOBAL_servo1_VAL){
  int ts_stimulus;
  if((millis() - ts_servo1) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }
  
  FSM_servo1_nextstate((FSM_servo1_AP < FSM_servo1_FP),(FSM_servo1_AP > FSM_servo1_FP),(FSM_servo1_AP == FSM_servo1_FP), FSM_GLOBAL_servo1_VAL, ts_stimulus);
  FSM_servo1_output();
}

void FSM_servo1_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo1_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo1_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo1_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo1_state = 1;
    }
    break;
    case 1:
      FSM_servo1_state = 4;
    break;
    case 2:
      FSM_servo1_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo1_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo1_state = 0;
    }
    break;
  }
}

void FSM_servo1_output(){
  switch(FSM_servo1_state){
    case 0:
    FSM_servo1_VAL = 0;
    break;
    case 1:
      FSM_servo1_AP = FSM_servo1_AP - stepNumber;
      servo1.write(FSM_servo1_AP);
      ts_servo1 = millis();
    break;
    case 2:
      FSM_servo1_AP = FSM_servo1_AP + stepNumber;
      servo1.write(FSM_servo1_AP);
      ts_servo1 = millis();
    break;
    case 3:
      FSM_servo1_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 1

///// FSM SERVO 2
void FSM_servo2(int FSM_servo2_FP, int FSM_GLOBAL_servo2_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo2) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo2_nextstate((FSM_servo2_AP < FSM_servo2_FP),(FSM_servo2_AP > FSM_servo2_FP),(FSM_servo2_AP == FSM_servo2_FP), FSM_GLOBAL_servo2_VAL, ts_stimulus);
  FSM_servo2_output();
}

void FSM_servo2_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo2_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo2_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo2_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo2_state = 1;
    }
    break;
    case 1:
      FSM_servo2_state = 4;
    break;
    case 2:
      FSM_servo2_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo2_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo2_state = 0;
    }
    break;
  }
}

void FSM_servo2_output(){
  switch(FSM_servo2_state){
    case 0:
      FSM_servo2_VAL = 0;
    break;
    case 1:
      FSM_servo2_AP = FSM_servo2_AP - stepNumber;
      servo2.write(FSM_servo2_AP);
      ts_servo2 = millis();
    break;
    case 2:
      FSM_servo2_AP = FSM_servo2_AP + stepNumber;
      servo2.write(FSM_servo2_AP);
      ts_servo2 = millis();
    break;
    case 3:
      FSM_servo2_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 2


///// FSM SERVO 3
void FSM_servo3(int FSM_servo3_FP, int FSM_GLOBAL_servo3_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo3) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo3_nextstate((FSM_servo3_AP < FSM_servo3_FP),(FSM_servo3_AP > FSM_servo3_FP),(FSM_servo3_AP == FSM_servo3_FP), FSM_GLOBAL_servo3_VAL, ts_stimulus);
  FSM_servo3_output();
}

void FSM_servo3_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo3_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo3_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo3_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo3_state = 1;
    }
    break;
    case 1:
      FSM_servo3_state = 4;
    break;
    case 2:
      FSM_servo3_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo3_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo3_state = 0;
    }
    break;
  }
}

void FSM_servo3_output(){
  switch(FSM_servo3_state){
    case 0:
      FSM_servo3_VAL = 0;
    break;
    case 1:
      FSM_servo3_AP = FSM_servo3_AP - stepNumber;
      servo3.write(FSM_servo3_AP);
      ts_servo3 = millis();
    break;
    case 2:
      FSM_servo3_AP = FSM_servo3_AP + stepNumber;
      servo3.write(FSM_servo3_AP);
      ts_servo3 = millis();
    break;
    case 3:
      FSM_servo3_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 3


///// FSM SERVO 4
void FSM_servo4(int FSM_servo4_FP, int FSM_GLOBAL_servo4_VAL){ 
  int ts_stimulus;
  if((millis() - ts_servo4) > servo_delay){ //Checks Delay
    ts_stimulus = 1;
  }else{
    ts_stimulus = 0;
  }

  
  FSM_servo4_nextstate((FSM_servo4_AP < FSM_servo4_FP),(FSM_servo4_AP > FSM_servo4_FP),(FSM_servo4_AP == FSM_servo4_FP),FSM_GLOBAL_servo4_VAL, ts_stimulus);
  FSM_servo4_output();
}

void FSM_servo4_nextstate(int m, int M, int I, int val, int ts_stimulus){
  switch(FSM_servo4_state){
    case 0:
    if(I && !val){ //IF AP == FP
      FSM_servo4_state = 3;
    } else if(m){ //IF AP is smaller
      FSM_servo4_state = 2;
    }else if(M){ //IF AP is bigger
      FSM_servo4_state = 1;
    }
    break;
    case 1:
      FSM_servo4_state = 4;
    break;
    case 2:
      FSM_servo4_state = 4;
    break;
    case 3:
    if (val == 1) FSM_servo4_state = 0;
    break;
    case 4:
    if(ts_stimulus){
      FSM_servo4_state = 0;
    }
    break;
  }
}

void FSM_servo4_output(){
  switch(FSM_servo4_state){
    case 0:
    FSM_servo4_VAL = 0;
    break;
    case 1:
      FSM_servo4_AP = FSM_servo4_AP - stepNumber;
      servo4.write(FSM_servo4_AP);
      ts_servo4 = millis();
    break;
    case 2:
      FSM_servo4_AP = FSM_servo4_AP + stepNumber;
      servo4.write(FSM_servo4_AP);
      ts_servo4 = millis();
    break;
    case 3:
      FSM_servo4_VAL = 1;
    break;
    case 4:
    break;
  }
}
// END FSM SERVO 4


///// FSM Obstacle
void FSM_Obstacle(int stimulus){
  
  FSM_Obstacle_nextstate(stimulus);
  FSM_Obstacle_output();
}

void FSM_Obstacle_nextstate(int stimulus){
  switch(FSM_Obstacle_state){
    case 0:
      if(stimulus){
        FSM_Obstacle_state = 1;
      }
    break;
    case 1:
      if(!stimulus){
        FSM_Obstacle_state = 0;
        }
    break;
  }
}

void FSM_Obstacle_output(){
  switch(FSM_Obstacle_state){
    case 0: //Activates interrupt
      digitalWrite(interruptPin, LOW);
    break;
    case 1: //Activates interrupt
      digitalWrite(interruptPin, HIGH);
    break;
  }
}
// END FSM Obstacle


  //Ultrasonic sensor without pulsein
  void getTime(){ //Interrupt Function connected with the echo pin of the Ultrasonic sensor
  Techo = micros();
  echoRead = 1;
}

//Ultrasonic FSM without pulseIn Function
void FSM_ultrasonic(byte echoRead, int reset_echoRead){ 
  
  FSM_ultrasonic_nextstate(echoRead);
  FSM_ultrasonic_output(reset_echoRead);
}

void FSM_ultrasonic_nextstate(byte echoRead){
  switch(FSM_ultrasonic_state){
    case 0:
      FSM_ultrasonic_state = 1;
    break;
    case 1:
      if(echoRead){
          FSM_ultrasonic_state = 2;
        }
      if((micros() - Ttrig) > 30000){ //Timeout
        FSM_ultrasonic_state = 0;
      }
      
    break;
    case 2:
      FSM_ultrasonic_state = 0;
    break;
  }
}

void FSM_ultrasonic_output(int reset_echoRead_local){ //////////////////No se est a utilizar o reset_echoRead_local/////////////////////////////////////
  switch(FSM_ultrasonic_state){
    case 0: //Sends trigger
      // Clears the trigPin
      digitalWrite(trigger_pin, LOW);
      delayMicroseconds(1);
      // Sets the trigPin on HIGH state for 10 micro seconds
      digitalWrite(trigger_pin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigger_pin, LOW);

      Ttrig = micros();
    break;
    case 1:
      reset_echoRead = 1; ///////////////////////////////////////Porqu aqui?????////////////////////////////////////////////////
    break;
    case 2: //Receives echo
      if(echoRead){
        if(!duration){ //If duration is zero, avoids negative distance
          distance = 0;
        }else{
          distance = duration*0.034/2 - 7.28;
        }
        reset_echoRead = 1;
      }
    break;
   
    
  }
}
//END Ultrasonic FSM without pulseIn Function

Schematics

Schematic
32913822 1910628402295325 1959588432346873856 n p0lvbn4erz

Comments

Similar projects you might like

Arduino Bluetooth Basic Tutorial

by Mayoogh Girish

  • 455,940 views
  • 44 comments
  • 242 respects

Home Automation Using Raspberry Pi 2 And Windows 10 IoT

Project tutorial by Anurag S. Vasanwala

  • 288,799 views
  • 95 comments
  • 676 respects

Security Access Using RFID Reader

by Aritro Mukherjee

  • 231,178 views
  • 38 comments
  • 241 respects

OpenCat

Project in progress by Team Petoi

  • 196,887 views
  • 154 comments
  • 1,369 respects
Add projectSign up / Login