# Function Problem. (Void Loop)

I have tried in different ways this code but i cant run two different functions within the void loop .

one of the code block is for IMU, servos and other one for pot and servos. When running this code GYRO and servos is working well but i cant control servos by pot.
Best Regards.

``````void loop(){

servoVal = map(servoVal, 0, 1023, 0, 179);

pservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

dservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

oservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

eservo.write(servoVal);

delay(15);

}

{
/* Update all the values */
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);

/* Calculate the angls based on the different sensors and algorithm */

double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);

//gyroX
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter

gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter

mapX = map(kalAngleX, 0, 200, 0, 179);     //calculate limitation of servo mechanical

kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);

mapY = map(kalAngleY, 0, 200, 0, 179);

timer = micros();

//gyroY

// /////////////////////////////

correctionX = 27;     // EDIT THIS VALUE FOR SERVO CORRECTION ANGLE

correctionY = 27;     // EDIT THIS VALUE FOR SERVO CORRECTION ANGLE

// ////////////////////////////

moveX = 270 - (kalAngleX) + correctionX;
moveY = 270 - (kalAngleY) + correctionY;

// ------- SEND TO SERVO START ----- /

xservo.write(moveX);   // Send signal to servo
delay(15);     // delay to allow servos to move (ms)
yservo.write(moveY);   // Send signal to servo
delay(15);     // delay to allow servos to move (ms)

// ------- SEND TO SERVO END ----- /

delay(1); // The accelerometer's maximum samples rate is 1kHz
}

// -- FUNCTIONS START --
Wire.write(data);
Wire.endTransmission();                       // Send stop
}

uint8_t data[nbytes];
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes);   // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
return data;
}
``````

That is 4 servos being controlled by 4 potentiometer. What is wrong with it ?

You can use a single delay(15); You could write the value of the potentiometer to the Serial monitor.

when i upload this code, pots and servos code block is not working. there is work only servos by gyro. i cant control all of servos. can i write different kind of function for 4 servos and 4 pots in void loop? i tried tons of different method but i cant.

All of this code.

``````#include <Servo.h>
const int pot1 = 0;
const int pot2 = 1;
const int pot3 = 2;
const int pot4 = 3;
int servoVal;
Servo xservo;
Servo yservo;
Servo pservo;
Servo dservo;
Servo oservo;
Servo eservo;

#include <Wire.h>
#include "Kalman.h"
Kalman kalmanX;
Kalman kalmanY;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

int moveX;
int mapX;
int correctionX;
int moveY;
int mapY;
int correctionY;

double accXangle;
double accYangle;
double gyroXangle = 9;
double gyroYangle = 9;
double compAngleX = 90;
double compAngleY = 90;
double kalAngleX;
double kalAngleY;
uint32_t timer;

void setup() {

xservo.attach(13);
yservo.attach(12);
pservo.attach(11);
dservo.attach(10);
oservo.attach(9);
eservo.attach(8);

Wire.begin();
i2cWrite(0x6B,0x00);           // Disable sleep mode

while(1);
}
kalmanX.setAngle(90);  // Set starting angle
kalmanY.setAngle(90);
timer = micros();
}

void loop() {
/* Update all the values */
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);

/* Calculate the angls based on the different sensors and algorithm */

double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);

//gyroX
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter

gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter

mapX = map(kalAngleX, 0, 200, 0, 179);     //calculate limitation of servo mechanical

kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);

mapY = map(kalAngleY, 0, 200, 0, 179);

timer = micros();

//gyroY

// /////////////////////////////

correctionX = 27;     // EDIT THIS VALUE FOR SERVO CORRECTION ANGLE

correctionY = 27;     // EDIT THIS VALUE FOR SERVO CORRECTION ANGLE

// ////////////////////////////

moveX = 270 - (kalAngleX) + correctionX;
moveY = 270 - (kalAngleY) + correctionY;

// ------- SEND TO SERVO START ----- /

xservo.write(moveX);   // Send signal to servo
delay(15);     // delay to allow servos to move (ms)
yservo.write(moveY);   // Send signal to servo
delay(15);     // delay to allow servos to move (ms)

servoVal = map(servoVal, 0, 1023, 0, 179);

pservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

dservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

oservo.write(servoVal);

delay(15);

servoVal = map(servoVal, 0, 1023, 0, 179);

eservo.write(servoVal);

delay(15);

// ------- SEND TO SERVO END ----- /

delay(1); // The accelerometer's maximum samples rate is 1kHz
}

// -- FUNCTIONS START --
Wire.write(data);
Wire.endTransmission();                       // Send stop
}

uint8_t data[nbytes];
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes);   // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)