Chieftain kit arrival

[Youngjae Bae]

I love the test rig

It looks to me as though the actuator operates much faster when controlled by the receiver than by the Arduino. Do you think that is the case ?

I don't think Google understood what you said here: "I found that sensors are more sensitive than raw giggles"

Adrian.

Adrian has a very good eye.
As you can see, the speed that the receiver controls is the maximum speed.
The control speed of the Arduino receiving the signal from the sensor was reduced to 35% of the speed controlled by the transmitter.
Sensitivity of the sensor is sensitive, increasing the control output of the Arduino results in very harsh stabilization. I'm in the process of adjusting the variables little by little.
And the word Google couldn't find was an error in my keyboard.


Youngjae

[Stephen White]

Yougjae, you’ll get there in the end, great test rig. Looks like you’re experiencing the real challenge of developing an effective stabiliser, which is to make sure the stabilization remains in phase with the movement of the hull. If they get out of phase, the system becomes dynamicall unstable and you get divergent oscillations. Funny for those watching from a distance!

[Youngjae Bae]

I know that you wish to crack the stabilization thing, but have you had a look at this , "GSU 2 Axis Gun Stabilize Unit - 35 RC Tank".

Or the recently seen "K12 - 35 RC Tank"- RC Conversion Kit for TAMIYA 1/35 Challenger 2, with MTC-2, ACU and GSU"

Could they be adapted? It has 2 axis control and deck sensing to raise gun all in one package.

I got this information at the end of last year and purchased a set earlier this year and completed the analysis.
And I found it difficult to apply it on a scale of 1/6 without servo motors.
After I finish stabilizing the current main gun and turret, I will apply ultrasonic and color sensors to Arduino to apply the anti-collision system.

Youngjae

[John Clarke]

The 1/35 Chally 2 Stabilizer. Tiny thing just needs scaling up a bit

https://youtu.be/o9R5iqPsXgo

[Youngjae Bae]

The 1/35 Chally 2 Stabilizer. Tiny thing just needs scaling up a bit

https://youtu.be/c1Q3BHgK2nc


Chieftain Main Gun Stabilizer using my Arduino found a much more stable condition than yesterday.
I will apply this result to my chieftain about this weekend, which perfectly fits the center of gravity, to get practical results.
And I will make changes if necessary.

And...... there's another interesting challenge.
As John Clarke said, I'm going to build a new controller by amplifying the capacity of the kits I've previously purchased at 35RC.
If I take advantage of the characteristics of Arduino that I learned for two months, I think I can integrate all signals from this control unit into Arduino to create a control system for 24V.
Of course, I may fail, but I've concluded that the theoretical review that I've been thinking about all day will be possible.
My new challenge continues.

Youngjae

[Adrian Harris]

It would be good to see your system working under slow conditions, maybe with a glass of water on the end of the barrel...

https://www.youtube.com/watch?v=7dd0Azxb2N0

Chieftain only had a cross country performance of about 18mph, which is roughly walking pace at our scale, and probably wouldn't be travelling over the worst contours at full speed, so I feel smoothness and accuracy of the stabilisation is probably more important than raw speed of response.

Adrian.

[Stephen White]

Adrian, it’s remarkable what a spot of superglue under the stein and some cling film over the beer will do to prevent spillage. But you’re right about smoothness over speed. The Chieftain stabilization system was by far the best system on any tank during the early days. I was able to watch T-64B firing on the move in East Germany and the Russians were able to frighten the targets firing on the move. With Chieftain, up to about 1200m, we aimed to kill the target and usually did.

[Adrian Harris]

I did find another video on beer stabilisation and the Russian tank, which may have been a T-64, did appear to be spilling some.

I can't believe they would stoop to cling film and superglue. I would have thought gelatin in the beer would have been enough

Adrian.

[Youngjae Bae]

It is not difficult to operate slowly because it has handled the rapid response.
Someone could refer to this source code and Hardware.
The variables obtained through the experiment were modified in the code that was previously uploaded.
The measurement speed of the MPU9250 has been increased to the maximum value expressed.
And I changed the attachment position of the sensor.
The direction of rotation of the motor may be used by the individual who needs to change the code himself.
"PMAC2A PC/104 - Delta Tau Data Systems," a more dedicated motion controller than Arduino, deviates from the price of hardware and the price of the program and That's more than a hobby's price.
I'm satisfied with the cost-effectiveness.
Above all, it is a great pleasure to do what you want to realize.

Youngjae

[Youngjae Bae]

I have changed my control method as below while searching for a lot of data over the past few days.
I focused on writing the source code a little while ago with the control method, also known as feedback(PID) control.
First, when I go home today, I will compile this code and try to improve the problem.
I will explain the interpretation consistently when the program is perfect.

Youngjae

=====================================================================================================================

/* 2020-07-06
* This source code was written by Youngjae Bae to reduce unnecessary elements and stabilize
* the main gun and turret of the big-scale model tank by referring to the code of
* Kris Winer and the condition statement of Vince Cutajar.
* I have introduced feedback(PID) control because I want the device to be a little softer.
*
* Based on the Arduino Mega, the power of the transmitter must be turned on by dust and
* supplied by the receiver and Arduino, and when switched off,
* the power of the transmitter must be shut off at the end.
*
* The transmitters and receivers for RC use spectrum DX6 and AR610.
* The motor driver used the SZDLT two-channel driver.
*/


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

// Main Control Parameters - The sensitivity or intensity of the test is adjusted here.
#define CheckTime 120 // (milliseconds) The shorter the time to check the displacement, the more vibration it can reflect.
#define deadzone 50 //(PWM) Anything between -50 and 50 is stop
#define ConstrainLimit 254 //(PWM) Constrain limits, Maximum of the strength of the motor rotation, usually 255
double kpPitch = 1.2, kiPitch = 2.5 , kdPitch = 3.2;
double kpYaw = 1.2, kiYaw = 2.5, kdYaw = 3.2;
double outMinPitch = -254, outMaxPitch = 254;
double outMinYaw = -254, outMaxYaw = 254;

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

// File call to use MPU9250 and quadrant formula
#include "MPU9250.h" // Make sure to upload the first head-up file of Kris Winer.
#include "quaternionFilters.h" // There are many head-up files of the same name, and if this is misselected, it is impossible to compile.

// MPU9250 setup
#define SCL 700000 // Pre-defined serial clock rate
#define SDA Wire // Use the built-in Wire Library for I2C communication
#define MPU9250_ADDRESS 0x68 // Address for using MPU9250 as 5V

MPU9250 IMU(MPU9250_ADDRESS, SDA, SCL); // Using MPU9250 named internal classes as IMU functions

// Transmitter connections
#define THRO_PIN 22 // THRO PIN IN AR610 CONNECTED TO Aduino Mega No.22
#define RUDD_PIN 23 // Can be replaced with your preferred pins
#define AUX1_PIN 52 // A switch that holds the main gun and turret

// Motor driver pins
#define A1_PIN 3 // Connect to each pin of the Arduino and SZDLT or L298N two-channel driver.
#define A2_PIN 4 // Can be replaced with your preferred pins
#define PA_PIN 2 // Two G's of the driver are connected to the G of the Arduino
#define B1_PIN 5 // and one of the two 5Vs is connected to the VIN of the Arduino later to provide power.
#define B2_PIN 6
#define PB_PIN 7

// Declaration of Variables for Stabilization
int THRO, RUDD, AUX1 = 0; // Declares the rudder and throttle channels as integer variables and defines zero

// Set to true to get Serial output for debugging
#define SerialDebug true

// working PID variables
unsigned long lastTime;
double errorPitch = 0, errorYaw = 0;
double InputPitch = IMU.pitch;
double OutputPitch, SetpointPitch;
double InputYaw = IMU.yaw;
double OutputYaw, SetpointYaw;
double ITermPitch, lastInputPitch;
double ITermYaw, lastInputYaw;
int SampleTime = (CheckTime / 4);
bool inAuto_PID = false;
#define MANUAL_PID 0
#define AUTOMATIC_PID 1
#define DIRECT_PID 0
#define REVERSE_PID 1
int controllerDirection = DIRECT_PID;


void setup() {

// Define a Pin Using Arduino
pinMode(THRO_PIN, INPUT); //Determine the input or output of the pins used by Arduino
pinMode(RUDD_PIN, INPUT);
pinMode(AUX1_PIN, INPUT);
pinMode(A1_PIN, OUTPUT);
pinMode(A2_PIN, OUTPUT);
pinMode(PA_PIN, OUTPUT);
pinMode(B1_PIN, OUTPUT);
pinMode(B2_PIN, OUTPUT);
pinMode(PB_PIN, OUTPUT);

// Settings for MPU9250 sensor Usage
IMU.initAK8963(IMU.factoryMagCalibration); // Initialize compass
IMU.initMPU9250(); // Initialize acceleration, gyro, etc.
IMU.getAres(); // Define Acceleration Usage Functions
IMU.getGres(); // Definition of gyro function
IMU.getMres(); // Magnetism Usage Definitions

Serial.begin(115200); // Specify serial communication calls and baud rates

}

void loop() {

IMU.readAccelData(IMU.accelCount); // Read the x/y/z adc values
IMU.ax = (double)IMU.accelCount[0] * IMU.aRes; // - IMU.accelBias[0];
IMU.ay = (double)IMU.accelCount[1] * IMU.aRes; // - IMU.accelBias[1];
IMU.az = (double)IMU.accelCount[2] * IMU.aRes; // - IMU.accelBias[2];

IMU.readGyroData(IMU.gyroCount); // Read the x/y/z adc values
IMU.gx = (double)IMU.gyroCount[0] * IMU.gRes;
IMU.gy = (double)IMU.gyroCount[1] * IMU.gRes;
IMU.gz = (double)IMU.gyroCount[2] * IMU.gRes;

IMU.readMagData(IMU.magCount); // Read the x/y/z adc values
IMU.mx = (double)IMU.magCount[0] * IMU.mRes * IMU.factoryMagCalibration[0] - IMU.magBias[0];
IMU.my = (double)IMU.magCount[1] * IMU.mRes * IMU.factoryMagCalibration[1] - IMU.magBias[1];
IMU.mz = (double)IMU.magCount[2] * IMU.mRes * IMU.factoryMagCalibration[2] - IMU.magBias[2];

IMU.updateTime();// Must be called before updating quaternions!

// conversion to quaternions
MahonyQuaternionUpdate(IMU.ax, IMU.ay, IMU.az, IMU.gx * DEG_TO_RAD, IMU.gy * DEG_TO_RAD, IMU.gz * DEG_TO_RAD, IMU.my, IMU.mx, IMU.mz, IMU.deltat);

IMU.delt_t = millis() - IMU.count;

if (IMU.delt_t > CheckTime) {
IMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()* *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)* *(getQ()+3));
IMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()* *(getQ()+2)));
IMU.pitch *= RAD_TO_DEG;
IMU.yaw *= RAD_TO_DEG;

// Read pulse width from receiver
THRO = pulseIn(THRO_PIN, HIGH);
RUDD = pulseIn(RUDD_PIN, HIGH);
AUX1 = pulseIn(AUX1_PIN, HIGH);

// Convert to PWM value (-ConstrainLimit to ConstrainLimit)
THRO = pulseToPWM(THRO);
RUDD = pulseToPWM(RUDD);
AUX1 = pulseToPWM(AUX1);

if(SerialDebug) {
Serial.print(THRO);
Serial.print(" ");
Serial.print(THRO);
Serial.println("This value is a GUN position.");
Serial.println();
Serial.print(RUDD);
Serial.print(" ");
Serial.print(RUDD);
Serial.println("This value is a TURRET position.");
Serial.println();
}

if (AUX1 == 0) { // The ability to hold the main gun and turret forcibly
digitalWrite(A1_PIN, LOW); // if the transmitter signal is missing or required.
digitalWrite(A2_PIN, LOW);
digitalWrite(B1_PIN, LOW);
digitalWrite(B2_PIN, LOW);
return inAuto_PID;

}
else {
if( THRO != 0 || RUDD != 0 ) {
// THRO and RUDD Drive motor move
drive(THRO, RUDD);
return SetpointPitch = IMU.pitch;
return SetpointYaw = IMU.yaw;
return !inAuto_PID;
}
else {
Compute();
//pitch stabilization
if (errorPitch > 0) {
digitalWrite(A1_PIN, HIGH);
digitalWrite(A2_PIN, LOW);
analogWrite(PA_PIN, OutputPitch);
}
else if (errorPitch < 0) {
digitalWrite(A1_PIN, LOW);
digitalWrite(A2_PIN, HIGH);
analogWrite(PA_PIN, OutputPitch);
}
else {
digitalWrite(A1_PIN, LOW);
digitalWrite(A2_PIN, LOW);
analogWrite(PA_PIN, 0);
}
if(SerialDebug) {
Serial.print(errorPitch);
Serial.print(" ");
Serial.print(errorPitch);
Serial.println("This is Pitch difference..");
Serial.println();
}

//Yaw stabilization
if (errorYaw > 0) {
digitalWrite(B1_PIN, HIGH);
digitalWrite(B2_PIN, LOW);
analogWrite(PB_PIN, OutputYaw);
}
else if (errorYaw < 0) {
digitalWrite(B1_PIN, LOW);
digitalWrite(B2_PIN, HIGH);
analogWrite(PB_PIN, OutputYaw);
}
else {
digitalWrite(B1_PIN, LOW);
digitalWrite(B2_PIN, LOW);
analogWrite(PB_PIN, 0);
}
if(SerialDebug) {
Serial.print(errorYaw);
Serial.print(" ");
Serial.print(errorYaw);
Serial.println("This is Yaw difference.");
Serial.println();
}
}
}
IMU.count = millis();
IMU.sumCount = 0;
IMU.sum = 0;
}
}

// Positive for forward, negative for reverse
void drive(int THRO, int RUDD) {

// Limit speed between -ConstrainLimit and ConstrainLimit
THRO = constrain(THRO, -ConstrainLimit, ConstrainLimit);
RUDD = constrain(RUDD, -ConstrainLimit, ConstrainLimit);

// Set direction for motor A
if ( THRO == 0 ) {

digitalWrite(A1_PIN, LOW);
digitalWrite(A2_PIN, LOW);

}

else if ( THRO > 0 ) {

digitalWrite(A1_PIN, HIGH);
digitalWrite(A2_PIN, LOW);

} else {
digitalWrite(A1_PIN, LOW);
digitalWrite(A2_PIN, HIGH);
}

// Set direction for motor B
if ( RUDD == 0 ) {

digitalWrite(B1_PIN, LOW);
digitalWrite(B2_PIN, LOW);

} else if ( RUDD > 0 ) {
digitalWrite(B1_PIN, HIGH);
digitalWrite(B2_PIN, LOW);

} else {
digitalWrite(B1_PIN, LOW);
digitalWrite(B2_PIN, HIGH);
}

// Set speed
analogWrite(PA_PIN, abs(THRO));
analogWrite(PB_PIN, abs(RUDD));
}

// Convert RC pulse value to motor PWM value
int pulseToPWM(int pulse) {

// If we're receiving numbers, convert them to motor PWM
if ( pulse > 900 ) {
pulse = constrain(map(pulse, 1058, 1877, -2*ConstrainLimit, 2*ConstrainLimit), -ConstrainLimit, ConstrainLimit);
} else {
pulse = 0;
}

// Anything in deadzone should stop the motor
if ( abs(pulse - 0) <= deadzone ) {
pulse = 0;
}
return pulse;
}

void Compute(){
if(!inAuto_PID) return;

// How long since I last calculated
unsigned long now = millis();
int timeChange = (now - lastTime);
if(timeChange >= SampleTime){

// Compute all the working error variables
double errorPitch = SetpointPitch - InputPitch;
double errorYaw = SetpointYaw - InputYaw;
ITermPitch += (kiPitch * errorPitch);
ITermYaw += (kiYaw * errorYaw);
if(ITermPitch > outMaxPitch) ITermPitch = outMaxPitch;
else if(ITermPitch < outMinPitch) ITermPitch = outMinPitch;
if(ITermYaw > outMaxYaw) ITermYaw= outMaxYaw;
else if(ITermYaw < outMinYaw) ITermYaw= outMinYaw;
double dInputPitch = (InputPitch - lastInputPitch);
double dInputYaw = (InputYaw - lastInputYaw);

// Compute PID Output
OutputPitch = kpPitch * errorPitch + ITermPitch - kdPitch * dInputPitch;
OutputYaw = kpYaw * errorYaw + ITermYaw - kdYaw * dInputYaw;

if(OutputPitch > outMaxPitch) OutputPitch = outMaxPitch;
else if(OutputPitch < outMinPitch) OutputPitch = outMinPitch;
if(OutputYaw > outMaxYaw) OutputYaw = outMaxYaw;
else if(OutputYaw < outMinYaw) OutputYaw = outMinYaw;

// Remember some variables for next time
lastInputPitch = InputPitch;
lastInputYaw = InputYaw;
lastTime = now;
}
}

void SetTuningsPitch(double KpPitch, double KiPitch, double KdPitch) {
if (KpPitch < 0 || KiPitch < 0 || KdPitch < 0) return;
double SampleTimeInSec = ((double)SampleTime)/1000;
kpPitch = KpPitch;
kiPitch = KiPitch * SampleTimeInSec;
kdPitch = KdPitch / SampleTimeInSec;
if(controllerDirection == REVERSE_PID) {
kpPitch = (0 - kpPitch);
kiPitch = (0 - kiPitch);
kdPitch = (0 - kdPitch);
}
}

void SetTuningsYaw(double KpYaw, double KiYaw, double KdYaw) {
if (KpYaw < 0 || KiYaw < 0 || KdYaw < 0) return;
double SampleTimeInSec = ((double)SampleTime)/1000;
kpYaw = KpYaw;
kiYaw = KiYaw * SampleTimeInSec;
kdYaw = KdYaw / SampleTimeInSec;
if(controllerDirection == REVERSE_PID) {
kpYaw = (0 - kpYaw);
kiYaw = (0 - kiYaw);
kdYaw = (0 - kdYaw);
}
}

void SetSampleTime(int NewSampleTime) {
if (NewSampleTime > 0) {
double ratio = (double)NewSampleTime / (double)SampleTime;
kiPitch *= ratio;
kdPitch /= ratio;
kiYaw *= ratio;
kdYaw /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}

void SetOutputLimitsPitch(double MinPitch, double MaxPitch) {
if(MinPitch > MaxPitch) return;
outMinPitch = MinPitch;
outMaxPitch = MaxPitch;
if(OutputPitch > outMaxPitch) OutputPitch = outMaxPitch;
else if(OutputPitch < outMinPitch) OutputPitch = outMinPitch;
if(ITermPitch > outMaxPitch) ITermPitch = outMaxPitch;
else if(ITermPitch < outMinPitch) ITermPitch = outMinPitch;
}

void SetOutputLimitsYaw(double MinYaw, double MaxYaw) {
if(MinYaw > MaxYaw) return;
outMinYaw = MinYaw;
outMaxYaw = MaxYaw;
if(OutputYaw > outMaxYaw) OutputYaw = outMaxYaw;
else if(OutputYaw < outMinYaw) OutputYaw = outMinYaw;
if(ITermYaw > outMaxYaw) ITermYaw = outMaxYaw;
else if(ITermYaw < outMinYaw) ITermYaw = outMinYaw;
}

void SetMode(int Mode) {
bool newAuto_PID = (Mode == AUTOMATIC_PID);
if(newAuto_PID && !inAuto_PID){
// I just went from manual to auto
Initialize_PID();
}
inAuto_PID = newAuto_PID;
}

void Initialize_PID() {
lastInputPitch = InputPitch;
lastInputYaw = InputYaw;
ITermPitch = OutputPitch;
ITermYaw = OutputYaw;
if(ITermPitch > outMaxPitch) ITermPitch = outMaxPitch;
else if(ITermPitch < outMinPitch) ITermPitch = outMinPitch;
if(ITermYaw > outMaxYaw) ITermYaw = outMaxYaw;
else if(ITermYaw < outMinYaw) ITermYaw = outMinYaw;
}

void SetControllerDirection(int Direction_PID) {
controllerDirection = Direction_PID;
}

[Youngjae Bae]

Following yesterday, today is a code that utilizes Sabertoot(motion controller B) using motor controller as below.
If anyone uses this code, they can use it while tuning the kp, ki, kd values.

Youngjae

==============================================================================================================================/////////////////////////////////////////////////////////////////////////////////////////////

/* Main Control Parameters - The sensitivity or intensity of the test is adjusted here.*/

#define CheckTime 240 // (milliseconds) The shorter the time to check the displacement, the more vibration it can reflect.
#define deadzone 150 // (PWM) Anything between -150 and 150 is stop
#define SerialDebug true
double outMinPitch = 1000, outMaxPitch = 2000;
double outMinYaw = 1000, outMaxYaw = 2000;
double kpPitch = 1.2, kiPitch = 2.5 , kdPitch = 3.2;
double kpYaw = 1.2, kiYaw = 2.5, kdYaw = 3.2;

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

/* File call to use MPU9250 and quadrant formula */
#include "MPU9250.h" // Make sure to upload the first head-up file of Kris Winer.
#include "quaternionFilters.h" // There are many head-up files of the same name, and if this is misselected, it is impossible to compile.
/* MPU9250 setup */
#define SCL 400000 // Pre-defined serial clock rate
#define SDA Wire // Use the built-in Wire Library for I2C communication
#define MPU9250_ADDRESS 0x68 // Address for using MPU9250 as 5V
/* Using MPU9250 named internal classes ad IMU function */
MPU9250 IMU(MPU9250_ADDRESS, SDA, SCL);
/*To use Sabertooth(Motion Controller B) */
#include <Servo.h>
/* Definition of the connection pin between RC receiver and Arduino */
#define THRO_PIN 22 // THRO PIN IN AR610 CONNECTED TO Aduino Mega No.22
#define RUDD_PIN 23 // Can be replaced with your preferred pins
#define AUX1_PIN 52 // A switch that holds the main gun and turret
/* Definition of the connection pin between Sabertooth and Arduino */
#define S1 2 // Arduino Pin 2 -> Sabertooth S1, Arduino GND -> Sabertooth 0V
#define S2 3 // Arduino Pin 3 -> Sabertooth S2, Arduino VIN <- Sabertooth 5V (OPTION)
/* Definition servo channel of Sabertooth */
Servo STS1; // To name the Sabertooth servo channel objects STS1 and STS2
Servo STS2; // Dip switch on are 2, 3, 5 The rest are off
/* Parameters for radio control */
int pulse, AUX1;
/* working PID variables */
unsigned long lastTime;
double errorPitch = 0, errorYaw = 0;
double InputPitch = IMU.pitch, OutputPitch, SetpointPitch;
double InputYaw = IMU.yaw, OutputYaw, SetpointYaw;
double ITermPitch, ITermYaw;
double outputSumPitch, lastInputPitch;
double outputSumYaw, lastInputYaw;
int SampleTime = (CheckTime / 4);
bool inAuto_PID = false;
#define MANUAL_PID 0
#define AUTOMATIC_PID 1
#define DIRECT_PID 0
#define REVERSE_PID 1
#define P_ON_M 0
#define P_ON_E 1
bool pOnE_Pitch, pOnE_Yaw = true;
int controllerDirection = DIRECT_PID;

void setup() {
/* Define a Pin Using Arduino */
pinMode(THRO_PIN, INPUT);
pinMode(RUDD_PIN, INPUT);
pinMode(AUX1_PIN, INPUT);
pinMode(S1, OUTPUT);
pinMode(S2, OUTPUT);
/* Attach pulse width value to servo channel */
STS1.attach(S1, 1000, 2000); // Sabertooth accepts servo pulses from 1000us to 2000us.
STS2.attach(S2, 1000, 2000); // I need to specify the pulse widths in attach().
/* Settings for MPU9250 sensor Usage */
IMU.initAK8963(IMU.factoryMagCalibration); // Initialize compass
IMU.initMPU9250(); // Initialize acceleration, gyro, etc.
IMU.getAres(); // Define Acceleration Usage Functions
IMU.getGres(); // Definition of gyro function
IMU.getMres(); // Magnetism Usage Definitions
/* Serial Communications for Debug */
Serial.begin(115200); // Specify serial communication calls and baud rates
}

void loop() {
mpu9250();
AUX1 = constrain(map((pulseIn(AUX1_PIN, HIGH)), 1058, 1887, 0, 1), 0, 1); // Convert RC pulse to switch signal (0 and 1)
if(SerialDebug) {
Serial.print("AUX1 =");
Serial.println(AUX1);
Serial.println();
}
Compute();
if(AUX1 == 0) { // Tansmitter turned off or switch off
STS1.writeMicroseconds(1500); // Stops the main gun un and down
STS2.writeMicroseconds(1500); // Stops the turret turn
return inAuto_PID;
}else{ // Tansmitter turned on and switch on
return !inAuto_PID;
if(abs(SetpointPitch - IMU.pitch) != 0) {
STS1.writeMicroseconds(OutputPitch); //pitch and yaw stabilization
}else{
STS1.writeMicroseconds(pulseTopulse(pulseIn(THRO_PIN, HIGH))); // Convert RC pulse and write to servo channel
return SetpointPitch = IMU.pitch;
}
if(abs(SetpointYaw - IMU.yaw) != 0) {
STS2.writeMicroseconds(OutputYaw);
}else{
STS2.writeMicroseconds(pulseTopulse(pulseIn(RUDD_PIN, HIGH)));
return SetpointYaw = IMU.yaw;
}
if(SerialDebug) {
Serial.print("IMU.pitch =");
Serial.println(IMU.pitch);
Serial.println();
Serial.println("IMU.yaw =");
Serial.println(IMU.yaw);
Serial.println();
}
}
}

/* Convert DX6 Transmitter pulse to servo pulse*/
int pulseTopulse(int pulse_r) {
if (pulse > 900) {
pulse = constrain(map(pulse_r, 1058, 1887, 1000, 2000), 1000, 2000);
} else {
return pulse = 1500; // Stop and hold position
}
if (abs(pulse - 1500) <= deadzone ) { // Anything in deadzone should stop the motor
return pulse = 1500; // Stop and hold position
} else {
return pulse;
}
}

void mpu9250() {
IMU.readAccelData(IMU.accelCount); // Read the x/y/z adc values
IMU.ax = (float)IMU.accelCount[0] * IMU.aRes; // - IMU.accelBias[0];
IMU.ay = (float)IMU.accelCount[1] * IMU.aRes; // - IMU.accelBias[1];
IMU.az = (float)IMU.accelCount[2] * IMU.aRes; // - IMU.accelBias[2];
IMU.readGyroData(IMU.gyroCount); // Read the x/y/z adc values
IMU.gx = (float)IMU.gyroCount[0] * IMU.gRes;
IMU.gy = (float)IMU.gyroCount[1] * IMU.gRes;
IMU.gz = (float)IMU.gyroCount[2] * IMU.gRes;
IMU.readMagData(IMU.magCount); // Read the x/y/z adc values
IMU.mx = (float)IMU.magCount[0] * IMU.mRes * IMU.factoryMagCalibration[0] - IMU.magBias[0];
IMU.my = (float)IMU.magCount[1] * IMU.mRes * IMU.factoryMagCalibration[1] - IMU.magBias[1];
IMU.mz = (float)IMU.magCount[2] * IMU.mRes * IMU.factoryMagCalibration[2] - IMU.magBias[2];
IMU.updateTime();// Must be called before updating quaternions!
/* Conversion to quaternions */
MahonyQuaternionUpdate(IMU.ax, IMU.ay, IMU.az, IMU.gx * DEG_TO_RAD, IMU.gy * DEG_TO_RAD, IMU.gz * DEG_TO_RAD, IMU.my, IMU.mx, IMU.mz, IMU.deltat);
IMU.delt_t = millis() - IMU.count;
if (IMU.delt_t > CheckTime) {
IMU.yaw = (atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ()* *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1)* *(getQ()+1) - *(getQ()+2) * *(getQ()+2) - *(getQ()+3)* *(getQ()+3)))*RAD_TO_DEG;
IMU.pitch = (-asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ()* *(getQ()+2))))*RAD_TO_DEG;
if(SerialDebug) {
Serial.println("Pitch position:");
Serial.println();
Serial.print(IMU.pitch);
Serial.println(" ");
Serial.println("Yaw position:");
Serial.println();
Serial.print(IMU.yaw);
Serial.print(" ");
}
IMU.count = millis();
IMU.sumCount = 0;
IMU.sum = 0;
}
}

void Compute(){
if(!inAuto_PID) return;
unsigned long now = millis(); // How long since I last calculated
int timeChange = (now - lastTime);
if(timeChange >= SampleTime){
double errorPitch = SetpointPitch - InputPitch; // Compute all the working error variables
double errorYaw = SetpointYaw - InputYaw;
double dInputPitch = (InputPitch - lastInputPitch);
double dInputYaw = (InputYaw - lastInputYaw);
outputSumPitch += (kiPitch * errorPitch);
outputSumYaw += (kiYaw * errorYaw);

/*Add Proportional on Measurement, if P_ON_M is specified*/
if(!pOnE_Pitch) outputSumPitch -= kpPitch * dInputPitch;
if(!pOnE_Yaw) outputSumYaw -= kpYaw * dInputYaw;

if(outputSumPitch > outMaxPitch) outputSumPitch = outMaxPitch;
else if(outputSumPitch < outMinPitch) outputSumPitch = outMinPitch;
if(outputSumYaw > outMaxYaw) outputSumYaw = outMaxYaw;
else if(outputSumYaw < outMinYaw) outputSumYaw = outMinYaw;

/*Add Proportional on Error, if P_ON_E is specified*/
if(pOnE_Pitch) OutputPitch = kpPitch * errorPitch;
else OutputPitch = 0;
if(pOnE_Yaw) OutputYaw = kpYaw * errorYaw;
else OutputYaw = 0;

/*Compute Rest of PID Output*/
OutputPitch += outputSumPitch - kdPitch * dInputPitch;
OutputYaw += outputSumYaw - kdYaw * dInputYaw;

if(OutputPitch > outMaxPitch) OutputPitch = outMaxPitch;
else if(OutputPitch < outMinPitch) OutputPitch = outMinPitch;
if(OutputYaw > outMaxYaw) OutputYaw = outMaxYaw;
else if(OutputYaw < outMinYaw) OutputYaw = outMinYaw;

/*Remember some variables for next time*/
lastInputPitch = InputPitch;
lastInputYaw = InputYaw;
lastTime = now;
}
}

void SetTuningsPitch(double KpPitch, double KiPitch, double KdPitch, int pOnPitch ) {
if (KpPitch < 0 || KiPitch < 0 || KdPitch < 0) return;
pOnE_Pitch = pOnPitch == P_ON_E;
double SampleTimeInSec = ((double)SampleTime)/1000;
kpPitch = KpPitch;
kiPitch = KiPitch * SampleTimeInSec;
kdPitch = KdPitch / SampleTimeInSec;
if(controllerDirection == REVERSE_PID) {
kpPitch = (0 - kpPitch);
kiPitch = (0 - kiPitch);
kdPitch = (0 - kdPitch);
}
}

void SetTuningsYaw(double KpYaw, double KiYaw, double KdYaw, int pOnYaw) {
if (KpYaw < 0 || KiYaw < 0 || KdYaw < 0) return;
pOnE_Yaw = pOnYaw == P_ON_E;
double SampleTimeInSec = ((double)SampleTime)/1000;
kpYaw = KpYaw;
kiYaw = KiYaw * SampleTimeInSec;
kdYaw = KdYaw / SampleTimeInSec;
if(controllerDirection == REVERSE_PID) {
kpYaw = (0 - kpYaw);
kiYaw = (0 - kiYaw);
kdYaw = (0 - kdYaw);
}
}

void SetSampleTime(int NewSampleTime) {
if (NewSampleTime > 0) {
double ratio = (double)NewSampleTime / (double)SampleTime;
kiPitch *= ratio;
kdPitch /= ratio;
kiYaw *= ratio;
kdYaw /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}

void SetOutputLimitsPitch(double MinPitch, double MaxPitch) {
if(MinPitch > MaxPitch) return;
outMinPitch = MinPitch;
outMaxPitch = MaxPitch;
if(OutputPitch > outMaxPitch) OutputPitch = outMaxPitch;
else if(OutputPitch < outMinPitch) OutputPitch = outMinPitch;
if(ITermPitch > outMaxPitch) ITermPitch = outMaxPitch;
else if(ITermPitch < outMinPitch) ITermPitch = outMinPitch;
}

void SetOutputLimitsYaw(double MinYaw, double MaxYaw) {
if(MinYaw > MaxYaw) return;
outMinYaw = MinYaw;
outMaxYaw = MaxYaw;
if(OutputYaw > outMaxYaw) OutputYaw = outMaxYaw;
else if(OutputYaw < outMinYaw) OutputYaw = outMinYaw;
if(ITermYaw > outMaxYaw) ITermYaw = outMaxYaw;
else if(ITermYaw < outMinYaw) ITermYaw = outMinYaw;
}

void SetMode(int Mode) {
bool newAuto_PID = (Mode == AUTOMATIC_PID);
if(newAuto_PID && !inAuto_PID){
// I just went from manual to auto
Initialize_PID();
}
inAuto_PID = newAuto_PID;
}

void Initialize_PID() {
lastInputPitch = InputPitch;
lastInputYaw = InputYaw;

outputSumPitch = OutputPitch;
outputSumYaw = OutputYaw;
if(outputSumPitch > outMaxPitch) outputSumPitch = outMaxPitch;
else if(outputSumPitch < outMinPitch) outputSumPitch = outMinPitch;
if(outputSumYaw > outMaxYaw) outputSumYaw = outMaxYaw;
else if(outputSumYaw < outMinYaw) outputSumYaw = outMinYaw;
}

void SetControllerDirection(int Direction_PID) {
controllerDirection = Direction_PID;
}

[Youngjae Bae]

Below is a code table of Arduino (due) regarding the 3-axis camera stabilizer.
I recently learned about the strength of Kalman Filter while studying stabilization, and I share it because it is worth referring to.
The gimbal works very well in this camera.

Youngjae

====================================================================================================================
#include <EEPROM.h>
#include <Kalman.h>
#include <MPU9250.h>

//Kalman filter objects created
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;

// MPU9250 object created.
#define SCL 700000 // Pre-defined serial clock rate
#define SDA Wire // Use the built-in Wire Library for I2C communication
#define MPU9250_ADDRESS 0x68 // Address for using MPU9250 as 5V
MPU9250 IMU(MPU9250_ADDRESS, SDA, SCL);
int status;
double timer;
double pitch, roll, yaw;
float ax, ay, az, accX, accY, accZ;
float gxb, gyb, gzb, gx, gy, gz, mxb, myb, mzb, mxs, mys, mzs;
double magx, magy, magz;
double r2d = (180 / M_PI);
double mx, my, mz;
double freq = 500;
int sineArraySize;
int table[] = {128, 130, 132, 134, 136, 138, 139, 141,
143, 145, 147, 149, 151, 153, 155, 157,
159, 161, 163, 165, 167, 169, 171, 173,
174, 176, 178, 180, 182, 184, 185, 187,
189, 191, 192, 194, 196, 198, 199, 201,
202, 204, 206, 207, 209, 210, 212, 213,
215, 216, 218, 219, 220, 222, 223, 224,
226, 227, 228, 229, 231, 232, 233, 234,
235, 236, 237, 238, 239, 240, 241, 242,
243, 244, 245, 245, 246, 247, 247, 248,
249, 249, 250, 250, 251, 251, 252, 252,
253, 253, 253, 254, 254, 254, 254, 255,
255, 255, 255, 255, 255, 255, 255, 255,
255, 255, 254, 254, 254, 254, 253, 253,
253, 252, 252, 251, 251, 250, 250, 249,
249, 248, 247, 247, 246, 245, 245, 244,
243, 242, 241, 240, 239, 238, 237, 236,
235, 234, 233, 232, 231, 229, 228, 227,
226, 224, 223, 222, 220, 219, 218, 216,
215, 213, 212, 210, 209, 207, 206, 204,
202, 201, 199, 198, 196, 194, 192, 191,
189, 187, 185, 184, 182, 180, 178, 176,
174, 173, 171, 169, 167, 165, 163, 161,
159, 157, 155, 153, 151, 149, 147, 145,
143, 141, 139, 138, 136, 134, 132, 130,
128, 125, 123, 121, 119, 117, 116, 114,
112, 110, 108, 106, 104, 102, 100, 98,
96, 94, 92, 90, 88, 86, 84, 82,
81, 79, 77, 75, 73, 71, 70, 68,
66, 64, 63, 61, 59, 57, 56, 54,
53, 51, 49, 48, 46, 45, 43, 42,
40, 39, 37, 36, 35, 33, 32, 31,
29, 28, 27, 26, 24, 23, 22, 21,
20, 19, 18, 17, 16, 15, 14, 13,
12, 11, 10, 10, 9, 8, 8, 7,
6, 6, 5, 5, 4, 4, 3, 3,
2, 2, 2, 1, 1, 1, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1, 1, 1, 1, 2, 2,
2, 3, 3, 4, 4, 5, 5, 6,
6, 7, 8, 8, 9, 10, 10, 11,
12, 13, 14, 15, 16, 17, 18, 19,
20, 21, 22, 23, 24, 26, 27, 28,
29, 31, 32, 33, 35, 36, 37, 39,
40, 42, 43, 45, 46, 48, 49, 51,
53, 54, 56, 57, 59, 61, 63, 64,
66, 68, 70, 71, 73, 75, 77, 79,
81, 82, 84, 86, 88, 90, 92, 94,
96, 98, 100, 102, 104, 106, 108, 110,
112, 114, 116, 117, 119, 121, 123, 125
};
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double compPitch, gyroYaw, compRoll, magYaw, gxang;

//PID Variabler.
float PIDX, PIDY, PIDZ, errorX, errorY, errorZ, prev_errorX, prev_errorY, prev_errorZ;
float pidX_p, pidY_p, pidZ_p = 0;
float pidX_i, pidY_i, pidZ_i = 0;
float pidX_d, pidY_d, pidZ_d = 0;
double kxp = 45, kyp = 35, kzp = 10; //3.55
double kxi = 0.002, kyi = 0.002, kzi = 0.00; //0.003
double kxd = 2, kyd = 1.5, kzd = 10; //2.05,d=5.32 7.5
float desired_angleX = 0, desired_angleY = 0, desired_angleZ = 0;

//Motorstyrning
int motorXDelayActual, motorYDelayActual, motorZDelayActual = 60000;
int mXPin1 = 2, mXPin2 = 3, mXPin3 = 4;
int mYPin1 = 5, mYPin2 = 6, mYPin3 = 7;
int mZPin1 = 8, mZPin2 = 9, mZPin3 = 10;
int stepX, stepY, stepZ = 1;
int EN1 = 11;
int EN2 = 12;
int currentStepXA;
int currentStepXB;
int currentStepXC;
int currentStepYA;
int currentStepYB;
int currentStepYC;
int currentStepZA;
int currentStepZB;
int currentStepZC;
long lastMotorXDelayTime, lastMotorYDelayTime, lastMotorZDelayTime = 0;
double mCounter = micros();
uint8_t EepromBuffer[48];
float axb, axs, ayb, ays, azb, azs;
float hxb, hxs, hyb, hys, hzb, hzs;

void setup() {
analogWriteResolution(8);
// serial to display data
Serial.begin(115200);
while (!Serial) {}
// start communication with IMU
status = IMU.begin();
delay(100);
accX = IMU.getAccelX_mss(), accY = IMU.getAccelY_mss(), accZ = IMU.getAccelZ_mss();
gx = IMU.getGyroX_rads(), gy = IMU.getGyroY_rads(), gz = IMU.getGyroZ_rads();
mx = IMU.getMagX_uT(), my = IMU.getMagY_uT(), mz = IMU.getMagZ_uT();
double roll = (180 / M_PI) * atan(accX / sqrt(sq(accY) + sq(accZ)));
double pitch = (180 / M_PI) * atan(accY / sqrt(sq(accX) + sq(accZ)));
double yaw = atan2(my, mx) * r2d;
// Set starting angle
kalmanX.setAngle(roll);
kalmanY.setAngle(pitch);
kalmanZ.setAngle(yaw);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while (1) {}
}
for (size_t i = 0; i < sizeof(EepromBuffer); i++) {
EepromBuffer = EEPROM.read(i);
}
memcpy(&axb, EepromBuffer + 0, 4);
memcpy(&axs, EepromBuffer + 4, 4);
memcpy(&ayb, EepromBuffer + 8, 4);
memcpy(&ays, EepromBuffer + 12, 4);
memcpy(&azb, EepromBuffer + 16, 4);
memcpy(&azs, EepromBuffer + 20, 4);
memcpy(&hxb, EepromBuffer + 24, 4);
memcpy(&hxs, EepromBuffer + 28, 4);
memcpy(&hyb, EepromBuffer + 32, 4);
memcpy(&hys, EepromBuffer + 36, 4);
memcpy(&hzb, EepromBuffer + 40, 4);
memcpy(&hzs, EepromBuffer + 44, 4);
IMU.setAccelCalX(axb, axs);
IMU.setAccelCalY(ayb, ays);
IMU.setAccelCalZ(azb, azs);
IMU.setMagCalX(hxb, hxs);
IMU.setMagCalY(hyb, hys);
IMU.setMagCalZ(hzb, hzs);
//Set the accelorometer range
IMU.setAccelRange(MPU9250::ACCEL_RANGE_2G);
// et the gyroscope range.
IMU.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
// setting DLPF bandwidth to 184 Hz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_184HZ);
// setting SRD to 0 for a 100 Hz update rate
IMU.setSrd(0);
pinMode(mXPin1, OUTPUT);
pinMode(mXPin2, OUTPUT);
pinMode(mXPin3, OUTPUT);
pinMode(EN1, OUTPUT);
pinMode(EN2, OUTPUT);
digitalWrite(EN1, HIGH);
digitalWrite(EN2, HIGH);
timer = micros();
analogWriteFrequency(mXPin1, freq);
analogWriteFrequency(mXPin2, freq);
analogWriteFrequency(mXPin3, freq);
analogWriteFrequency(mYPin1, freq);
analogWriteFrequency(mYPin2, freq);
analogWriteFrequency(mYPin3, freq);
analogWriteFrequency(mZPin1, freq);
analogWriteFrequency(mZPin2, freq);
analogWriteFrequency(mZPin3, freq);
sineArraySize = sizeof(table) / sizeof(int);
int phaseShift = sineArraySize / 3;
currentStepXA = 0;
currentStepXB = currentStepXA + phaseShift;
currentStepXC = currentStepXB + phaseShift;
currentStepYA = 0;
currentStepYB = currentStepYA + phaseShift;
currentStepYC = currentStepYB + phaseShift;
currentStepZA = 0;
currentStepZB = currentStepZA + phaseShift;
currentStepZC = currentStepZB + phaseShift;
sineArraySize--;
}

void loop() {
IMU.readSensor(); // read the sensor
double dT = (double)(micros() - timer) / 1000000;
timer = micros();
double accX = IMU.getAccelX_mss();
double accY = IMU.getAccelY_mss();
double accZ = IMU.getAccelZ_mss();
double gyroX = IMU.getGyroX_rads();
double gyroY = IMU.getGyroY_rads();
double gyroZ = IMU.getGyroZ_rads();
mx = IMU.getMagX_uT();
my = IMU.getMagY_uT();
mz = IMU.getMagZ_uT();
//Angle from accelorometer
double roll = (180 / M_PI) * atan(accX / sqrt(sq(accY) + sq(accZ)));
double pitch = (180 / M_PI) * atan(accY / sqrt(sq(accX) + sq(accZ)));
double yaw = atan2(my, mx) * r2d;
// Angle from gyro
double gyroXrate = gyroXrate + (gyroX * RAD_TO_DEG) * dT;
double gyroYrate = gyroYrate + (gyroY * RAD_TO_DEG) * dT;
double gyroZrate = gyroZrate + (gyroZ * RAD_TO_DEG) * dT;
//Angle from kalman
double kalRoll = kalmanX.getAngle(roll, gyroXrate, dT);
double kalPitch = kalmanY.getAngle(pitch, gyroYrate, dT);
double kalYaw = kalmanZ.getAngle(yaw, gyroZrate, dT);
//Angle from comp.
compRoll = (double)0.96 * (compRoll + gyroY * dT) + 0.04 * roll;
compPitch = (double)0.96 * (compPitch + gyroX * dT) + 0.04 * pitch;
gyroYaw = (double)(gyroYaw + gyroZ * dT);
runPIDX(kalRoll, desired_angleX, dT);
runPIDY(kalPitch, desired_angleY, dT);
runPIDZ(gyroYaw, desired_angleZ, dT);
//Run Motors
if ((micros() - lastMotorXDelayTime) > motorXDelayActual) {
runMotorX();
lastMotorXDelayTime = micros();
}
if ((micros() - lastMotorYDelayTime) > motorYDelayActual) {
runMotorY();
lastMotorYDelayTime = micros();
}
if ((micros() - lastMotorZDelayTime) > motorZDelayActual) {
runMotorZ();
lastMotorZDelayTime = micros();
}
}

void calAcc(void) {
Serial.println("Starting Accelerometer Calibration"), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Switch"), delay(5000), IMU.calibrateAccel();
Serial.println("Done");
}

//Run motors
void runMotorX(void) {
currentStepXA = currentStepXA + stepX;
if (currentStepXA > sineArraySize) currentStepXA = 0;
if (currentStepXA < 0) currentStepXA = sineArraySize;
currentStepXB = currentStepXB + stepX;
if (currentStepXB > sineArraySize) currentStepXB = 0;
if (currentStepXB < 0) currentStepXB = sineArraySize;
currentStepXC = currentStepXC + stepX;
if (currentStepXC > sineArraySize) currentStepXC = 0;
if (currentStepXC < 0) currentStepXC = sineArraySize;
analogWrite(mXPin1, table[currentStepXA]);
analogWrite(mXPin2, table[currentStepXB]);
analogWrite(mXPin3, table[currentStepXC]);
}

void runMotorY(void) {
currentStepYA = currentStepYA + stepY;
if (currentStepYA > sineArraySize) currentStepYA = 0;
if (currentStepYA < 0) currentStepYA = sineArraySize;
currentStepYB = currentStepYB + stepY;
if (currentStepYB > sineArraySize) currentStepYB = 0;
if (currentStepYB < 0) currentStepYB = sineArraySize;
currentStepYC = currentStepYC + stepY;
if (currentStepYC > sineArraySize) currentStepYC = 0;
if (currentStepYC < 0) currentStepYC = sineArraySize;
analogWrite(mYPin1, table[currentStepYA]);
analogWrite(mYPin2, table[currentStepYB]);
analogWrite(mYPin3, table[currentStepYC]);
}

void runMotorZ(void) {
currentStepZA = currentStepZA + stepZ;
if (currentStepZA > sineArraySize) currentStepZA = 0;
if (currentStepZA < 0) currentStepZA = sineArraySize;
currentStepZB = currentStepZB + stepZ;
if (currentStepZB > sineArraySize) currentStepZB = 0;
if (currentStepZB < 0) currentStepZB = sineArraySize;
currentStepZC = currentStepZC + stepZ;
if (currentStepZC > sineArraySize) currentStepZC = 0;
if (currentStepZC < 0) currentStepZC = sineArraySize;
analogWrite(mZPin1, table[currentStepZA]);
analogWrite(mZPin2, table[currentStepZB]);
analogWrite(mZPin3, table[currentStepZC]);
}

//Run PIDs
void runPIDX(double kalRoll, double desired_angleX, double dT) {
errorX = kalRoll - desired_angleX; pidX_p = kxp * errorX;
if (errorX < 5 && errorX > -5) {
pidX_i = pidX_i + (kxi * errorX);
} else {
pidX_i = 0;
}
pidX_d = kxd * ((errorX - prev_errorX) / dT);
PIDX = pidX_p + pidX_i + pidX_d;
prev_errorX = errorX;
if (errorX > 0) {
stepX = -1;
} else {
stepX = 1;
}
if (PIDX < 5 && PIDX > -5) {
motorXDelayActual = 100000;
} else {
motorXDelayActual = abs(motorXDelayActual - abs(PIDX));
}
if (motorXDelayActual < 1000) {
motorXDelayActual = 1000;
}
}

void runPIDY(double kalPitch, double desired_angleY, double dT) {
errorY = kalPitch - desired_angleY;
pidY_p = kyp * errorY;
if (-5 < errorY && errorY < 5) {
pidY_i = pidY_i + (kyi * errorY);
} else {
pidY_i = 0;
}
pidY_d = kyd * ((errorY - prev_errorY) / dT);
PIDY = pidY_p + pidY_i + pidY_d;
prev_errorY = errorY;
if (errorY > 0) {
stepY = -1;
} else {
stepY = 1;
}
if (PIDY < 5 && PIDY > -5) {
motorYDelayActual = 100000;
} else {
motorYDelayActual = abs(motorYDelayActual - abs(PIDY));
}
if (motorYDelayActual < 1000) {
motorYDelayActual = 1000;
}
}

void runPIDZ(double kalYaw, double desired_angleZ, double dT) {
errorZ = kalYaw - desired_angleZ;
pidZ_p = kzp * errorZ;
if (-5 < errorZ < 5) {
pidZ_i = pidZ_i + (kzi * errorZ);
} else {
pidZ_i = 0;
}
pidZ_d = kzd * ((errorZ - prev_errorZ) / dT);
PIDZ = pidZ_p + pidZ_i + pidZ_d;
prev_errorZ = errorZ;
if (errorZ > 0) {
stepZ = -1;
} else {
stepZ = 1;
}
if (PIDZ < 5 && PIDZ > -5) {
motorZDelayActual = 100000;
} else {
motorZDelayActual = abs(motorZDelayActual - abs(PIDZ));
}
if (motorZDelayActual < 1000) {
motorZDelayActual = 1000;
}
}