Hi i'm a student making a diy drone.
First, im not good at English. so if you dont understand what i mean please let me know.
i use [servo] function to operate motors.
my code's response time is 0.02s, and i wnat to reduce it to 0.01s at least.
as i heard, i can chane the response time through changing the servo head file. (as i remember, it was refrash time or something)
so i change it, but it doesn't work.
what can i do for reducing response time? or should i use different function?
please help me... T_T
and this is my code, sorry about messy code and i delet some code lines unneccesary for this problems and length limits
#include <Wire.h>
#include <Servo.h>
#define MAX_SIGNAL 2000
#define MIN_SIGNAL 1000
double channel[4];
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; //MPU output
float motorA_speed, motorB_speed, motorC_speed, motorD_speed;
float throttle =1350;
Servo motorA;
Servo motorB;
Servo motorC;
Servo motorD;
void setup() {
initMPU6050();
Serial.begin(115200);
calibAccelGyro();
initDT();
pinMode(2, INPUT); //channel1
pinMode(4, INPUT); //channel2
pinMode(5, INPUT); //channel3
pinMode(6, INPUT); //channel4
motorA.attach(3,1000,2000);
motorB.attach(9,1000,2000);
motorC.attach(10,1000,2000);
motorD.attach(11,1000,2000);
Serial.setTimeout(50);
motorA.writeMicroseconds(1000);
motorB.writeMicroseconds(1000);
motorC.writeMicroseconds(1000);
motorD.writeMicroseconds(1000);
delay(15000);
}
void loop() {
float set_roll;
float set_pitch;
float set_yaw;
// channel[0] = pulseIn(2, HIGH);
channel[1] = pulseIn(4, HIGH);
// channel[2] = pulseIn(5, HIGH); //thrust
channel[3] = pulseIn(6, HIGH);
readAccelGyro();
//SendDataToProcessing();
calcDT();
calcAccelYPR();
calcGyroYPR();
calcFilteredYPR();
calcYPRtoStdPID();
// checkPIDChanged();
calcMotorSpeed();
sendtomotor();
}
void initMPU6050(){
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
}
void readAccelGyro(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
AcX = Wire.read() << 8 | Wire.read();
AcY = Wire.read() << 8 | Wire.read();
AcZ = Wire.read() << 8 | Wire.read();
Tmp = Wire.read() << 8 | Wire.read();
GyX = Wire.read() << 8 | Wire.read();
GyY = Wire.read() << 8 | Wire.read();
GyZ = Wire.read() << 8 | Wire.read();
}
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;
extern float roll_output, pitch_output, yaw_output;
extern float motorA_speed, motorB_speed, motorC_speed, motorD_speed;
void calibAccelGyro(){
float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
float sumGyX = 0, sumGyY = 0, sumGyZ = 0;
readAccelGyro();
for(int i=0; i<10; i++){
readAccelGyro();
sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
delay(100);
}
baseAcX = sumAcX / 10; baseAcY = sumAcY / 10; baseAcZ = sumAcZ / 10;
baseGyX = sumGyX / 10; baseGyY = sumGyY / 10; baseGyZ = sumGyZ / 10;
}
unsigned long t_now;
unsigned long t_prev;
void initDT(){
t_prev = micros();
}
void calcDT(){
t_now = micros();
dt = (t_now - t_prev) / 1000000.0;
t_prev = t_now;
}
void calcAccelYPR(){
float accel_x, accel_y, accel_z;
float accel_xz, accel_yz;
const float RADIANS_TO_DEGREES = 180/3.14159;
accel_x = AcX - baseAcX;
accel_y = AcY - baseAcY;
accel_z = AcZ + (16384 - baseAcZ);
accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_angle_x = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;
accel_angle_z = 0;
}
float gyro_x, gyro_y, gyro_z;
void calcGyroYPR(){
const float GYROXYZ_TO_DEGREES_PER_SED = 131;
gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SED;
gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SED;
gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SED;
}
void calcFilteredYPR(){
const float ALPHA = 0.96;
float tmp_angle_x, tmp_angle_y, tmp_angle_z;
tmp_angle_x = filtered_angle_x + gyro_x * dt;
tmp_angle_y = filtered_angle_y + gyro_y * dt;
tmp_angle_z = filtered_angle_z + gyro_z * dt;
filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
filtered_angle_z = tmp_angle_z;
}
void stdPID(float& setpoint,
float& input,
float& prev_input,
float& kp, float& ki, float& kd,
float& iterm,
float& output){
float error;
float dInput;
float pterm, dterm;
error = setpoint - input;
dInput = input - prev_input;
prev_input = input;
pterm - kp + error;
iterm += ki + error * dt;
dterm = -kd * (dInput / dt);
output = pterm + iterm + dterm;
}
float set_roll;
float set_pitch;
float set_yaw;
int pp = 5;
int ii = 0;
int dd = 0;
float roll_target_angle = 0.0;
float roll_prev_angle = 0.0;
float roll_kp = pp;
float roll_ki = ii;
float roll_kd = dd;
float roll_iterm;
float roll_output;
float pitch_target_angle = 0.0;
float pitch_prev_angle = 0.0;
float pitch_kp = pp;
float pitch_ki = ii;
float pitch_kd = dd;
float pitch_iterm;
float pitch_output;
float yaw_target_angle = 0.0;
float yaw_prev_angle = 0.0;
float yaw_kp = 1;
float yaw_ki = 0;
float yaw_kd = 0;
float yaw_iterm;
float yaw_output;
void calcYPRtoStdPID(){
stdPID(set_roll, filtered_angle_y, roll_prev_angle, roll_kp, roll_ki, roll_kd, roll_iterm, roll_output);
stdPID(set_pitch, filtered_angle_x, pitch_prev_angle, pitch_kp, pitch_ki, pitch_kd, pitch_iterm, pitch_output);
stdPID(set_yaw, filtered_angle_z, yaw_prev_angle, yaw_kp, yaw_ki, yaw_kd, yaw_iterm, yaw_output);
}
void calcMotorSpeed() {
motorA_speed =
throttle + roll_output + pitch_output; //+ yaw_output
motorB_speed =
throttle - roll_output + pitch_output;//- yaw_output
motorC_speed =
throttle - roll_output - pitch_output;//+ yaw_output
motorD_speed =
throttle + roll_output - pitch_output;//- yaw_output
if(motorA_speed < 1200) motorA_speed = 1200;
if(motorA_speed > 1700) motorA_speed = 1700;
if(motorB_speed < 1200) motorB_speed = 1200;
if(motorB_speed > 1700) motorB_speed = 1700;
if(motorC_speed < 1200) motorC_speed = 1200;
if(motorC_speed > 1700) motorC_speed = 1700;
if(motorD_speed < 1200) motorD_speed = 1200;
if(motorD_speed > 1700) motorD_speed = 1700;
}
void sendtomotor() {
motorA.writeMicroseconds(motorA_speed);
motorB.writeMicroseconds(motorB_speed);
motorC.writeMicroseconds(motorC_speed);
motorD.writeMicroseconds(motorD_speed);
}