FlexiPro by VUB

#include <Servo.h> 
//Define the pin numbers 
const int JoystickH = 0;
const int JoystickV = 1;
const int PotmeterH = 4;
const int PotmeterV = 5;
const int ServoHPin = 11;
const int ServoVPin = 9;
const int ServoGripperPin = 10;
const int EMG= 3;

//Define the constants for the inputs and outputs
float SpeedX;
float SpeedY;
float VoltagePotH;
float VoltagePotV;
float RotationsPotH;
float RotationsPotV;
int EMG_Value; 
int JoystickX;
int JoystickY;

//Define resting positions of the servos
const int RestPosX = 65;
const int RestPosY = 88;

//Define the threshold for the EMG sensor
const int EMG_threshold = 250;

//Define servo objects
Servo ServoH;
Servo ServoV;
Servo ServoGripper;
  
void setup() {
  //This code is run only once, in the setup phase
  Serial.begin(9600);
  
  //Set the pin mode for the analog inputs
  pinMode(JoystickH, INPUT);
  pinMode(JoystickV, INPUT);
  pinMode(PotmeterV, INPUT);
  pinMode(PotmeterH, INPUT);
 
  //Add the pins to the right servos
  ServoH.attach(ServoHPin);
  ServoV.attach(ServoVPin);
  ServoGripper.attach(ServoGripperPin);
}

void loop() {
  //This code is executed constantly

  //Read the joystick values
  JoystickX = analogRead(JoystickH); 
  JoystickY = analogRead(JoystickV);

  //Read the potentiometer values
  VoltagePotH = analogRead(PotmeterH);
  VoltagePotV = analogRead(PotmeterV);

  //Convert the potentiometer readings to the amount of rotations
  RotationsPotH =  VoltagePotH*5/1024*2;
  RotationsPotV =  VoltagePotV*5/1024*2; 

 
  ////////////////////////////////////
  //  Code for the first direction  //
  ////////////////////////////////////

  if (RotationsPotH > 4 && RotationsPotH < 6)
  //Rotations are in the safe range
    {
      if (JoystickX < 300)
        {
          //Convert to a speed linear with the joystick value
          SpeedX = (512-JoystickX)/512*180;
          ServoH.write(SpeedX); //Send to servo
        } 
      else if (JoystickX > 750)
        {
          //Convert to a speed linear with the joystick value
          SpeedX = (JoystickX-512)/512*180;
          ServoH.write(-1*SpeedX); //Send to servo
        }
      else
        {
          //Servo should not move
          ServoH.write(RestPosX);
        }
    }
      
  else if (RotationsPotH < 4)
    //No longer in the safe zone, disable further movement in this direction
    {
      if (JoystickX > 750)
        {
          //It can still move in this direction
          SpeedX = (JoystickX-512)/512*180;
          ServoH.write(-1*SpeedX);
        }
      else 
        {
          //No more movement permitted in this direction
          ServoH.write(RestPosX);
        }
    }
  
  else if (rotationsPotH > 6)
    //No longer in the safe zone, disable further movement in this direction
    {
      if (JoystickX < 300)
        {
          //It can still move in this direction
          SpeedX = (512-JoystickX)/512*180;
          ServoH.write(SpeedX);
        }
      else 
        {
          //No more movement permitted in this direction
          ServoH.write(RestPosX);
        }
    }
   

  /////////////////////////////////////
  //  Code for the second direction  //
  /////////////////////////////////////
     
  if (RotationsPotV > 4 && RotationsPotV < 6)
  //Rotations are in the safe range
    {
      if (JoystickY < 300)
        {
          //Move at full speed
          ServoV.write(180); //Send to servo
        } 
      else if (JoystickY > 750)
        {
          //Move at full speed
          ServoV.write(-180); //Send to servo
        }
      else
        {
          //Servo should not move
          ServoV.write(RestPosY);
        }
    }

  else if (RotationsPotV < 4)
    //No longer in the safe zone, disable further movement in this direction
    {
      if (JoystickY < 300)
        {
          //It can still move in this direction
          ServoV.write(180);
        }
      else 
        {
          //No more movement permitted in this direction
          ServoV.write(RestPosY);
        }
    }
  else if (RotationsPotV > 6)
    //No longer in the safe zone, disable further movement in this direction
    {
      if (JoystickY > 750)
        {
          //It can still move in this direction
          ServoV.write(-180);
        }
      else 
        {
          //No more movement permitted in this direction
          ServoV.write(RestPosY);
        }
    }
   

  ///////////////////////////////
  //  Code for the EMG sensor  //
  ///////////////////////////////
 
  //Read EMG value
  EMG_Value = analogRead(EMG);
  
  if (EMG_Value < EMG_threshold)
    {
      //Muscle relaxed, open gripper
      ServoGripper.write(0);
    }
  else
    {
      //Muscle tensioned, close gripper 
      ServoGripper.write(180);
    } 
  
  //Wait 100 ms before starting a new loop
  delay(100);
}