Writing Servos at 400kHz TWBR

After so many different types of DMP programs for MPU 9255, I made one which works just fine at a faster rate and doesn’t have any interrupt in it, however I cannot write the servos with this program, I believe its the “TWBR” which is at 400kHz but I might be wrong as I’ve tried changing TWBR to lesser values also I’ve made changes to my Servo library and changed delay from 20000us(50Hz) to 6000us(154Hz), Any help would be appreciated.

/*
* MPU6050 to Arduino Compementary Angle Finder
* 
* This sketch calculates the angle of an MPU6050 relative to the ground using a complementary filter.
* It is designed to be easy to understand and easy to use. 
* 
* The following are your pin connections for a GY-521 breakout board to Arduino UNO or NANO:
* MPU6050  UNO/NANO
* VCC      +5v
* GND      GND (duh)
* SCL      A5
* SDA      A4
* 
* Of note: this sketch uses the "I2C Protocol," which allows the Arduino to access multiple pieces of data
* on the MPU6050 with only two pins.  Way cooler than needing 7 pins for the 7 pieces of data you can get
* from your MPU6050.  The two wires are a data wire and a clock wire. The "Wire.h" library does most of this 
* communication between the Arduino and the MPU6050. It requires that the Arduino use the A5 adn A4 pins.
* Other Arduinos (Arduini?) may use different pins for the I2C protocol, so look it up on the Arduino.cc website. 
* The I2C protocol works this way like a conversation between the Arduino and the MPU.  It goes something like this:
* Arduino: "Hey 0x68." (0x68 is the MPU's address)
* MPU6050: "wat"
* Arduino: "Gimme yer x acceleration data." (except Arduino is a computer, so it calls that data "0x3B"
* MPU6050: "Ugh fine. 16980"
* Arduino: "K stop now"
* MPU6050: "K"
* 
*/
int ch1; // Here's where we'll keep our channel values
int ch2;
int ch3;
int ch4;
int ch5;
int ch6;

#include<Wire.h>
#include <Servo.h>


Servo ESC1;
Servo ESC2;
Servo ESC3;
Servo ESC4;
Servo ESC5;
Servo ESC6;

const int MPU_addr=0x68;
double AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int.  We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double compAngleX, compAngleY, compAngleZ;//These are the angles in the complementary filter
#define degconvert 57.2957786 //there are like 57 degrees in a radian.

void setSpeed(int ch5){
if(ch5<1100) ch5=1100;
if (ch5>1900) ch5=1900; 

  int angle = map(ch5, 1100, 1900, 1000, 2000); //Sets servo positions to different speeds 
ESC1.writeMicroseconds(angle);
ESC2.writeMicroseconds(angle);
ESC3.writeMicroseconds(angle);
ESC4.writeMicroseconds(angle);
ESC5.writeMicroseconds(angle);
ESC6.writeMicroseconds(angle);
Serial.println(angle);
}


void setup() {
 // Set up MPU 6050:
 Wire.begin();
 #if ARDUINO >= 157
 Wire.setClock(400000UL); // Set I2C frequency to 400kHz
 #else
   TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
 #endif

 
 Wire.beginTransmission(MPU_addr);
 Wire.write(0x6B);  // PWR_MGMT_1 register
 Wire.write(0);     // set to zero (wakes up the MPU-6050)
 Wire.endTransmission(true);
 Serial.begin(115200);
 delay(100);

 //setup starting angle
 //1) collect the data
 Wire.beginTransmission(MPU_addr);
 Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
 Wire.endTransmission(false);
 Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
 AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
 AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
 AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
 Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
 GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
 GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
 GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

 //2) calculate pitch and roll
 double roll = atan2(AcY, AcZ)*degconvert;
 double pitch = atan2(-AcX, AcZ)*degconvert;
 double yaw = atan2(AcZ,-AcY)*degconvert;

 //3) set the starting angle to this pitch and roll
 double gyroXangle = roll;
 double gyroYangle = pitch;
 double gyroZangle = yaw;
 
 double compAngleX = roll;
 double compAngleY = pitch;
 double compAngleZ = yaw;

 //start a timer
 timer = micros();
}

void loop() {
//Now begins the main loop. 
 //Collect raw data from the sensor.
 Wire.beginTransmission(MPU_addr);
 Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
 Wire.endTransmission(false);
 Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
 AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
 AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
 AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
 Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
 GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
 GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
 GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
 
 double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
 timer = micros(); //start the timer again so that we can calculate the next dt.

 //the next two lines calculate the orientation of the accelerometer relative to the earth and convert the output of atan2 from radians to degrees
 //We will use this data to correct any cumulative errors in the orientation that the gyroscope develops.
 double roll = atan2(AcY, AcZ)*degconvert;
 double pitch = atan2(-AcX, AcZ)*degconvert;
 double yaw = atan2(AcZ,-AcY)*degconvert;

 //The gyroscope outputs angular velocities.  To convert these velocities from the raw data to deg/second, divide by 131.  
 //Notice, we're dividing by a double "131.0" instead of the int 131.
 double gyroXrate = GyX/131.0;
 double gyroYrate = GyY/131.0;
 double gyroZrate = GyZ/131.0;
 
 //THE COMPLEMENTARY FILTER
 //This filter calculates the angle based MOSTLY on integrating the angular velocity to an angular displacement.
 //dt, recall, is the time between gathering data from the MPU6050.  We'll pretend that the 
 //angular velocity has remained constant over the time dt, and multiply angular velocity by 
 //time to get displacement.
 //The filter then adds a small correcting factor from the accelerometer ("roll" or "pitch"), so the gyroscope knows which way is down. 
 compAngleX = 0.99 * (compAngleX + gyroXrate * dt) + 0.01 * roll; // Calculate the angle using a Complimentary filter
 compAngleY = 0.99 * (compAngleY + gyroYrate * dt) + 0.01 * pitch; 
 compAngleZ = 0.99 * (compAngleZ + gyroZrate * dt) + 0.01 * yaw;
 //W00T print dat shit out, yo!
 Serial.print(compAngleX);Serial.print("\t");
 Serial.print(compAngleY);Serial.print("\t");
 Serial.print(compAngleZ);Serial.print("\t");

  // ch1 = pulseIn(44, HIGH);
   ch2 = pulseIn(46, HIGH);
   ch3 = pulseIn(48, HIGH);
   ch4 = pulseIn(50, HIGH);
   ch5 = pulseIn(52, HIGH);
  // ch6 = pulseIn(42, HIGH);
 

   Serial.print(" Channel :");
 Serial.print(ch3); //Stepper

 Serial.print(" Channel 2:");
 Serial.print(ch4); //Roll
 
  Serial.print(" Channel 3:");
 Serial.print(ch5); //Throttle

 Serial.print(" Channel 4:");
 Serial.println(ch2); //Yaw

setSpeed(ch5);

}

Guys I am stuck at this point, please help me figure out whats wrong with the code.

Hi,

Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html

Have you written code just for the ESCs to see if your moded library works?
If the ESC is controlled by servo signal, then it is expecting a standard servo signal.

Thanks.. Tom.. :slight_smile:

Thanks Tom, I've made changes to my post as suggested by the link you've provided :slight_smile: .

Yes the servos are working if I do not include DMP for IMU in my code, I also have tried using esc.write (ranges from 0-180) instead of esc.writemicroseconds (ranges from 1000-2000), none of them work when I include DMP and vice versa.

Have you encountered any such problem?

jashraf:
also I've made changes to my Servo library and changed delay from 20000us(50Hz) to 6000us(154Hz),

What servo library?
What do you mean by "delay"?

You don't seem to have Servo.attach() anywhere

...R

Robin2:
You don't seem to have Servo.attach() anywhere

...R

Yes Just attached ESC's in void setup and its working, dah

Thanks everyone for the help