bms001:
So you don't have a front/back/left/right motor, you have a front left, front right, back left and back right motor.
Which means you'll need to apply PID roll correction to all motors, and PID pitch correction to all motors.
E.g. motor X = base throttle +/- PID roll +/- PID pitch
Thanks for the advice. I have changed a few of the code and now I have tried to compile it together with another code that allow me to hover the quad copter at 30 cm. However, once i tried to compile it, the code can't seem to run. Is there any problem with the code? Thanks
#include <Servo.h>
#include <Wire.h>
#include <math.h>
#define PI 3.14159265
Servo esc1; //Create servo object for the 4 esc
Servo esc2;
Servo esc3;
Servo esc4;
float pid_p_gain_roll = 1.3;
float pid_i_gain_roll = 0.04;
float pid_d_gain_roll = 18.0;
float pid_p_gain_pitch = pid_p_gain_roll;
float pid_i_gain_pitch = pid_i_gain_roll;
float pid_d_gain_pitch = pid_d_gain_roll;
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
double pitchAngle, rollAngle;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;
float pid_error_temp;
float pid_i_mem_roll, pid_roll_setpoint, pid_output_pitch, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_roll, pid_last_pitch_d_error;
float front_right, front_left, rear_right, rear_left;
long duration;
int distance;
int ch1 = 3;
int ch2 = 4;
int ch3 = 5;
int ch4 = 6;
int ch5 = 7;
int trigPin = A2;
int echoPin = A3;
int roll_channel = 8;
int pitch_channel = 9;
int throttle_channel = 10;
int yaw_channel = 11;
int alt_hold_switch = 12;
void setup()
{
Serial.begin(9600);
pinMode(ch1, INPUT); //Attach pin to the recevier input
pinMode(ch2, INPUT);
pinMode(ch3, INPUT);
pinMode(ch4, INPUT);
pinMode(ch5, INPUT);
pinMode(echoPin, INPUT);
pinMode(trigPin, OUTPUT);
pinMode(roll_channel,OUTPUT); //Attach pin to the motor controlling the roll
esc1.attach(8); //Attach pin 8 to esc 1 to control the motor
pinMode(pitch_channel, OUTPUT);
esc2.attach(9);
pinMode(throttle_channel, OUTPUT);
esc3.attach(10);
pinMode(yaw_channel, OUTPUT);
esc4.attach(11);
pinMode(alt_hold_switch, OUTPUT);
setupMPU();
}
void setupMPU()
{
Wire.beginTransmission(0b1101000);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
}
void recordAccelRegisters()
{
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000,6);
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read();
accelY = Wire.read()<<8|Wire.read();
accelZ = Wire.read()<<8|Wire.read();
processAccelData();
}
void processAccelData()
{
gForceX = accelX/16384.0;
gForceY = accelY/16384.0;
gForceZ = accelZ/16384.0;
pitchAngle = (atan2(gForceY, sqrt(sq(gForceX) + sqrt(gForceZ))))* (360/(2*PI));
rollAngle = (atan2(gForceX, sqrt (sq(gForceY) + sqrt(gForceZ)))) * (360/(2*PI));
}
void recordGyroRegisters()
{
Wire.beginTransmission(0b1101000);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6);
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read();
gyroY = Wire.read()<<8|Wire.read();
gyroZ = Wire.read()<<8|Wire.read();
processGyroData();
}
void processGyroData()
{
rotX = gyroZ/131.0;
rotY = gyroY/131.0;
rotZ = gyroZ/131.0;
}
void calculate_pid()
{
pid_error_temp = rollAngle - pid_roll_setpoint;
pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
pid_last_roll_d_error = pid_error_temp;
pid_error_temp = pitchAngle - pid_roll_setpoint;
pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
pid_output_pitch = pid_p_gain_roll * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
pid_last_pitch_d_error = pid_error_temp;
}
void loop()
{
recordAccelRegisters();
recordGyroRegisters();
delay(50);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = duration*0.034/2;
distance = constrain(distance, 0, 30);
roll_channel = pulseIn(ch1, HIGH, 20000); //Program to start the motor and to control it
esc1.writeMicroseconds(roll_channel);
delay(20);
pitch_channel = pulseIn(ch2, HIGH, 20000);
esc2.writeMicroseconds(pitch_channel);
delay(20);
throttle_channel = pulseIn(ch3, HIGH, 20000);
esc3.writeMicroseconds(throttle_channel);
delay(20);
yaw_channel = pulseIn(ch4, HIGH, 20000);
esc4.writeMicroseconds(yaw_channel);
delay(20);
alt_hold_switch = pulseIn(ch5, HIGH, 20000);
delay(20);
if (distance == 30 && alt_hold_switch <= 1300)
{
esc1.writeMicroseconds(roll_channel);
esc2.writeMicroseconds(pitch_channel);
esc3.writeMicroseconds(2000);
esc4.writeMicroseconds(yaw_channel);
}
else if (distance <= 30 && alt_hold_switch >= 1300)
{
esc1.writeMicroseconds(roll_channel);
esc2.writeMicroseconds(pitch_channel);
esc3.writeMicroseconds(throttle_channel);
esc4.writeMicroseconds(yaw_channel);
}
pid_roll_setpoint = 0;
pid_pitch_setpoint = 0;
roll_channel = throttle_channel - pid_output_pitch + pid_output_roll;
pitch_channel = throttle_channel + pid_output_pitch + pid_output_roll;
throttle_channel = throttle_channel + pid_output_pitch - pid_output_roll;
yaw_channel = throttle_channel - pid_output_pitch - pid_output_roll;
Serial.print("Channel 1 (Roll): ");
Serial.print(roll_channel);
Serial.print("\t\tChannel 2 (Pitch): ");
Serial.print(pitch_channel);
Serial.print("\t\tChannel 3 (Throttle): ");
Serial.print(throttle_channel);
Serial.print("\t\tChannel 4 (Yaw): ");
Serial.print(yaw_channel);
Serial.print("\t\tChannel 5 (Alt): ");
Serial.print(alt_hold_switch);
Serial.print("\t\tDistance: ");
Serial.print(distance);
Serial.print("\t\tPitch Angle (deg) = ");
Serial.print(pitchAngle);
Serial.print("\t\tRoll Angle (deg) = ");
Serial.println(rollAngle);
}