Project showcase

Hand Gesture Controlled Robotic Arm © GPL3+

I use accelerometer,gyroscope,magnetometer sensors to control the movement of the base rotation, shoulder, wrist pitch on robotic arm.

  • 596 views
  • 0 comments
  • 0 respects

Components and supplies

About this project

Robot is a mechanical device that can perform physical tasks. On of types of robots that are very popular and simplest is the robotic arm system. Robotic arm is a mechanical system which is used in manipulating the movement of lifting,moving, and manipulating the workpiece to lighten the work of man.

The robotic arm of this project is controlled by hand gesture. Accelerometer, gyroscope and magnetometer sensors is mounted on human hand so that the user can control the movement of the base rotation, shoulder, wrist pitch on robotic arm. The Flex sensor mounted on finger is used to control the movement of the gripper.

As for controlling the speed of a DC motor,a fuzzy logic control method is used in order to minimize the position error rate and shorten the response time required for robotic arm when it reaches the desired position according to the user's hand gestures.

Code

Hand_Gesture_Robotic_Arm.inoC/C++
#include <AFMotor.h>
#include <Wire.h>
#include <I2Cdev.h>
#define RESTRICT_PITCH 
#define RESTRICT_PITCH2
#define ROTARY_ANGLE_SENSOR A8
#define ROTARY_ANGLE_SENSOR2 A2
#define ROTARY_ANGLE_SENSOR3 A10
#define ADC_REF 5 //Rreference voltage of ADC is 3.3v
#define FULL_ANGLE 300.0 //Full value of the rotary angle is 300 degrees


int flexSensorPin = A0;
int set1=2;
int set2=3;
int set3=4;
int set4=5;
int set5=6;
int set6=7;
int set7=13;
int set8=12;
int enA=9;
int enB=10;
int enC=11;
int enD=8;


float  kalmanYY_f,kalmanYY_f2,x1=80,x2=0,y1=0,y2=80,y3=0,y4=180,x3=180,x4=0,sp=0,sp2=0,sp3=0,ei=0,eii=0,eii2=0,eii3=0,de=0,pv=0,pv2=0,pv3=0;
int kalmanYY,kalmanYY2;

float neb=-70,nes=-40,azel=-5,azeh=5,pes=40,peb=70;
float ndeb=-40,ndes=-30,azdel=-5,azdeh=5,pdes=30,pdeb=40;
float uneb,unes,uazel,uaze,uazeh,upes,upeb,undeb,undes,uazdel,uazde,uazdeh,updes,updeb,outdefuzzy, outdefuzzy2, outdefuzzy3;
float ka_sc=-200,ka_c=-100,stp=0,ki_c=100,ki_sc=200;
float a,b,c,d,e,f,g,h,i;

    
//---------------------------------ALGORITMA KALMAN FILTER--------------------------------------------------------
class Kalman {
public:
    Kalman() {
        
        Q_angle = 0.001;
        Q_bias = 0.003;
        R_measure = 0.03;

        angle = 0; // Reset the angle
        bias = 0; // Reset bias

        P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
        P[0][1] = 0;
        P[1][0] = 0;
        P[1][1] = 0;
    };
    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    double getAngle(double newAngle, double newRate, double dt) {
      
        // Discrete Kalman filter time update equations - Time Update ("Predict")
        // Update xhat - Project the state ahead
        /* Step 1 */
        rate = newRate - bias;
        angle += dt * rate;

       // Update prediksi kovarian error
        /* Step 2 */
        P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
        P[0][1] -= dt * P[1][1];
        P[1][0] -= dt * P[1][1];
        P[1][1] += Q_bias * dt;

        // Perhitungan kalman disktrit - Measurement Update ("Correct")
        
        /* Step 4 */
        S = P[0][0] + R_measure;
        
        // Mendapatkan Kalman Gain
        /* Step 5 */
        K[0] = P[0][0] / S;
        K[1] = P[1][0] / S;

        // Menghitung angle dan bias - untuk mencarai z_k (newAngle)
        /* Step 3 */
        y = newAngle - angle;
        /* Step 6 */
        angle += K[0] * y;
        bias += K[1] * y;

        // Update kovarian error
        /* Step 7 */
        P[0][0] -= K[0] * P[0][0];
        P[0][1] -= K[0] * P[0][1];
        P[1][0] -= K[1] * P[0][0];
        P[1][1] -= K[1] * P[0][1];

        return angle;
    };
    void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
    double getRate() { return rate; }; // Return the unbiased rate

    /* These are used to tune the Kalman filter */
    void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
    void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
    void setRmeasure(double newR_measure) { R_measure = newR_measure; };

    double getQangle() { return Q_angle; };
    double getQbias() { return Q_bias; };
    double getRmeasure() { return R_measure; };

private:
    /* Kalman filter variables */
    double Q_angle; // Process noise variance for the accelerometer
    double Q_bias; // Process noise variance for the gyro bias
    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise

    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
    double K[2]; // Kalman gain - This is a 2x1 vector
    double y; // Angle difference
    double S; // Estimate error
};


Kalman kalmanX, kalmanY,kalmanX2, kalmanY2, kalmanZ; // Create the Kalman instances

const uint8_t MPU6050 = 0x68;// If AD0 is logic low on the PCB the address is 0x68, otherwise set this to 0x69
const uint8_t MPU6050_2 = 0x69;
const uint8_t HMC5883L = 0x1E; // Address of magnetometer

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
double accX2, accY2, accZ2;
double gyroX2, gyroY2, gyroZ2;
double magX, magY, magZ;
int16_t tempRaw;

double roll,roll2, pitch, pitch2, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer

double gyroXangle, gyroYangle, gyroZangle,gyroXangle2, gyroYangle2, gyroZangle2; // Angle calculate using the gyro only
double compAngleX, compAngleY, compAngleZ,compAngleX2, compAngleY2, compAngleZ2; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY,kalAngleX2, kalAngleY2, kalAngleZ,kalAngleZ2; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

#define MAG0MAX 603
#define MAG0MIN -578

#define MAG1MAX 542
#define MAG1MIN -701

#define MAG2MAX 547
#define MAG2MIN -556

float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 };
double magGain[3];

void setup() {
  delay(100); // Wait for sensors to get ready

  Serial.begin(115200);
  Wire.begin();
  
  pinMode(set1, OUTPUT);
 pinMode(set2, OUTPUT);
 pinMode(set3, OUTPUT);
 pinMode(set4, OUTPUT);
 pinMode(set5, OUTPUT);
 pinMode(set6, OUTPUT);
 pinMode(set7, OUTPUT);
 pinMode(set8, OUTPUT);
 pinMode(enA, OUTPUT);
 pinMode(enB, OUTPUT);
 pinMode(enC, OUTPUT);
 pinMode(enD, OUTPUT);
 
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to 2g
  while (i2cWrite(MPU6050, 0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(MPU6050, 0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
   while (i2cWrite(MPU6050, 0x37, 0x02, true));
     while (i2cWrite(MPU6050, 0x6A, 0x00, true));


     
  while (i2cRead(MPU6050, 0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
    
  }
  
  while (i2cWrite(MPU6050_2, 0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(MPU6050_2, 0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

     
  

  while (i2cWrite(HMC5883L, 0x02, 0x00, true)); // Configure device for continuous mode
  calibrateMag();

  delay(100); // Wait for sensors to stabilize

  /* Set Kalman and gyro starting angle */
  updateMPU6050();
  updateMPU6050_2();
  updateHMC5883L();
  updatePitchRoll();
  updatePitchRoll2();
  updateYaw();

  kalmanX.setAngle(roll); // First set roll starting angle
  gyroXangle = roll;
  compAngleX = roll;

  kalmanY.setAngle(pitch); // Then pitch
  gyroYangle = pitch;
  compAngleY = pitch;
  
   kalmanX2.setAngle(roll2); // First set roll starting angle
  gyroXangle2 = roll2;
  compAngleX2 = roll2;

  kalmanY2.setAngle(pitch2); // Then pitch
  gyroYangle2 = pitch2;
  compAngleY2 = pitch2;

  kalmanZ.setAngle(yaw); // And finally yaw
  gyroZangle = yaw;
  compAngleZ = yaw;

  timer = micros(); // Initialize the timer
}

void loop() {
  /* Update all the IMU values */
  updateMPU6050();
   updateMPU6050_2();
  updateHMC5883L();

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();


  /* Roll and pitch estimation */
  updatePitchRoll();
  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s
  
    /* Roll and pitch estimation */
  updatePitchRoll2();
  double gyroXrate2 = gyroX2 / 131.0; // Convert to deg/s
  double gyroYrate2 = gyroY2 / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif

#ifdef RESTRICT_PITCH2
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll2 < -90 && kalAngleX2 > 90) || (roll2 > 90 && kalAngleX2 < -90)) {
    kalmanX2.setAngle(roll2);
    compAngleX2 = roll2;
    kalAngleX2 = roll2;
    gyroXangle2 = roll2;
  } else
    kalAngleX2 = kalmanX2.getAngle(roll2, gyroXrate2, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX2) > 90)
    gyroYrate2 = -gyroYrate2; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleY2 = kalmanY2.getAngle(pitch2, gyroYrate2, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch2 < -90 && kalAngleY2 > 90) || (pitch2 > 90 && kalAngleY2 < -90)) {
    kalmanY2.setAngle(pitch2);
    compAngleY2 = pitch2;
    kalAngleY2 = pitch2;
    gyroYangle2 = pitch2;
  } else
    kalAngleY2 = kalmanY2.getAngle(pitch2, gyroYrate2, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY2) > 90)
    gyroXrate2 = -gyroXrate2; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleX2= kalmanX2.getAngle(roll2, gyroXrate2, dt); // Calculate the angle using a Kalman filter
#endif


  /* Yaw estimation */
  updateYaw();
  double gyroZrate = gyroZ / 131.0; // Convert to deg/s
  // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
  if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
    kalmanZ.setAngle(yaw);
    compAngleZ = yaw;
    kalAngleZ = yaw;
    gyroZangle = yaw;
  } else
    kalAngleZ = kalmanZ.getAngle(yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter


  /* Estimate angles using gyro only */
  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  gyroZangle += gyroZrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
  //gyroYangle += kalmanY.getRate() * dt;
  //gyroZangle += kalmanZ.getRate() * dt;

  /* Estimate angles using complimentary filter */
  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
  compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
  
    /* Estimate angles using complimentary filter */
  compAngleX2 = 0.93 * (compAngleX2 + gyroXrate2 * dt) + 0.07 * roll2; // Calculate the angle using a Complimentary filter
  compAngleY2= 0.93 * (compAngleY2 + gyroYrate2 * dt) + 0.07 * pitch2;
  compAngleZ2 = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;


  // Reset the gyro angles when they has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
  if (gyroZangle < -180 || gyroZangle > 180)
    gyroZangle = kalAngleZ;
    
      // Reset the gyro angles when they has drifted too much
  if (gyroXangle2 < -180 || gyroXangle2 > 180)
    gyroXangle2 = kalAngleX2;
  if (gyroYangle2 < -180 || gyroYangle2 > 180)
    gyroYangle2 = kalAngleY2;
  if (gyroZangle2 < -180 || gyroZangle2 > 180)
    gyroZangle2 = kalAngleZ2;




  
   Serial.print(sp); Serial.print(",");
  Serial.print(pv); 
  


  
 
 Fuzzy();  
 Fuzzy2();
 Fuzzy3();
  Gripper();
 




}

void updateMPU6050() {
  while (i2cRead(MPU6050, 0x3B, i2cData, 14)); // Get accelerometer and gyroscope values
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = -((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = -(i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = -(i2cData[12] << 8) | i2cData[13];
}

void updateMPU6050_2() {
  while (i2cRead(MPU6050_2, 0x3B, i2cData, 14)); // Get accelerometer and gyroscope values
  accX2 = ((i2cData[0] << 8) | i2cData[1]);
  accY2 = -((i2cData[2] << 8) | i2cData[3]);
  accZ2 = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX2 = -(i2cData[8] << 8) | i2cData[9];
  gyroY2 = (i2cData[10] << 8) | i2cData[11];
  gyroZ2= -(i2cData[12] << 8) | i2cData[13];
}

void updateHMC5883L() {
  while (i2cRead(HMC5883L, 0x03, i2cData, 6)); // Get magnetometer values
  magX = ((i2cData[0] << 8) | i2cData[1]);
  magZ = ((i2cData[2] << 8) | i2cData[3]);
  magY = ((i2cData[4] << 8) | i2cData[5]);
}

void updatePitchRoll() {
  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of - to  (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
}

void updatePitchRoll2() {
  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of - to  (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH2 // Eq. 25 and 26
  roll2 = atan2(accY2, accZ2) * RAD_TO_DEG;
  pitch2 = atan(-accX2 / sqrt(accY2 * accY2 + accZ2 * accZ2)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll2 = atan(accY2 / sqrt(accX2 * accX2 + accZ2 * accZ2)) * RAD_TO_DEG;
  pitch2 = atan2(-accX2, accZ2) * RAD_TO_DEG;
#endif
}

void updateYaw() { // See: http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
  magX *= -1; // Invert axis - this it done here, as it should be done after the calibration
  magZ *= -1;

  magX *= magGain[0];
  magY *= magGain[1];
  magZ *= magGain[2];

  magX -= magOffset[0];
  magY -= magOffset[1];
  magZ -= magOffset[2];

  double rollAngle = kalAngleX * DEG_TO_RAD;
  double pitchAngle = kalAngleY * DEG_TO_RAD;

  double Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);
  double Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);
  yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;

  yaw *= -1;
}

void calibrateMag() { // Inspired by: https://code.google.com/p/open-headtracker/
  i2cWrite(HMC5883L, 0x00, 0x11, true);
  delay(100); // Wait for sensor to get ready
  updateHMC5883L(); // Read positive bias values

  int16_t magPosOff[3] = { magX, magY, magZ };

  i2cWrite(HMC5883L, 0x00, 0x12, true);
  delay(100); // Wait for sensor to get ready
  updateHMC5883L(); // Read negative bias values

  int16_t magNegOff[3] = { magX, magY, magZ };

  i2cWrite(HMC5883L, 0x00, 0x10, true); // Back to normal

  magGain[0] = -2500 / float(magNegOff[0] - magPosOff[0]);
  magGain[1] = -2500 / float(magNegOff[1] - magPosOff[1]);
  magGain[2] = -2500 / float(magNegOff[2] - magPosOff[2]);


  

 
 
}

void Gripper() {

  
 int  flexSensorReading = analogRead(flexSensorPin); 
  int flex=map(flexSensorReading, 350, 200, 0, 200 );

  Serial.println(flexSensorReading);
delay(200);

  
if(flexSensorReading<=330  && flexSensorReading>=300 ){
digitalWrite(set7, LOW);// turn it on going forward
digitalWrite(set8, HIGH);
analogWrite(enD, 100);

}

if(flexSensorReading<=295){
digitalWrite(set7, LOW);// turn it on going forward
digitalWrite(set8, LOW);
analogWrite(enD, 0);

}



if(flexSensorReading>=340 && flexSensorReading<=365) {
 digitalWrite(set7, HIGH);// turn it on going forward
digitalWrite(set8, LOW);
analogWrite(enD, -100);


}

if(flexSensorReading>=370){
digitalWrite(set7, LOW);// turn it on going forward
digitalWrite(set8, LOW);
analogWrite(enD, 0);

}


}





  

void Fuzzy() {
  
    //Read the raw sensor value
  int sensor_value = analogRead(ROTARY_ANGLE_SENSOR);
  
  //Convert the sensor reading to degrees and return that value
  float voltage = (float)sensor_value * ADC_REF / 1023; 
  float degrees = (voltage * FULL_ANGLE) / ADC_REF; 
  pv=(float)degrees;
 
 
  
  //-------------------ERROR dan DELTA ERROR-------------------------------------  
  sp=kalAngleY2;
  ei=sp-pv;
  de=ei-eii;
  eii=ei;

//--------------------ALGORITMA FUZZY LOGIC (WRIST)------------------------
   //1-1
if (ei<=neb && de<=ndeb)
{
uneb=1;
undeb=1;
outdefuzzy=ka_sc;

analogWrite(enA, outdefuzzy);


}

//1-2
if (ei<=neb && de>ndeb && de<=ndes)
{
uneb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_sc))/(a+b));

analogWrite(enA, outdefuzzy);



}

 //1-3
if (ei<=neb &&  de>ndes && de<=azdel)
{
uneb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uneb<undes)  //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)  //ka_sc
{b=uneb;}
else
{b=uazdel;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_sc))/(a+b));

analogWrite(enA, outdefuzzy);

}

//1-4
if (ei<=neb && de>azdel && de<=azdeh )
{
uneb=1;
uazde=1;

outdefuzzy=ka_sc;


analogWrite(enA, outdefuzzy);

}



//1-5
if (ei<=neb && de>azdeh && de<=pdes)
{
uneb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uneb<uazdeh)  //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)  //ka_sc
{b=uneb;}
else
{b=updes;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_sc))/(a+b));

analogWrite(enA, outdefuzzy);

}


 //1-6
if (ei<=neb &&  de>pdes && de<=pdeb)
{
uneb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ks_sc
{b=uneb;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_sc))/(a+b));

analogWrite(enA, outdefuzzy);

}


//1-7
if (ei<=neb && de>pdeb )
{
uneb=1;
updeb=1;

outdefuzzy=ka_sc;

analogWrite(enA, outdefuzzy);

}

//2-1
if (ei>neb && ei<=nes && de<=ndeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=1;
//operator and
if (uneb<undeb)    //ka_sc
{a=uneb;}
else
{a=undeb;}

if (unes<undeb)    //ka_c
{b=unes;}
else
{b=undeb;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_c))/(a+b));

analogWrite(enA, outdefuzzy);

}


//2-2
if (ei>neb && ei<=nes && de>ndeb && de<=ndes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

if (unes<undeb)  //ka_c
{c=unes;}
else
{c=undeb;}

if (unes<undes)  //ka_c
{d=unes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy
outdefuzzy=(((e*ka_sc)+(f*ka_c))/(e+f));

analogWrite(enA, outdefuzzy);

}




//2-3
if (ei>neb && ei<=nes && de>ndes && de<=azdel)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));
//operator and
if (uneb<undes)    //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)    //ka_sC
{b=uneb;}
else
{b=uazdel;}

if (unes<undes)    //ka_c
{c=unes;}
else
{c=undes;}

if (unes<uazdel)    //ka_C
{d=unes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy
outdefuzzy=(((e*ka_sc)+(f*ka_c))/(e+f));

analogWrite(enA, outdefuzzy);


}


//2-4
if (ei>neb && ei<=nes && de>azdel && de<=azdeh)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazde=1;

//operator and
if (uneb<uazde)    //ka_sc
{a=uneb;}
else
{a=uazde;}

if (unes<uazde)    //ka_c
{b=unes;}
else
{b=uazde;}


//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_c))/(a+b));

analogWrite(enA, outdefuzzy);


}

//2-5
if (ei>neb && ei<=nes && de>azdeh && de<=pdes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));
//operator and
if (uneb<uazdeh)    //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)    //ka_sc
{b=uneb;}
else
{b=updes;}

if (unes<uazdeh)    //ka_c
{c=unes;}
else
{c=uazdeh;}

if (unes<updes)    //ka_c
{d=unes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy
outdefuzzy=(((e*ka_sc)+(f*ka_c))/(e+f));

analogWrite(enA, outdefuzzy);


}

//2-6
if (ei>neb && ei<=nes &&  de>pdes && de<=pdeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ka_sc
{b=uneb;}
else
{b=updeb;}

if (unes<updes)  //ka_c
{c=unes;}
else
{c=updes;}

if (unes<updeb)  //ka_c
{d=unes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy
outdefuzzy=(((e*ka_sc)+(f*ka_c))/(e+f));

analogWrite(enA, outdefuzzy);


}

//2-7
if (ei>neb && ei<=nes && de>pdeb )
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updeb=1;

//operator and
if (uneb<updeb)    //ka_sc
{a=uneb;}
else
{a=updeb;}

if (unes<updeb)    //ka_c
{b=unes;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*ka_sc)+(b*ka_c)/(a+b)));

analogWrite(enA, outdefuzzy);


}

//3-1
if (ei>nes && ei<=azel && de<=ndeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=1;

//operator and
if (unes<undeb)  //k_sc
{a=unes;}
else
{a=undeb;}

if (uazel<undeb)  //stp
{b=uazel;}
else
{b=undeb;}

//output defuzzy
outdefuzzy=(((a*ka_c)+(b*stp))/(a+b));

analogWrite(enA, outdefuzzy);



}

//3-2
if (ei>nes && ei<=azel && de>ndeb && de<=ndes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (unes<undeb)  //ka_c
{a=unes;}
else
{a=undeb;}

if (unes<undes)  //ka_c
{b=unes;}
else
{b=undes;}

//operator and
if (uazel<undeb)  //stp
{c=uazel;}
else
{c=undeb;}

if (uazel<undes)  //stp
{d=uazel;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ka_c)+(f*stp))/(e+f));

analogWrite(enA, outdefuzzy);

}



 //3-3
if (ei>nes && ei<=azel&&  de>ndes && de<=azdel)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (unes<undes)  //ka_c
{a=unes;}
else
{a=undes;}

if (unes<uazdel)  //ka_c
{b=unes;}
else
{b=uazdel;}

//operator and
if (uazel<undes)  //stp
{c=uazel;}
else
{c=undes;}

if (uazel<uazdel)  //stp
{d=uazel;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ka_c)+(f*stp))/(e+f));

analogWrite(enA, outdefuzzy);


}

//3-4
if (ei>nes && ei<=azel && de>azdel && de<=azdeh )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazde=1;

//operator and
if (unes<uazde)  //ka_c
{a=unes;}
else
{a=uazde;}

if (uazel<uazde)  //stp
{b=uazel;}
else
{b=uazde;}

//output defuzzy
outdefuzzy=(((a*ka_c)+(b*stp)/(a+b)));

analogWrite(enA, outdefuzzy);


}

//3-5
if (ei>nes && ei<=azel && de>azdeh && de<=pdes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (unes<uazdeh)  //ka_c
{a=unes;}
else
{a=uazdeh;}

if (unes<updes)  //ka_c
{b=unes;}
else
{b=updes;}
//operator and
if (uazel<uazdeh)  //stp
{c=uazel;}
else
{c=uazdeh;}

if (uazel<updes)  //stp
{d=uazel;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ka_c)+(f*stp))/(e+f));

analogWrite(enA, outdefuzzy);


}

 //3-6
if (ei>nes && ei<=azel &&  de>pdes && de<=pdeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (unes<updes)  //ka_c
{a=unes;}
else
{a=updes;}

if (unes<updeb)  //ka_c
{b=unes;}
else
{b=updeb;}

//operator and
if (uazel<updes)  //stp
{c=uazel;}
else
{c=updes;}

if (uazel<updeb)  //tp
{d=uazel;}
else
{d=updeb;}


//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ka_c)+(f*stp))/(e+f));

analogWrite(enA, outdefuzzy);


}

//3-7
if (ei>nes && ei<=azel && de>pdeb )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updeb=1;

//operator and
if (unes<updeb)  //ka_c
{a=unes;}
else
{a=updeb;}

if (uazel<updeb)  //stp
{b=uazel;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*ka_c)+(b*stp)/(a+b)));

analogWrite(enA, outdefuzzy);


}

//4-1
if (ei>azel && ei<=azeh && de<=ndeb)
{
uaze=1;
undeb=1;
outdefuzzy=stp;

analogWrite(enA, outdefuzzy);



}

//4-2
if (ei>azel && ei<=azeh && de>ndeb && de<=ndes)
{
uaze=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uaze<undeb)  //stp
{a=uaze;}
else
{a=undeb;}

if (uaze<undes)  //stp
{b=uaze;}
else
{b=undes;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*stp))/(a+b));

analogWrite(enA, outdefuzzy);



}

 //4-3
if (ei>azel && ei<=azeh &&  de>ndes && de<=azdel)
{
uaze=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uaze<undes)  //stp
{a=uaze;}
else
{a=undes;}

if (uaze<uazdel)  //stp
{b=uaze;}
else
{b=uazdel;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*stp))/(a+b));

analogWrite(enA, outdefuzzy);


}

//4-4
if (ei>azel && ei<=azeh && de>azdel && de<=azdeh )
{
uaze=1;
uazde=1;

outdefuzzy=stp;

analogWrite(enA, outdefuzzy);


}

//4-5
if (ei>azel && ei<=azeh && de>azdeh && de<=pdes)
{
uaze=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uaze<uazdeh)  //stp
{a=uaze;}
else
{a=uazdeh;}

if (uaze<updes)  //stp
{b=uaze;}
else
{b=updes;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*stp))/(a+b));

analogWrite(enA, outdefuzzy);


}

 //4-6
if (ei>azel && ei<=azeh &&  de>pdes && de<=pdeb)
{
uaze=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uaze<updes)  //stp
{a=uaze;}
else
{a=updes;}

if (uaze<updeb)  //stp
{b=uaze;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*stp))/(a+b));

analogWrite(enA, outdefuzzy);


}

//4-7
if (ei>azel && ei<=azeh && de>pdeb )
{
uaze=1;
updeb=1;


outdefuzzy=stp;

analogWrite(enA, outdefuzzy);

}

//5-1
if (ei>azeh && ei<=pes && de<=ndeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=1;

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (upes<undeb)  //ki_c
{b=upes;}
else
{b=undeb;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*ki_c)/(a+b)));

analogWrite(enA, outdefuzzy);


}



//5-2
if (ei>azeh && ei<=pes && de>ndeb && de<=ndes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (uazeh<undes)  //stp
{b=uazeh;}
else
{b=undes;}

//operator and
if (upes<undeb)  //ki_c
{c=upes;}
else
{c=undeb;}

if (upes<undes)  //ki_c
{d=upes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*stp)+(f*ki_c))/(e+f));

analogWrite(enA, outdefuzzy);


}

 //5-3
if (ei>azeh && ei<=pes &&  de>ndes && de<=azdel)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uazeh<undes)  //stp
{a=uazeh;}
else
{a=undes;}

if (uazeh<uazdel)  //stp
{b=uazeh;}
else
{b=uazdel;}

//operator and
if (upes<undes)  //ki_c
{c=upes;}
else
{c=undes;}

if (upes<uazdel)  //ki_c
{d=upes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*stp)+(f*ki_c))/(e+f));

analogWrite(enA, outdefuzzy);


}


//5-4
if (ei>azeh && ei<=pes && de>azdel && de<=azdeh )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazde=1;

//operator and
if (uazeh<uazde)  //stp
{a=uazeh;}
else
{a=uazde;}

if (upes<uazde)  //ki_c
{b=upes;}
else
{b=uazde;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*ki_c)/(a+b)));

analogWrite(enA, outdefuzzy);


}

//5-5
if (ei>azeh && ei<=pes && de>azdeh && de<=pdes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uazeh<uazdeh)  //stp
{a=uazeh;}
else
{a=uazdeh;}

if (uazeh<updes)  //stp
{b=uazeh;}
else
{b=updes;}

//operator and
if (upes<uazdeh)  //ki_c
{c=upes;}
else
{c=uazdeh;}

if (upes<updes)  //ki_c
{d=upes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*stp)+(f*ki_c))/(e+f));

analogWrite(enA, outdefuzzy);


}

 //5-6
if (ei>azeh && ei<=pes &&  de>pdes && de<=pdeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uazeh<updes)  //stp
{a=uazeh;}
else
{a=updes;}

if (uazeh<updeb)  //stp
{b=uazeh;}
else
{b=updeb;}

//operator and
if (upes<updes)  //ki_c
{c=uazeh;}
else
{c=updes;}

if (upes<updeb)  //ki_c
{d=upes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*stp)+(f*ki_c))/(e+f));

analogWrite(enA, outdefuzzy);


}

//5-7
if (ei>azeh && ei<=pes && de>pdeb )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updeb=1;

//operator and
if (uazeh<updeb)  //stp
{a=uazeh;}
else
{a=updeb;}

if (upes<updeb)  //ki_c
{b=upes;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*stp)+(b*ki_c)/(a+b)));

analogWrite(enA, outdefuzzy);

}

//6-1
if (ei>pes && ei<=peb && de<=ndeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=1;

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upeb<undeb)  //ki-sc
{b=upeb;}
else
{b=undeb;}

//output defuzzy
outdefuzzy=(((a*ki_c)+(b*ki_sc)/(a+b)));

analogWrite(enA, outdefuzzy);

}



//6-2
if (ei>pes && ei<=peb && de>ndeb && de<=ndes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upes<undes)  //ki_c
{b=upes;}
else
{b=undes;}

//operator and
if (upeb<undeb)  //ki_sc
{c=upeb;}
else
{c=undeb;}

if (upeb<undes)  //ki_sc
{d=upeb;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ki_c)+(f*ki_sc))/(e+f));

analogWrite(enA, outdefuzzy);

}

 //6-3
if (ei>pes && ei<=peb &&  de>ndes && de<=azdel)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upes<undes)  //ki_c
{a=upes;}
else
{a=undes;}

if (upes<uazdel)  //ki_c
{b=upes;}
else
{b=uazdel;}

//operator and
if (upeb<undes)  //ki_sc
{c=upeb;}
else
{c=undes;}

if (upeb<uazdel)  //ki_sc
{d=upeb;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ki_c)+(f*ki_sc))/(e+f));

analogWrite(enA, outdefuzzy);

}

//6-4
if (ei>pes && ei<=peb && de>azdel && de<=azdeh )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazde=1;

//operator and
if (upes<uazde)  //ki_c
{a=upes;}
else
{a=uazde;}

if (upeb<uazde)  //ki_sc
{b=upeb;}
else
{b=uazde;}

//output defuzzy
outdefuzzy=(((a*ki_c)+(b*ki_sc)/(a+b)));

analogWrite(enA, outdefuzzy);

}

//6-5
if (ei>pes && ei<=peb && de>azdeh && de<=pdes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upes<uazdeh)  //ki_c
{a=upes;}
else
{a=uazdeh;}

if (upes<updes)  //ki_c
{b=upes;}
else
{b=updes;}

//operator and
if (upeb<uazdeh)  //ki_sc
{c=upeb;}
else
{c=uazdeh;}

if (upeb<updes)  //ki_sc
{d=upeb;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ki_c)+(f*ki_sc))/(e+f));

analogWrite(enA, outdefuzzy);


}



 //6-6
if (ei>pes && ei<=peb &&  de>pdes && de<=pdeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upes<updes)  //ki_c
{a=upes;}
else
{a=updes;}

if (upes<updeb)  //ki_sc
{b=upes;}
else
{b=updeb;}

//operator and
if (upeb<updes)  //ki_sc
{c=upeb;}
else
{c=updes;}

if (upeb<updeb)  //ki_sc
{d=upeb;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy=(((e*ki_c)+(f*ki_sc))/(e+f));

analogWrite(enA, outdefuzzy);

}

//6-7
if (ei>pes && ei<=peb && de>pdeb )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updeb=1;

//operator and
if (upes<updeb)  //ki_c
{a=upes;}
else
{a=updeb;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*ki_c)+(b*ki_sc)/(a+b)));

analogWrite(enA, outdefuzzy);

}

//7-1
if (ei>peb && de<=ndeb)
{

upeb=1;
undeb=1;

outdefuzzy=ki_sc;

analogWrite(enA, outdefuzzy);




//7-2
if (ei>peb && de>ndeb && de<=ndes)
{
upeb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upeb<undeb)  //ki_sc
{a=upeb;}
else
{a=undeb;}

if (upeb<undes)  //ki_sc
{b=upeb;}
else
{b=undes;}



//output defuzzy
outdefuzzy=(((a*ki_sc)+(b*ki_sc)/(a+b)));

analogWrite(enA, outdefuzzy);

}

 //7-3
if (ei>peb &&  de>ndes && de<=azdel)
{
upeb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upeb<undes)  //ki_sc
{a=upeb;}
else
{a=undes;}

if (upeb<uazdel)  //ki_sc
{b=upeb;}
else
{b=uazdel;}


//output defuzzy
outdefuzzy=(((a*ki_sc)+(b*ki_sc)/(a+b)));

analogWrite(enA, outdefuzzy);

}

//7-4
if (ei>peb && de>azdel && de<=azdeh )
{
upeb=1;
uazde=1;


//output defuzzy
outdefuzzy=ki_sc;

analogWrite(enA, outdefuzzy);

}

//7-5
if (ei>peb && de>azdeh && de<=pdes)
{
upeb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upeb<uazdeh)  //ki_sc
{a=upeb;}
else
{a=uazdeh;}

if (upeb<updes)  //ki_sc
{b=upeb;}
else
{b=updes;}



//output defuzzy
outdefuzzy=(((a*ki_sc)+(b*ki_sc))/(a+b));

analogWrite(enA, outdefuzzy);

}

 //7-6
if (ei>peb &&  de>pdes && de<=pdeb)
{
upeb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upeb<updes)  //ki_sc
{a=upeb;}
else
{a=updes;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy
outdefuzzy=(((a*ki_sc)+(b*ki_sc))/(a+b));

analogWrite(enA, outdefuzzy);

}


//7-7
if (ei>peb && de>pdeb )
{
upeb=1;
updeb=1;


//output defuzzy
outdefuzzy=ki_sc;

analogWrite(enA, outdefuzzy);

}
}

 if(outdefuzzy>0) {
digitalWrite(set1, LOW);
digitalWrite(set2, HIGH); //Gerak ke Bawah
}            //Gerak ke Bawah

if(outdefuzzy<0) {
digitalWrite(set1, HIGH); //Gerak ke Atas
digitalWrite(set2, LOW);
}

if(outdefuzzy=0) {
digitalWrite(set1, LOW);
digitalWrite(set2, LOW); 
}
if(kalAngleY2<0){
  digitalWrite(set1, LOW);
digitalWrite(set2, LOW);
}
}

void Fuzzy2() {
  
  
  
  
   //Read the raw sensor value
  int sensor_value = analogRead(ROTARY_ANGLE_SENSOR2);
  
  //Convert the sensor reading to degrees and return that value
  float voltage = (float)sensor_value * ADC_REF / 1023; 
  float degrees = (voltage * FULL_ANGLE) / ADC_REF; 
  pv2=(float)degrees;
  
  
  
//-----------KONVERT NEGATIF KE POSITIF---------------------------
  kalmanYY=kalAngleY+80;
  kalmanYY_f=(float)kalmanYY;  
  
 //-------------------INTERPOLASI-------------------------------------  
  sp2=((((y2-y1)/(x2-x1))*(kalmanYY_f-x1))+y1);
  ei=sp2-pv2;
  de=ei-eii2;
  eii2=ei;
  
//--------------------ALGORITMA FUZZY LOGIC (WRIST)------------------------
   //1-1
if (ei<=neb && de<=ndeb)
{
uneb=1;
undeb=1;

outdefuzzy2=ka_sc;
analogWrite(enB, outdefuzzy2);

}

//1-2
if (ei<=neb && de>ndeb && de<=ndes)
{
uneb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enB, outdefuzzy2);


}

 //1-3
if (ei<=neb &&  de>ndes && de<=azdel)
{
uneb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uneb<undes)  //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)  //ka_sc
{b=uneb;}
else
{b=uazdel;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enB, outdefuzzy2);
}

//1-4
if (ei<=neb && de>azdel && de<=azdeh )
{
uneb=1;
uazde=1;


outdefuzzy2=ka_sc;

analogWrite(enB, outdefuzzy2);
}



//1-5
if (ei<=neb && de>azdeh && de<=pdes)
{
uneb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uneb<uazdeh)  //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)  //ka_sc
{b=uneb;}
else
{b=updes;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enB, outdefuzzy2);
}


 //1-6
if (ei<=neb &&  de>pdes && de<=pdeb)
{
uneb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ks_sc
{b=uneb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enB, outdefuzzy2);
}


//1-7
if (ei<=neb && de>pdeb )
{
uneb=1;
updeb=1;


outdefuzzy2=ka_sc;
analogWrite(enB, outdefuzzy2);
}

//2-1
if (ei>neb && ei<=nes && de<=ndeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=1;
//operator and
if (uneb<undeb)    //ka_sc
{a=uneb;}
else
{a=undeb;}

if (unes<undeb)    //ka_c
{b=unes;}
else
{b=undeb;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_c))/(a+b));
analogWrite(enB, outdefuzzy2);
}


//2-2
if (ei>neb && ei<=nes && de>ndeb && de<=ndes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

if (unes<undeb)  //ka_c
{c=unes;}
else
{c=undeb;}

if (unes<undes)  //ka_c
{d=unes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy

outdefuzzy2=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enB, outdefuzzy2);
}




//2-3
if (ei>neb && ei<=nes && de>ndes && de<=azdel)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));
//operator and
if (uneb<undes)    //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)    //ka_sC
{b=uneb;}
else
{b=uazdel;}

if (unes<undes)    //ka_c
{c=unes;}
else
{c=undes;}

if (unes<uazdel)    //ka_C
{d=unes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy

outdefuzzy2=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}


//2-4
if (ei>neb && ei<=nes && de>azdel && de<=azdeh)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazde=1;

//operator and
if (uneb<uazde)    //ka_sc
{a=uneb;}
else
{a=uazde;}

if (unes<uazde)    //ka_c
{b=unes;}
else
{b=uazde;}


//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_c))/(a+b));
analogWrite(enB, outdefuzzy2);

}

//2-5
if (ei>neb && ei<=nes && de>azdeh && de<=pdes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));
//operator and
if (uneb<uazdeh)    //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)    //ka_sc
{b=uneb;}
else
{b=updes;}

if (unes<uazdeh)    //ka_c
{c=unes;}
else
{c=uazdeh;}

if (unes<updes)    //ka_c
{d=unes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy

outdefuzzy2=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}

//2-6
if (ei>neb && ei<=nes &&  de>pdes && de<=pdeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ka_sc
{b=uneb;}
else
{b=updeb;}

if (unes<updes)  //ka_c
{c=unes;}
else
{c=updes;}

if (unes<updeb)  //ka_c
{d=unes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy

outdefuzzy2=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}

//2-7
if (ei>neb && ei<=nes && de>pdeb )
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updeb=1;

//operator and
if (uneb<updeb)    //ka_sc
{a=uneb;}
else
{a=updeb;}

if (unes<updeb)    //ka_c
{b=unes;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*ka_sc)+(b*ka_c)/(a+b)));
analogWrite(enB, outdefuzzy2);

}

//3-1
if (ei>nes && ei<=azel && de<=ndeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=1;

//operator and
if (unes<undeb)  //k_sc
{a=unes;}
else
{a=undeb;}

if (uazel<undeb)  //stp
{b=uazel;}
else
{b=undeb;}

//output defuzzy

outdefuzzy2=(((a*ka_c)+(b*stp))/(a+b));
analogWrite(enB, outdefuzzy2);


}

//3-2
if (ei>nes && ei<=azel && de>ndeb && de<=ndes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (unes<undeb)  //ka_c
{a=unes;}
else
{a=undeb;}

if (unes<undes)  //ka_c
{b=unes;}
else
{b=undes;}

//operator and
if (uazel<undeb)  //stp
{c=uazel;}
else
{c=undeb;}

if (uazel<undes)  //stp
{d=uazel;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enB, outdefuzzy2);
}



 //3-3
if (ei>nes && ei<=azel&&  de>ndes && de<=azdel)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (unes<undes)  //ka_c
{a=unes;}
else
{a=undes;}

if (unes<uazdel)  //ka_c
{b=unes;}
else
{b=uazdel;}

//operator and
if (uazel<undes)  //stp
{c=uazel;}
else
{c=undes;}

if (uazel<uazdel)  //stp
{d=uazel;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enB, outdefuzzy2);

}

//3-4
if (ei>nes && ei<=azel && de>azdel && de<=azdeh )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazde=1;

//operator and
if (unes<uazde)  //ka_c
{a=unes;}
else
{a=uazde;}

if (uazel<uazde)  //stp
{b=uazel;}
else
{b=uazde;}

//output defuzzy

outdefuzzy2=(((a*ka_c)+(b*stp)/(a+b)));
analogWrite(enB, outdefuzzy2);

}

//3-5
if (ei>nes && ei<=azel && de>azdeh && de<=pdes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (unes<uazdeh)  //ka_c
{a=unes;}
else
{a=uazdeh;}

if (unes<updes)  //ka_c
{b=unes;}
else
{b=updes;}
//operator and
if (uazel<uazdeh)  //stp
{c=uazel;}
else
{c=uazdeh;}

if (uazel<updes)  //stp
{d=uazel;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enB, outdefuzzy2);

}

 //3-6
if (ei>nes && ei<=azel &&  de>pdes && de<=pdeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (unes<updes)  //ka_c
{a=unes;}
else
{a=updes;}

if (unes<updeb)  //ka_c
{b=unes;}
else
{b=updeb;}

//operator and
if (uazel<updes)  //stp
{c=uazel;}
else
{c=updes;}

if (uazel<updeb)  //tp
{d=uazel;}
else
{d=updeb;}


//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enB, outdefuzzy2);

}

//3-7
if (ei>nes && ei<=azel && de>pdeb )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updeb=1;

//operator and
if (unes<updeb)  //ka_c
{a=unes;}
else
{a=updeb;}

if (uazel<updeb)  //stp
{b=uazel;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*ka_c)+(b*stp)/(a+b)));
analogWrite(enB, outdefuzzy2);

}

//4-1
if (ei>azel && ei<=azeh && de<=ndeb)
{
uaze=1;
undeb=1;

outdefuzzy2=stp;
analogWrite(enB, outdefuzzy2);


}

//4-2
if (ei>azel && ei<=azeh && de>ndeb && de<=ndes)
{
uaze=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uaze<undeb)  //stp
{a=uaze;}
else
{a=undeb;}

if (uaze<undes)  //stp
{b=uaze;}
else
{b=undes;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*stp))/(a+b));
analogWrite(enB, outdefuzzy2);


}

 //4-3
if (ei>azel && ei<=azeh &&  de>ndes && de<=azdel)
{
uaze=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uaze<undes)  //stp
{a=uaze;}
else
{a=undes;}

if (uaze<uazdel)  //stp
{b=uaze;}
else
{b=uazdel;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*stp))/(a+b));
analogWrite(enB, outdefuzzy2);

}

//4-4
if (ei>azel && ei<=azeh && de>azdel && de<=azdeh )
{
uaze=1;
uazde=1;


outdefuzzy2=stp;
analogWrite(enB, outdefuzzy2);

}

//4-5
if (ei>azel && ei<=azeh && de>azdeh && de<=pdes)
{
uaze=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uaze<uazdeh)  //stp
{a=uaze;}
else
{a=uazdeh;}

if (uaze<updes)  //stp
{b=uaze;}
else
{b=updes;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*stp))/(a+b));
analogWrite(enB, outdefuzzy2);

}

 //4-6
if (ei>azel && ei<=azeh &&  de>pdes && de<=pdeb)
{
uaze=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uaze<updes)  //stp
{a=uaze;}
else
{a=updes;}

if (uaze<updeb)  //stp
{b=uaze;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*stp))/(a+b));
analogWrite(enB, outdefuzzy2);

}

//4-7
if (ei>azel && ei<=azeh && de>pdeb )
{
uaze=1;
updeb=1;



outdefuzzy2=stp;
analogWrite(enB, outdefuzzy2);
}

//5-1
if (ei>azeh && ei<=pes && de<=ndeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=1;

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (upes<undeb)  //ki_c
{b=upes;}
else
{b=undeb;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enB, outdefuzzy2);

}



//5-2
if (ei>azeh && ei<=pes && de>ndeb && de<=ndes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (uazeh<undes)  //stp
{b=uazeh;}
else
{b=undes;}

//operator and
if (upes<undeb)  //ki_c
{c=upes;}
else
{c=undeb;}

if (upes<undes)  //ki_c
{d=upes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}

 //5-3
if (ei>azeh && ei<=pes &&  de>ndes && de<=azdel)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uazeh<undes)  //stp
{a=uazeh;}
else
{a=undes;}

if (uazeh<uazdel)  //stp
{b=uazeh;}
else
{b=uazdel;}

//operator and
if (upes<undes)  //ki_c
{c=upes;}
else
{c=undes;}

if (upes<uazdel)  //ki_c
{d=upes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}


//5-4
if (ei>azeh && ei<=pes && de>azdel && de<=azdeh )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazde=1;

//operator and
if (uazeh<uazde)  //stp
{a=uazeh;}
else
{a=uazde;}

if (upes<uazde)  //ki_c
{b=upes;}
else
{b=uazde;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enB, outdefuzzy2);

}

//5-5
if (ei>azeh && ei<=pes && de>azdeh && de<=pdes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uazeh<uazdeh)  //stp
{a=uazeh;}
else
{a=uazdeh;}

if (uazeh<updes)  //stp
{b=uazeh;}
else
{b=updes;}

//operator and
if (upes<uazdeh)  //ki_c
{c=upes;}
else
{c=uazdeh;}

if (upes<updes)  //ki_c
{d=upes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy


outdefuzzy2=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}

 //5-6
if (ei>azeh && ei<=pes &&  de>pdes && de<=pdeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uazeh<updes)  //stp
{a=uazeh;}
else
{a=updes;}

if (uazeh<updeb)  //stp
{b=uazeh;}
else
{b=updeb;}

//operator and
if (upes<updes)  //ki_c
{c=uazeh;}
else
{c=updes;}

if (upes<updeb)  //ki_c
{d=upes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enB, outdefuzzy2);

}

//5-7
if (ei>azeh && ei<=pes && de>pdeb )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updeb=1;

//operator and
if (uazeh<updeb)  //stp
{a=uazeh;}
else
{a=updeb;}

if (upes<updeb)  //ki_c
{b=upes;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enB, outdefuzzy2);
}

//6-1
if (ei>pes && ei<=peb && de<=ndeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=1;

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upeb<undeb)  //ki-sc
{b=upeb;}
else
{b=undeb;}

//output defuzzy

outdefuzzy2=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enB, outdefuzzy2);
}



//6-2
if (ei>pes && ei<=peb && de>ndeb && de<=ndes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upes<undes)  //ki_c
{b=upes;}
else
{b=undes;}

//operator and
if (upeb<undeb)  //ki_sc
{c=upeb;}
else
{c=undeb;}

if (upeb<undes)  //ki_sc
{d=upeb;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy2=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enB, outdefuzzy2);
}

 //6-3
if (ei>pes && ei<=peb &&  de>ndes && de<=azdel)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upes<undes)  //ki_c
{a=upes;}
else
{a=undes;}

if (upes<uazdel)  //ki_c
{b=upes;}
else
{b=uazdel;}

//operator and
if (upeb<undes)  //ki_sc
{c=upeb;}
else
{c=undes;}

if (upeb<uazdel)  //ki_sc
{d=upeb;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enB, outdefuzzy2);
}

//6-4
if (ei>pes && ei<=peb && de>azdel && de<=azdeh )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazde=1;

//operator and
if (upes<uazde)  //ki_c
{a=upes;}
else
{a=uazde;}

if (upeb<uazde)  //ki_sc
{b=upeb;}
else
{b=uazde;}

//output defuzzy

outdefuzzy2=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enB, outdefuzzy2);
}

//6-5
if (ei>pes && ei<=peb && de>azdeh && de<=pdes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upes<uazdeh)  //ki_c
{a=upes;}
else
{a=uazdeh;}

if (upes<updes)  //ki_c
{b=upes;}
else
{b=updes;}

//operator and
if (upeb<uazdeh)  //ki_sc
{c=upeb;}
else
{c=uazdeh;}

if (upeb<updes)  //ki_sc
{d=upeb;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enB, outdefuzzy2);

}



 //6-6
if (ei>pes && ei<=peb &&  de>pdes && de<=pdeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upes<updes)  //ki_c
{a=upes;}
else
{a=updes;}

if (upes<updeb)  //ki_sc
{b=upes;}
else
{b=updeb;}

//operator and
if (upeb<updes)  //ki_sc
{c=upeb;}
else
{c=updes;}

if (upeb<updeb)  //ki_sc
{d=upeb;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy2=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enB, outdefuzzy2);
}

//6-7
if (ei>pes && ei<=peb && de>pdeb )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updeb=1;

//operator and
if (upes<updeb)  //ki_c
{a=upes;}
else
{a=updeb;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enB, outdefuzzy2);
}

//7-1
if (ei>peb && de<=ndeb)
{

upeb=1;
undeb=1;


outdefuzzy2=ki_sc;
analogWrite(enB, outdefuzzy2);



//7-2
if (ei>peb && de>ndeb && de<=ndes)
{
upeb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upeb<undeb)  //ki_sc
{a=upeb;}
else
{a=undeb;}

if (upeb<undes)  //ki_sc
{b=upeb;}
else
{b=undes;}



//output defuzzy

outdefuzzy2=(((a*ki_sc)+(b*ki_sc)/(a+b)));
analogWrite(enB, outdefuzzy2);
}

 //7-3
if (ei>peb &&  de>ndes && de<=azdel)
{
upeb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upeb<undes)  //ki_sc
{a=upeb;}
else
{a=undes;}

if (upeb<uazdel)  //ki_sc
{b=upeb;}
else
{b=uazdel;}


//output defuzzy

outdefuzzy2=(((a*ki_sc)+(b*ki_sc)/(a+b)));
analogWrite(enB, outdefuzzy2);
}

//7-4
if (ei>peb && de>azdel && de<=azdeh )
{
upeb=1;
uazde=1;


//output defuzzy

outdefuzzy2=ki_sc;
analogWrite(enB, outdefuzzy2);
}

//7-5
if (ei>peb && de>azdeh && de<=pdes)
{
upeb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upeb<uazdeh)  //ki_sc
{a=upeb;}
else
{a=uazdeh;}

if (upeb<updes)  //ki_sc
{b=upeb;}
else
{b=updes;}



//output defuzzy

outdefuzzy2=(((a*ki_sc)+(b*ki_sc))/(a+b));
analogWrite(enB, outdefuzzy2);
}

 //7-6
if (ei>peb &&  de>pdes && de<=pdeb)
{
upeb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upeb<updes)  //ki_sc
{a=upeb;}
else
{a=updes;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy2=(((a*ki_sc)+(b*ki_sc))/(a+b));
analogWrite(enB, outdefuzzy2);
}

//7-7
if (ei>peb && de>pdeb )
{
upeb=1;
updeb=1;


//output defuzzy

outdefuzzy2=ki_sc;
analogWrite(enB, outdefuzzy2);

}



}




 

 
 if(outdefuzzy2>0) {
digitalWrite(set3, LOW);
digitalWrite(set4, HIGH); //Gerak ke Bawah
}            //Gerak ke Bawah

if(outdefuzzy2<0) {
digitalWrite(set3, HIGH); //Gerak ke Atas
digitalWrite(set4, LOW);
}

if(outdefuzzy2=0) {
digitalWrite(set3, LOW);
digitalWrite(set4, LOW); 
}
if(kalAngleY>0){
  digitalWrite(set3, HIGH);
digitalWrite(set4, HIGH);

}
}


void Fuzzy3() {
 
  
  //Read the raw sensor value
  int sensor_value = analogRead(ROTARY_ANGLE_SENSOR3);
  
  //Convert the sensor reading to degrees and return that value
  float voltage = (float)sensor_value * ADC_REF / 1023; 
  float degrees = (voltage * FULL_ANGLE) / ADC_REF; 
  pv3=(float)degrees;
  
    //-----------KONVERT NEGATIF KE POSITIF---------------------------
  kalmanYY2=kalAngleZ+180;
  kalmanYY_f2=(float)kalmanYY2;  
  
  
 //-------------------ERROR dan DELTA ERROR-------------------------------------  
  sp3=((((y4-y3)/(x4-x3))*(kalmanYY_f2-x3))+y3);
  ei=sp3-pv3;
  de=ei-eii3;
  eii3=ei;
  


 
   //1-1
if (ei<=neb && de<=ndeb)
{
uneb=1;
undeb=1;

outdefuzzy3=ka_sc;
analogWrite(enC, outdefuzzy3);

}

//1-2
if (ei<=neb && de>ndeb && de<=ndes)
{
uneb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enC, outdefuzzy3);


}

 //1-3
if (ei<=neb &&  de>ndes && de<=azdel)
{
uneb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uneb<undes)  //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)  //ka_sc
{b=uneb;}
else
{b=uazdel;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enC, outdefuzzy3);
}

//1-4
if (ei<=neb && de>azdel && de<=azdeh )
{
uneb=1;
uazde=1;


outdefuzzy3=ka_sc;

analogWrite(enC, outdefuzzy3);
}



//1-5
if (ei<=neb && de>azdeh && de<=pdes)
{
uneb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uneb<uazdeh)  //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)  //ka_sc
{b=uneb;}
else
{b=updes;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enC, outdefuzzy3);
}


 //1-6
if (ei<=neb &&  de>pdes && de<=pdeb)
{
uneb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ks_sc
{b=uneb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_sc))/(a+b));
analogWrite(enC, outdefuzzy3);
}


//1-7
if (ei<=neb && de>pdeb )
{
uneb=1;
updeb=1;


outdefuzzy3=ka_sc;
analogWrite(enC, outdefuzzy3);
}

//2-1
if (ei>neb && ei<=nes && de<=ndeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=1;
//operator and
if (uneb<undeb)    //ka_sc
{a=uneb;}
else
{a=undeb;}

if (unes<undeb)    //ka_c
{b=unes;}
else
{b=undeb;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_c))/(a+b));
analogWrite(enC, outdefuzzy3);
}


//2-2
if (ei>neb && ei<=nes && de>ndeb && de<=ndes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uneb<undeb)  //ka_sc
{a=uneb;}
else
{a=undeb;}

if (uneb<undes)  //ka_sc
{b=uneb;}
else
{b=undes;}

if (unes<undeb)  //ka_c
{c=unes;}
else
{c=undeb;}

if (unes<undes)  //ka_c
{d=unes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy

outdefuzzy3=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enC, outdefuzzy3);
}




//2-3
if (ei>neb && ei<=nes && de>ndes && de<=azdel)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));
//operator and
if (uneb<undes)    //ka_sc
{a=uneb;}
else
{a=undes;}

if (uneb<uazdel)    //ka_sC
{b=uneb;}
else
{b=uazdel;}

if (unes<undes)    //ka_c
{c=unes;}
else
{c=undes;}

if (unes<uazdel)    //ka_C
{d=unes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=c;}
else
{f=d;}

//output defuzzy

outdefuzzy3=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}


//2-4
if (ei>neb && ei<=nes && de>azdel && de<=azdeh)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazde=1;

//operator and
if (uneb<uazde)    //ka_sc
{a=uneb;}
else
{a=uazde;}

if (unes<uazde)    //ka_c
{b=unes;}
else
{b=uazde;}


//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_c))/(a+b));
analogWrite(enC, outdefuzzy3);

}

//2-5
if (ei>neb && ei<=nes && de>azdeh && de<=pdes)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));
//operator and
if (uneb<uazdeh)    //ka_sc
{a=uneb;}
else
{a=uazdeh;}

if (uneb<updes)    //ka_sc
{b=uneb;}
else
{b=updes;}

if (unes<uazdeh)    //ka_c
{c=unes;}
else
{c=uazdeh;}

if (unes<updes)    //ka_c
{d=unes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy

outdefuzzy3=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}

//2-6
if (ei>neb && ei<=nes &&  de>pdes && de<=pdeb)
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uneb<updes)  //ka_sc
{a=uneb;}
else
{a=updes;}

if (uneb<updeb)  //ka_sc
{b=uneb;}
else
{b=updeb;}

if (unes<updes)  //ka_c
{c=unes;}
else
{c=updes;}

if (unes<updeb)  //ka_c
{d=unes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=d;}

//output defuzzy

outdefuzzy3=(((e*ka_sc)+(f*ka_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}

//2-7
if (ei>neb && ei<=nes && de>pdeb )
{
uneb=((nes-ei)/(nes-neb));
unes=((ei-neb)/(nes-neb));
updeb=1;

//operator and
if (uneb<updeb)    //ka_sc
{a=uneb;}
else
{a=updeb;}

if (unes<updeb)    //ka_c
{b=unes;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*ka_sc)+(b*ka_c)/(a+b)));
analogWrite(enC, outdefuzzy3);

}

//3-1
if (ei>nes && ei<=azel && de<=ndeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=1;

//operator and
if (unes<undeb)  //k_sc
{a=unes;}
else
{a=undeb;}

if (uazel<undeb)  //stp
{b=uazel;}
else
{b=undeb;}

//output defuzzy

outdefuzzy3=(((a*ka_c)+(b*stp))/(a+b));
analogWrite(enC, outdefuzzy3);


}

//3-2
if (ei>nes && ei<=azel && de>ndeb && de<=ndes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (unes<undeb)  //ka_c
{a=unes;}
else
{a=undeb;}

if (unes<undes)  //ka_c
{b=unes;}
else
{b=undes;}

//operator and
if (uazel<undeb)  //stp
{c=uazel;}
else
{c=undeb;}

if (uazel<undes)  //stp
{d=uazel;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enC, outdefuzzy3);
}



 //3-3
if (ei>nes && ei<=azel&&  de>ndes && de<=azdel)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (unes<undes)  //ka_c
{a=unes;}
else
{a=undes;}

if (unes<uazdel)  //ka_c
{b=unes;}
else
{b=uazdel;}

//operator and
if (uazel<undes)  //stp
{c=uazel;}
else
{c=undes;}

if (uazel<uazdel)  //stp
{d=uazel;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enC, outdefuzzy3);

}

//3-4
if (ei>nes && ei<=azel && de>azdel && de<=azdeh )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazde=1;

//operator and
if (unes<uazde)  //ka_c
{a=unes;}
else
{a=uazde;}

if (uazel<uazde)  //stp
{b=uazel;}
else
{b=uazde;}

//output defuzzy

outdefuzzy3=(((a*ka_c)+(b*stp)/(a+b)));
analogWrite(enC, outdefuzzy3);

}

//3-5
if (ei>nes && ei<=azel && de>azdeh && de<=pdes)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (unes<uazdeh)  //ka_c
{a=unes;}
else
{a=uazdeh;}

if (unes<updes)  //ka_c
{b=unes;}
else
{b=updes;}
//operator and
if (uazel<uazdeh)  //stp
{c=uazel;}
else
{c=uazdeh;}

if (uazel<updes)  //stp
{d=uazel;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enC, outdefuzzy3);

}

 //3-6
if (ei>nes && ei<=azel &&  de>pdes && de<=pdeb)
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (unes<updes)  //ka_c
{a=unes;}
else
{a=updes;}

if (unes<updeb)  //ka_c
{b=unes;}
else
{b=updeb;}

//operator and
if (uazel<updes)  //stp
{c=uazel;}
else
{c=updes;}

if (uazel<updeb)  //tp
{d=uazel;}
else
{d=updeb;}


//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ka_c)+(f*stp))/(e+f));
analogWrite(enC, outdefuzzy3);

}

//3-7
if (ei>nes && ei<=azel && de>pdeb )
{
unes=((azel-ei)/(azel-nes));
uazel=((ei-nes)/(azel-nes));
updeb=1;

//operator and
if (unes<updeb)  //ka_c
{a=unes;}
else
{a=updeb;}

if (uazel<updeb)  //stp
{b=uazel;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*ka_c)+(b*stp)/(a+b)));
analogWrite(enC, outdefuzzy3);

}

//4-1
if (ei>azel && ei<=azeh && de<=ndeb)
{
uaze=1;
undeb=1;

outdefuzzy3=stp;
analogWrite(enC, outdefuzzy3);


}

//4-2
if (ei>azel && ei<=azeh && de>ndeb && de<=ndes)
{
uaze=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uaze<undeb)  //stp
{a=uaze;}
else
{a=undeb;}

if (uaze<undes)  //stp
{b=uaze;}
else
{b=undes;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*stp))/(a+b));
analogWrite(enC, outdefuzzy3);


}

 //4-3
if (ei>azel && ei<=azeh &&  de>ndes && de<=azdel)
{
uaze=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uaze<undes)  //stp
{a=uaze;}
else
{a=undes;}

if (uaze<uazdel)  //stp
{b=uaze;}
else
{b=uazdel;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*stp))/(a+b));
analogWrite(enC, outdefuzzy3);

}

//4-4
if (ei>azel && ei<=azeh && de>azdel && de<=azdeh )
{
uaze=1;
uazde=1;


outdefuzzy3=stp;
analogWrite(enC, outdefuzzy3);

}

//4-5
if (ei>azel && ei<=azeh && de>azdeh && de<=pdes)
{
uaze=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uaze<uazdeh)  //stp
{a=uaze;}
else
{a=uazdeh;}

if (uaze<updes)  //stp
{b=uaze;}
else
{b=updes;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*stp))/(a+b));
analogWrite(enC, outdefuzzy3);

}

 //4-6
if (ei>azel && ei<=azeh &&  de>pdes && de<=pdeb)
{
uaze=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uaze<updes)  //stp
{a=uaze;}
else
{a=updes;}

if (uaze<updeb)  //stp
{b=uaze;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*stp))/(a+b));
analogWrite(enC, outdefuzzy3);

}

//4-7
if (ei>azel && ei<=azeh && de>pdeb )
{
uaze=1;
updeb=1;



outdefuzzy3=stp;
analogWrite(enC, outdefuzzy3);
}

//5-1
if (ei>azeh && ei<=pes && de<=ndeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=1;

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (upes<undeb)  //ki_c
{b=upes;}
else
{b=undeb;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enC, outdefuzzy3);

}



//5-2
if (ei>azeh && ei<=pes && de>ndeb && de<=ndes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (uazeh<undeb)  //stp
{a=uazeh;}
else
{a=undeb;}

if (uazeh<undes)  //stp
{b=uazeh;}
else
{b=undes;}

//operator and
if (upes<undeb)  //ki_c
{c=upes;}
else
{c=undeb;}

if (upes<undes)  //ki_c
{d=upes;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}

 //5-3
if (ei>azeh && ei<=pes &&  de>ndes && de<=azdel)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (uazeh<undes)  //stp
{a=uazeh;}
else
{a=undes;}

if (uazeh<uazdel)  //stp
{b=uazeh;}
else
{b=uazdel;}

//operator and
if (upes<undes)  //ki_c
{c=upes;}
else
{c=undes;}

if (upes<uazdel)  //ki_c
{d=upes;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}


//5-4
if (ei>azeh && ei<=pes && de>azdel && de<=azdeh )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazde=1;

//operator and
if (uazeh<uazde)  //stp
{a=uazeh;}
else
{a=uazde;}

if (upes<uazde)  //ki_c
{b=upes;}
else
{b=uazde;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enC, outdefuzzy3);

}

//5-5
if (ei>azeh && ei<=pes && de>azdeh && de<=pdes)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (uazeh<uazdeh)  //stp
{a=uazeh;}
else
{a=uazdeh;}

if (uazeh<updes)  //stp
{b=uazeh;}
else
{b=updes;}

//operator and
if (upes<uazdeh)  //ki_c
{c=upes;}
else
{c=uazdeh;}

if (upes<updes)  //ki_c
{d=upes;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}

 //5-6
if (ei>azeh && ei<=pes &&  de>pdes && de<=pdeb)
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (uazeh<updes)  //stp
{a=uazeh;}
else
{a=updes;}

if (uazeh<updeb)  //stp
{b=uazeh;}
else
{b=updeb;}

//operator and
if (upes<updes)  //ki_c
{c=uazeh;}
else
{c=updes;}

if (upes<updeb)  //ki_c
{d=upes;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*stp)+(f*ki_c))/(e+f));
analogWrite(enC, outdefuzzy3);

}

//5-7
if (ei>azeh && ei<=pes && de>pdeb )
{
uazeh=((pes-ei)/(pes-azeh));
upes=((ei-azeh)/(pes-azeh));
updeb=1;

//operator and
if (uazeh<updeb)  //stp
{a=uazeh;}
else
{a=updeb;}

if (upes<updeb)  //ki_c
{b=upes;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*stp)+(b*ki_c)/(a+b)));
analogWrite(enC, outdefuzzy3);
}

//6-1
if (ei>pes && ei<=peb && de<=ndeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=1;

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upeb<undeb)  //ki-sc
{b=upeb;}
else
{b=undeb;}

//output defuzzy

outdefuzzy3=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enC, outdefuzzy3);
}



//6-2
if (ei>pes && ei<=peb && de>ndeb && de<=ndes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upes<undeb)  //ki_c
{a=upes;}
else
{a=undeb;}

if (upes<undes)  //ki_c
{b=upes;}
else
{b=undes;}

//operator and
if (upeb<undeb)  //ki_sc
{c=upeb;}
else
{c=undeb;}

if (upeb<undes)  //ki_sc
{d=upeb;}
else
{d=undes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy
outdefuzzy3=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enC, outdefuzzy3);
}

 //6-3
if (ei>pes && ei<=peb &&  de>ndes && de<=azdel)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upes<undes)  //ki_c
{a=upes;}
else
{a=undes;}

if (upes<uazdel)  //ki_c
{b=upes;}
else
{b=uazdel;}

//operator and
if (upeb<undes)  //ki_sc
{c=upeb;}
else
{c=undes;}

if (upeb<uazdel)  //ki_sc
{d=upeb;}
else
{d=uazdel;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enC, outdefuzzy3);
}

//6-4
if (ei>pes && ei<=peb && de>azdel && de<=azdeh )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazde=1;

//operator and
if (upes<uazde)  //ki_c
{a=upes;}
else
{a=uazde;}

if (upeb<uazde)  //ki_sc
{b=upeb;}
else
{b=uazde;}

//output defuzzy

outdefuzzy3=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enC, outdefuzzy3);
}

//6-5
if (ei>pes && ei<=peb && de>azdeh && de<=pdes)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upes<uazdeh)  //ki_c
{a=upes;}
else
{a=uazdeh;}

if (upes<updes)  //ki_c
{b=upes;}
else
{b=updes;}

//operator and
if (upeb<uazdeh)  //ki_sc
{c=upeb;}
else
{c=uazdeh;}

if (upeb<updes)  //ki_sc
{d=upeb;}
else
{d=updes;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enC, outdefuzzy3);

}



 //6-6
if (ei>pes && ei<=peb &&  de>pdes && de<=pdeb)
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upes<updes)  //ki_c
{a=upes;}
else
{a=updes;}

if (upes<updeb)  //ki_sc
{b=upes;}
else
{b=updeb;}

//operator and
if (upeb<updes)  //ki_sc
{c=upeb;}
else
{c=updes;}

if (upeb<updeb)  //ki_sc
{d=upeb;}
else
{d=updeb;}

//operator OR
if (a<b)
{e=b;}
else
{e=a;}

//operator OR
if (c<d)
{f=d;}
else
{f=c;}

//output defuzzy

outdefuzzy3=(((e*ki_c)+(f*ki_sc))/(e+f));
analogWrite(enC, outdefuzzy3);
}

//6-7
if (ei>pes && ei<=peb && de>pdeb )
{
upes=((peb-ei)/(peb-pes));
upeb=((ei-pes)/(peb-pes));
updeb=1;

//operator and
if (upes<updeb)  //ki_c
{a=upes;}
else
{a=updeb;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*ki_c)+(b*ki_sc)/(a+b)));
analogWrite(enC, outdefuzzy3);
}

//7-1
if (ei>peb && de<=ndeb)
{

upeb=1;
undeb=1;


outdefuzzy3=ki_sc;
analogWrite(enC, outdefuzzy3);



//7-2
if (ei>peb && de>ndeb && de<=ndes)
{
upeb=1;
undeb=((ndes-de)/(ndes-ndeb));
undes=((de-ndeb)/(ndes-ndeb));

//operator and
if (upeb<undeb)  //ki_sc
{a=upeb;}
else
{a=undeb;}

if (upeb<undes)  //ki_sc
{b=upeb;}
else
{b=undes;}



//output defuzzy

outdefuzzy3=(((a*ki_sc)+(b*ki_sc)/(a+b)));
analogWrite(enC, outdefuzzy3);
}

 //7-3
if (ei>peb &&  de>ndes && de<=azdel)
{
upeb=1;
undes=((azdel-de)/(azdel-ndes));
uazdel=((de-ndes)/(azdel-ndes));

//operator and
if (upeb<undes)  //ki_sc
{a=upeb;}
else
{a=undes;}

if (upeb<uazdel)  //ki_sc
{b=upeb;}
else
{b=uazdel;}


//output defuzzy

outdefuzzy3=(((a*ki_sc)+(b*ki_sc)/(a+b)));
analogWrite(enC, outdefuzzy3);
}

//7-4
if (ei>peb && de>azdel && de<=azdeh )
{
upeb=1;
uazde=1;


//output defuzzy

outdefuzzy3=ki_sc;
analogWrite(enC, outdefuzzy3);
}

//7-5
if (ei>peb && de>azdeh && de<=pdes)
{
upeb=1;
uazdeh=((pdes-de)/(pdes-azdeh));
updes=((de-azdeh)/(pdes-azdeh));

//operator and
if (upeb<uazdeh)  //ki_sc
{a=upeb;}
else
{a=uazdeh;}

if (upeb<updes)  //ki_sc
{b=upeb;}
else
{b=updes;}



//output defuzzy

outdefuzzy3=(((a*ki_sc)+(b*ki_sc))/(a+b));
analogWrite(enC, outdefuzzy3);
}

 //7-6
if (ei>peb &&  de>pdes && de<=pdeb)
{
upeb=1;
updes=((pdeb-de)/(pdeb-pdes));
updeb=((de-pdes)/(pdeb-pdes));

//operator and
if (upeb<updes)  //ki_sc
{a=upeb;}
else
{a=updes;}

if (upeb<updeb)  //ki_sc
{b=upeb;}
else
{b=updeb;}

//output defuzzy

outdefuzzy3=(((a*ki_sc)+(b*ki_sc))/(a+b));
analogWrite(enC, outdefuzzy3);
}

//7-7
if (ei>peb && de>pdeb )
{
upeb=1;
updeb=1;


//output defuzzy

outdefuzzy3=ki_sc;
analogWrite(enC, outdefuzzy3);

}

}

if(outdefuzzy3>0) {
digitalWrite(set5, HIGH);
digitalWrite(set6, LOW); //Gerak ke Bawah
}            //Gerak ke Bawah

if(outdefuzzy3<0) {
digitalWrite(set5, LOW); //Gerak ke Atas
digitalWrite(set6, HIGH);
}

if(outdefuzzy3=0) {
digitalWrite(set5, LOW);
digitalWrite(set6, LOW);   // STOP
}
}

Schematics

Block Diagram
Block diagram ztseeyqy8c

Comments

Similar projects you might like

Cocktail Parasol

by alain_haerri

  • 165 views
  • 5 comments
  • 2 respects

Arduino-Powered Robotic Arm Controlled with T-Skin!

Project showcase by Michele Valentini and Massimiliano

  • 2,311 views
  • 2 comments
  • 22 respects

GRawler - The Glass Roof Cleaner

Project tutorial by Gelstronic

  • 5,179 views
  • 3 comments
  • 23 respects

Homemade Claw Machine

Project tutorial by -MMM-

  • 3,372 views
  • 4 comments
  • 22 respects

SpeechLess

by tazasproject

  • 257 views
  • 0 comments
  • 5 respects

Arduino Motorcycle Tail Lights

Project showcase by david schneider

  • 3,773 views
  • 6 comments
  • 13 respects
Add projectSign up / Login