Hi!
I am making a drone with 4 ESCs, I am wondering how I can control my ESCs at 400Hz instead of 50Hz (I believe thats default for the servo library). This is my code, I set the wire clock to 400000 (400Hz) but I dont think the Servo library does that to:
#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
Servo motor_LF;
Servo motor_RF;
Servo motor_LB;
Servo motor_RB;
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13
bool blinkState = false;
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
float input_throttle;
/* Valut */
float elapsedTime, time, timePrev;
long loop_timer;
/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 7;
double roll_ki = 0.006;
double roll_kd = 1.2;
float roll_desired_angle = 0;
/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = 72;
double pitch_ki = 0.006;
double pitch_kd = 1.22;
float pitch_desired_angle = 0;
/* Accelerometer X PID */
float accX_PID, accX_error, accX_previous_error;
float accX_pid_p = 0;
float accX_pid_i = 0;
float accX_pid_d = 0;
/* Accelerometer X PID Constants */
double accX_kp = 72;
double accX_ki = 0.006;
double accX_kd = 1.22;
/* Accelerometer Y PID */
float accY_PID, accY_error, accY_previous_error;
float accY_pid_p = 0;
float accY_pid_i = 0;
float accY_pid_d = 0;
/* Accelerometer Y PID Constants */
double accY_kp = 72;
double accY_ki = 0.006;
double accY_kd = 1.22;
void setup() {
motor_LF.attach(4);
motor_RF.attach(5);
motor_LB.attach(6);
motor_RB.attach(7);
motor_LF.writeMicroseconds(1000);
motor_RF.writeMicroseconds(1000);
motor_LB.writeMicroseconds(1000);
motor_RB.writeMicroseconds(1000);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial);
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
pinMode(LED_PIN, OUTPUT);
}
void loop() {
// if setup fail dont run the program
if (!dmpReady) return;
/* Start Timer */
timePrev = time;
time = millis();
elapsedTime = (time - timePrev) / 1000;
/* Get throttle value from reciver */
int read_input_throttle = analogRead(A0);
input_throttle = map(read_input_throttle, 0, 1023, 1000, 1500);
/* PID */
roll_error = roll_desired_angle - (ypr[2] * 180 / M_PI);
pitch_error = pitch_desired_angle - (ypr[1] * 180 / M_PI);
/* Acceleromteter PID */
accX_error = 0 - aaReal.x;
accY_error = 0 - aaReal.y;
/* Prospect */
roll_pid_p = roll_kp * roll_error;
pitch_pid_p = pitch_kp * pitch_error;
/* Acceleromteter Prospect */
accX_pid_p = accX_kp * accX_error;
accY_pid_p = accY_kp * accY_error;
/* Integral */
if (-3 < roll_error < 3)
{
roll_pid_i = roll_pid_i + (roll_ki * roll_error);
}
if (-3 < pitch_error < 3)
{
pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
}
/* Acceleromteter Integral */
if (-3 < accX_error < 3)
{
accX_pid_i = accX_pid_i + (accX_ki * accX_error);
}
if (-3 < accY_error < 3)
{
accY_pid_i = accY_pid_i + (accY_ki * accY_error);
}
/* Derivate */
roll_pid_d = roll_kd * ((roll_error - roll_previous_error) / elapsedTime);
pitch_pid_d = pitch_kd * ((pitch_error - pitch_previous_error) / elapsedTime);
/* Acceleromteter Derivate */
accX_pid_d = accX_kd * ((accX_error - accX_previous_error) / elapsedTime);
accY_pid_d = accY_kd * ((accY_error - accY_previous_error) / elapsedTime);
/* PID summery */
roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
/* Acceleromteter PID summery */
accX_PID = accX_pid_p + accX_pid_i + accX_pid_d;
accY_PID = accY_pid_p + accY_pid_i + accY_pid_d;
/* Regulate PID output for ESCs */
if (roll_PID < -400) {
roll_PID = -400;
}
if (roll_PID > 400) {
roll_PID = 400;
}
if (pitch_PID < -400) {
pitch_PID = -400;
}
if (pitch_PID > 400) {
pitch_PID = 400;
}
/* Regulate Acceleromteter PID */
if (accX_PID > 20) {
accX_PID = 20;
}
if (accY_PID > 20) {
accY_PID = 20;
}
if (accY_PID < -20) {
accY_PID = -20;
}
/* Set desired angle for Accelerometer stabilization */
if (roll_desired_angle == 0) {
roll_desired_angle = accX_PID;
}
if (pitch_desired_angle == 0) {
pitch_desired_angle = accY_PID;
}
/* Set the throttle PID for each motor */
pid_throttle_R_F = input_throttle + roll_PID - pitch_PID;
pid_throttle_L_F = input_throttle - roll_PID - pitch_PID;
pid_throttle_R_B = input_throttle + roll_PID + pitch_PID;
pid_throttle_L_B = input_throttle - roll_PID + pitch_PID;
/* Regulate throttle for ESCs */
//Right front
if (pid_throttle_R_F < 1100)
{
pid_throttle_R_F = 1100;
}
if (pid_throttle_R_F > 2000)
{
pid_throttle_R_F = 2000;
}
//Left front
if (pid_throttle_L_F < 1100)
{
pid_throttle_L_F = 1100;
}
if (pid_throttle_L_F > 2000)
{
pid_throttle_L_F = 2000;
}
//Right back
if (pid_throttle_R_B < 1100)
{
pid_throttle_R_B = 1100;
}
if (pid_throttle_R_B > 2000)
{
pid_throttle_R_B = 2000;
}
//Left back
if (pid_throttle_L_B < 1100)
{
pid_throttle_L_B = 1100;
}
if (pid_throttle_L_B > 2000)
{
pid_throttle_L_B = 2000;
}
/* Save Previous Error */
roll_previous_error = roll_error;
pitch_previous_error = pitch_error;
/* Save Acceleromteter Previous Error */
accX_previous_error = accX_error;
accY_previous_error = accY_error;
Serial.println(input_throttle);
if (input_throttle < 1100) {
pid_throttle_L_F = input_throttle;
pid_throttle_L_B = input_throttle;
pid_throttle_R_F = input_throttle;
pid_throttle_R_B = input_throttle;
roll_error = 0;
pitch_error = 0;
roll_previous_error = 0;
pitch_previous_error = 0;
}
motor_LF.writeMicroseconds(pid_throttle_L_F);
motor_RF.writeMicroseconds(pid_throttle_R_F);
motor_LB.writeMicroseconds(pid_throttle_L_B);
motor_RB.writeMicroseconds(pid_throttle_R_B);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180 / M_PI);
}
Thanks beforehand!
Best regards Max