My First Quadrotor: Control Overview and Filtering Sensor Feedback

So I'm about to sit down and write the flight controller for my Quadrotor (My First Quadrotor? MFQ? MFQuad? MFCopter?), but rather than simply document that "I wrote the code and it worked", like I have been doing, I am going to try and be more explicit with my thought processes, in order to become more aware of my coding style, as well as to receive criticism in my style. 

So, a quad flight controller. What's that?

In its most basic form, a Quadrotor Flight Controller (QFC for now, because Three Letter Acronyms (TLAs) are Really Freaking Cool (RFQ)) does the following: 

• Reads data from the Gyroscope and Accelerometer
• Filters it into a usable form
• Generate commands the four motors based on the sensor values and user input in order to keep the quadrotor in a certain state, which is usually balancing pitch and roll to keep a stable hovering platform. 
• Drives motors with these commands

Additionally, a QFC should be able to wirelessly receive commands from an external source in order to be able to move around. This can be as simple as 4 servo commands from a handheld receiver, or as complicated as X,Y,Z,Pitch,Roll,Yaw, and their derivatives. To keep the amount of data being transferred wirelessly down, however, I will keep this down to pitch, roll, yaw, and altitude.

Finally, for robustness, the QFC should be able to keep the quadrotor stable in the case of a broken connection mid-flight. Should bytes be dropped or a sudden change in communication occur, an onboard "Lost Connection" timeout will trigger, and the quadrotor will orient itself to a more stable orientation (The Pitch and Roll commands will reset to 0, or a landing sequence will initiate). 

Okay.

*cracks knuckles*
Let's do this. 

So first of all our QFC needs a way to read sensor data. The gorgeous MultiWii board Shane was kind enough to give me is essentially an Arduino Nano 328 with a built-in 3-axis accelerometer, 3-axis gyroscope, magnetometer, barometer, and a bunch of servo-style breakout pins. The sensors all communicate with the Atmega 328 using the i2c protocol. While I have dealt with i2c communication at my job, every device is different, and requires slightly different requests/commands. 

Shane gave me some example MultiWii code to be able to start talking with the sensors over i2c.

 #include <Wire.h>  
   
 // I2C addresses for each sensor:  
 #define GYR_ADR 104  
 #define ACC_ADR 64  
 #define BMP085_ADDRESS 119  
 #define MAG_ADR 30  
   
 // sensor scaling  
 #define GYR_K 0.06956           // [(deg/s)/LSB]  
 #define ACC_K 0.01399           // [deg/LSB]  
 #define VEL_K 0.002395          // [(ft/s^2)/LSB]  
 #define ACC_Z_ZERO 4096          // [LSB]  
 #define RADTODEG 57.2957795  
   
 float gyro_pitch, gyro_roll, gyro_yaw;       // [deg/s]  
 float accel_pitch, accel_roll;           // [deg]  
 float accel_x;                   // [ft/s^2]  
 float accel_y;                   // [ft/s^2]  
 float accel_z;                   // [ft/s^2]  
 float rate_z;                   // [ft/s]  
   
 void setup(){  
  Wire.begin();                  // init I2C  
  TWBR = ((F_CPU / 400000) - 16) / 2;       // fast I2C   
  initSensors();  
 }  
   
 void initSensors(){  
  // initialize the ITG-3205 gyro  
  // range: 2000deg/s, bandwidth: 188Hz  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x16);            
  Wire.write(0x19);            
  Wire.endTransmission();  
    
  // initialize the BMA180 accelerometer  
  // range: 2g, bandwidth: 150Hz (all defaults)   
 }  
   
 void readSensors(){  
  unsigned char i;  
  unsigned char raw_data[6];  
  signed int raw_value;  
    
  // read the ITG-3205 gyro  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x1D);                    
  Wire.endTransmission();  
    
  i = 0;  
  Wire.requestFrom(GYR_ADR, 6);  
  while(Wire.available())  
  {  
   raw_data[i++] = Wire.read();  
  }  
    
  // convert raw gyro data to [deg/s]  
  raw_value = (raw_data[0] << 8) + raw_data[1];  
  gyro_pitch = -1.0*(float) raw_value * GYR_K;  
  raw_value = (raw_data[2] << 8) + raw_data[3];  
  gyro_roll = (float) raw_value * GYR_K;  
  raw_value = (raw_data[4] << 8) + raw_data[5];  
  gyro_yaw = (float) raw_value * GYR_K;  
    
  // remind the gyro to stay in 2000deg/s mode???  
  // range: 2000deg/s, bandwidth: 188Hz  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x16);            
  Wire.write(0x19);            
  Wire.endTransmission();  
    
  // read the BMA180 accelerometer  
  Wire.beginTransmission(ACC_ADR);  
  Wire.write(0x02);                    
  Wire.endTransmission();  
    
  i = 0;  
  Wire.requestFrom(ACC_ADR, 6);  
  while(Wire.available())  
  {  
   raw_data[i++] = Wire.read();  
  }  
    
  // convert raw accel data to [deg] for pitch/roll  
  raw_value = ((raw_data[1] << 8) + raw_data[0]) >> 2;  
  accel_x = (float)(raw_value) * VEL_K;  
  accel_roll = -(float) raw_value * ACC_K;//(atan2(accel_z,accel_x)*RADTODEG-90.0);  
    
  raw_value = ((raw_data[3] << 8) + raw_data[2]) >> 2;  
  accel_y = (float)(raw_value) * VEL_K;  
  accel_pitch =-1.0*(float) raw_value * ACC_K; //(atan2(accel_z,accel_y)*RADTODEG-90.0);  
    
  // convert raw accel data to [ft/s^2] for z  
  raw_value = ((raw_data[5] << 8) + raw_data[4]) >> 2;  
  accel_z = (float)(raw_value) * VEL_K;  
 }  
   

The above code contains the proper library inclusion (Wire.h) for i2c interfacing, the proper i2c addresses and conversion constants for the various MultiWii board sensors, a function to initialize each of these sensors, the i2c initialization code needed within the Arduino setup() function, and a function to read the raw sensor values from the Accelerometer and Gyroscope and convert them to usable forms (angle, acceleration, angle rate, etc). 

Note: I eventually want to be able to read the Barometer data, so I found some excellent explanations and example code here (http://www.sparkfun.com/tutorials/253) as well as a more official source of barometric altimetry theory (which i used to great affect in 2.671). For now, we'll ignore the barometer data. 

In addition to reading sensor data, we are going to need to filter it. The Second Order Complementary Filter that i'm used to requires a PI controller in order for it to work.  Additionally, a PID controller will necessary for quadrotor self-balancing, so I opted to write a generic C++ PID class. For more info on PID Feedback Control, just Google it. Wikipedia offers an excellent writeup, as well as an excellent textbook document hosted by Caltech. 

Here's the generic PID Controller class I've written:

 class PIDController{  
   double kP;  
   double kI;  
   double kD;  
   double xError;  
   double xDesired;  
   double xErrorPrev;  
   double xErrorIntegral;  
   boolean feed;  
   public:  
   PIDController(double myKP, double myKI, double myKD, boolean isFeed){  
     kP = myKP;  
     kI = myKI;  
     kD = myKD;  
     xError = 0;  
     xDesired = 0;  
     xErrorPrev = 0;  
     xErrorIntegral = 0;  
     feed = isFeed;  
     }  
   
     void setDesired(double myXDesired){  
     xDesired = myXDesired;  
     }  
   
   double update(double x,double dx){  
     double command = 0.0;  
     xError = (xDesired - x);  
     xErrorIntegral+=xError;  
     if(feed)  
      command = kP*xError + kI*xErrorIntegral +kD*dx;  
     else  
      command = kP*xError + kI*xErrorIntegral +kD*(xError-xErrorPrev);  
     xErrorPrev = xError;  
     return command;  
   }  
 };  

When the class is instantiated, it requires the three PID constants and an option of whether the rate (derivative) is fed to the controller at each update, or if it should estimate it from past timesteps. This last option is useful if you have separate feedback for rate and position (like on this quad, which has both a filtered andle and a dedicated gyro for feedback). Before the controller is used, however, the set point for the control variable must be set via the setDesired() function. The update() function takes the current sensor feedback, as well as the (optional) derivative feedback, and spits out an actuator command. 

This controller can command the motors of my quadrotor to counteract its roll or pitch angle deviating from the angle I want it at (which is usually 0), maintain a constant altitude with barometer feedback, or act as the backbone for the Second Order Complementary Filter in order to get proper pitch and roll angle feedback by combining both the Accelerometer and Gyroscope data, as I did in 2.12. 

Here is the entire filter implemented in Arduino:

 #define FREQ 200.0  
   
 double dt = 1.0/FREQ;  
 long loopStart;  
 double pitch = 0;  
 double roll = 0;  
 double initialPitch = 0;  
 double initialRoll = 0;  
 float wn = .045;  
 float filterKP = wn*2.0;  
 float filterKI = wn*wn*dt;  
   
 PIDController rollFilter(filterKP,filterKI,0, 0);  
 PIDController pitchFilter(filterKP,filterKI,0, 0);  
   
 void IMUFilterUpdate(){  
  pitchFilter.setDesired(accel_pitch);  
  pitch = pitch + (gyro_pitch + pitchFilter.update(pitch,0))*dt;  
    
  rollFilter.setDesired(accel_roll);  
  roll = roll + (gyro_roll + rollFilter.update(roll,0))*dt;  
 }   

Sweet! Now in my main loop I can call IMUFilterUpdate() immediately after updating the raw sensor values to get actual pitch and roll, provided I ensure the main loop runs at a frequency of 200 Hz. This can be done with a simple if statement that checks if the time elapsed since the previous run of the loop is greater than or equal to the period (dt = 1/Frequency = 1/(200 Hz) = 5ms). 

An excellent forum post about the Second Order Complementary Filter (SOCF?) can be found here (post #1286), and a writeup pdf by the author here. Also, is Shane Colton's incredibly useful whitepaper on the First-Order Complementary Filter (Not quite as robust, but super intuitive, easy to understand, and easy to implement). 

The Second Order Complementary Filter (block diagram pictured above) works by first integrating the gyroscope data over time ( or multiplying it by 1/s in Laplace space), resulting in an estimated angle. To do that, the the old angle estimate is added to the new change in angle: 

angleNew = angleOld + (dThetadt * dt);

where dThetadt is the change in angle read directly from the gyroscope, in [degrees/s], and dt = 0.005 seconds. 

However, the gyroscope drifts! Over a short amount of time, the angle estimate will have accumulated error, leading to an incorrect angle estimate (and causing the quadrotor to possibly become unstable). For short-term situations, especially when the helicopter is moving, the gyro is pretty accurate. 

So this is corrected by taking advantage of the fact that the direction of gravitational acceleration can be found from the  3-axis accelerometer, regardless of the orientation, by taking the arctangent of the horizontal and vertical values of the accelerometer (Or linearizing the angle only using the horizontal component to save computation time, as I did above. The full atan version is commented out). This value is only useful if the quadrotor is not moving, however, and can be waaaay off in situations where the vehicle is accelerating on its own accord. 

So, we want the integrated gyro data when the vehicle is in motion, and the accelerometer angle estimate when the vehicle is static. 

We can use both simultaneously, by providing the angleFilter PI controller the estimated angle as feedback, and the current Accelerometer estimated angle as the set point. The difference will generate a rate command to overcome the gyroscope drift, resulting in a more accurate angle that is robust against both gyro drift AND vehicle accelerations. 

The values for the proportional and integrator constants, Kp and Ki respectively, can be determined if we model the PI controller as a second order system. The transfer function for the PI controller turns out in a fashion such that 

Kp = 2 * wn * zeta;

and

Ki = wn*wn;

where wn is the natural frequency of the system and zeta is the damping ratio. Because we are designing this system (It isn't a physical system with its own uneditable system dynamics), we can set the damping ratio to be a perfect 1 (effectively eliminating the need for  a zeta term) and the natural frequency wn to be the frequency at which we trust the gyroscope (or, alternatively, how often we correct the gyroscope with the Accelerometer estimate). I chose a natural frequency of 0.55Hz (period 1.81 seconds), though this may depend on your sensors, loop frequency, etc. I multiply the integral term by dt in order to take into account that our system is in discrete (digital) time. 

UPDATE on wn: After testing some with this configuration and seeing delay between copter motion and , I've decided to trust the gyroscope even more and make wn 0.045, about an order of magnitude less. The resulting behavior leads to much faster response to copter motion. 

A good test to see if your natural frequency is correct is to run the code and wait a while and see if the angle estimate drifts. Then, shake (accelerate) your vehicle to try and induce spikes in the angle estimate due to the accelerometer picking up the unwanted noise. If the angle remains stable for both tests, it works! :D 

angleFilter.setDesired( AccelerometerEstimate );
angleNew = angleOld + (dThetadt + angleFilter.update(angleOld,0)) * dt;

The code all together, which prints out the filtered roll and pitch values of a multiWii328 board:

 /*  
 Quadrotor Sensor and Filter Firmware  
 Daniel J. Gonzalez  
 dgonz@mit.edu  
 January 2013  
  */  
   
 #include <Wire.h>  
 #include <math.h>  
   
 // I2C addresses for each sensor:  
 #define GYR_ADR 104  
 #define ACC_ADR 64  
 #define BMP085_ADDRESS 119  
 #define MAG_ADR 30  
   
 // sensor scaling  
 #define GYR_K 0.06956           // [(deg/s)/LSB]  
 #define ACC_K 0.01399           // [deg/LSB]  
 #define VEL_K 0.002395          // [(ft/s^2)/LSB]  
 #define ACC_Z_ZERO 4096          // [LSB]  
 #define RADTODEG 57.2957795  
   
 float gyro_pitch, gyro_roll, gyro_yaw;       // [deg/s]  
 float accel_pitch, accel_roll;           // [deg]  
 float accel_x;                   // [ft/s^2]  
 float accel_y;                   // [ft/s^2]  
 float accel_z;                   // [ft/s^2]  
 float rate_z;                   // [ft/s]  
   
   
 #define FREQ 200.0  
   
 double dt = 1.0/FREQ;  
 long loopStart;  
 double pitch = 0;  
 double roll = 0;  
 double initialPitch = 0;  
 double initialRoll = 0;  
 float wn = .55;  
 float filterKP = wn*2.0;  
 float filterKI = wn*wn*dt;  
   
 class PIDController{  
   double kP;  
   double kI;  
   double kD;  
   double xError;  
   double xDesired;  
   double xErrorPrev;  
   double xErrorIntegral;  
   boolean feed;  
   public:  
   PIDController(double myKP, double myKI, double myKD, boolean isFeed){  
     kP = myKP;  
     kI = myKI;  
     kD = myKD;  
     xError = 0;  
     xDesired = 0;  
     xErrorPrev = 0;  
     xErrorIntegral = 0;  
     feed = isFeed;  
     }  
   
     void setDesired(double myXDesired){  
     xDesired = myXDesired;  
     }  
   
   double update(double x,double dx){  
     double command = 0.0;  
     xError = (xDesired - x);  
     xErrorIntegral+=xError;  
     if(feed)  
      command = kP*xError + kI*xErrorIntegral +kD*dx;  
     else  
      command = kP*xError + kI*xErrorIntegral +kD*(xError-xErrorPrev);  
     xErrorPrev = xError;  
     return command;  
   }  
 };  
   
 PIDController rollFilter(filterKP,filterKI,0, 0);  
 PIDController pitchFilter(filterKP,filterKI,0, 0);  
   
 void setup(){  
  Serial.begin(57600);               // init UART  
  Wire.begin();                  // init I2C  
  TWBR = ((F_CPU / 400000) - 16) / 2;       // fast I2C   
  initSensors();  
  loopStart = millis();  
 }  
   
 void loop(){  
  if(millis()-loopStart>=1000.0/FREQ){  
   loopStart = millis();  
   readSensors();  
   IMUFilterUpdate();  
   Serial.print(pitch-initialPitch);  
   Serial.print(" | ");  
   Serial.println(roll-initialRoll);  
  }  
 }  
   
 void initSensors(){  
  // initialize the ITG-3205 gyro  
  // range: 2000deg/s, bandwidth: 188Hz  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x16);            
  Wire.write(0x19);            
  Wire.endTransmission();  
    
  // initialize the BMA180 accelerometer  
  // range: 2g, bandwidth: 150Hz (all defaults)  
  loopStart = millis();  
  for (int i;i<1000;i++){  
   while(millis()-loopStart < 1000.0/FREQ);  
   loopStart = millis();  
   readSensors();  
   IMUFilterUpdate();  
  }  
  initialPitch = pitch;  
  initialRoll = roll;  
 }  
   
 void readSensors(){  
  unsigned char i;  
  unsigned char raw_data[6];  
  signed int raw_value;  
    
  // read the ITG-3205 gyro  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x1D);                    
  Wire.endTransmission();  
    
  i = 0;  
  Wire.requestFrom(GYR_ADR, 6);  
  while(Wire.available())  
  {  
   raw_data[i++] = Wire.read();  
  }  
    
  // convert raw gyro data to [deg/s]  
  raw_value = (raw_data[0] << 8) + raw_data[1];  
  gyro_pitch = -1.0*(float) raw_value * GYR_K;  
  raw_value = (raw_data[2] << 8) + raw_data[3];  
  gyro_roll = (float) raw_value * GYR_K;  
  raw_value = (raw_data[4] << 8) + raw_data[5];  
  gyro_yaw = (float) raw_value * GYR_K;  
    
  // remind the gyro to stay in 2000deg/s mode???  
  // range: 2000deg/s, bandwidth: 188Hz  
  Wire.beginTransmission(GYR_ADR);  
  Wire.write(0x16);            
  Wire.write(0x19);            
  Wire.endTransmission();  
    
  // read the BMA180 accelerometer  
  Wire.beginTransmission(ACC_ADR);  
  Wire.write(0x02);                    
  Wire.endTransmission();  
    
  i = 0;  
  Wire.requestFrom(ACC_ADR, 6);  
  while(Wire.available())  
  {  
   raw_data[i++] = Wire.read();  
  }  
    
  // convert raw accel data to [deg] for pitch/roll  
  raw_value = ((raw_data[1] << 8) + raw_data[0]) >> 2;  
  accel_x = (float)(raw_value) * VEL_K;  
  accel_roll = -(float) raw_value * ACC_K;//(atan2(accel_z,accel_x)*RADTODEG-90.0);  
    
  raw_value = ((raw_data[3] << 8) + raw_data[2]) >> 2;  
  accel_y = (float)(raw_value) * VEL_K;  
  accel_pitch =-1.0*(float) raw_value * ACC_K; //(atan2(accel_z,accel_y)*RADTODEG-90.0);  
    
  // convert raw accel data to [ft/s^2] for z  
  raw_value = ((raw_data[5] << 8) + raw_data[4]) >> 2;  
  accel_z = (float)(raw_value) * VEL_K;  
 }  
   
 void IMUFilterUpdate(){  
  pitchFilter.setDesired(accel_pitch);  
  pitch = pitch + (gyro_pitch + pitchFilter.update(pitch,0))*dt;  
    
  rollFilter.setDesired(accel_roll);  
  roll = roll + (gyro_roll + rollFilter.update(roll,0))*dt;  
 }   

See you next post! 
break;

My First Quadrotor: Parts Selection and Build

Note: All this happened in mid-January. I've just been too busy to finish this and post until now...

Yupp. I built it! Here's how it happened...

Being a practical man, I want to build a quadrotor for a few concrete reasons: 

1. I want to write the flight controller from pretty much scratch, as an exercise in my control system knowledge. 
2. I want to show to the world just how easy and inexpensive it is to choose parts for, order, and build one of these. 
3. I want to to document my software design process in a way a blog reader can learn from. 
4. THEY'RE F****ING COOL. I mean, LOOK AT THESE: 

AWWWW ^_^

Shane's 4PCB at MITERS. 

Parts Selection: 

I began the process by asking the experienced quadrotor-ers around MITERS and the IDC about where to start. Shane suggested I find a 330mm-size frame. Searching the Hobbyking Quadcopter frame section for "330" yielded the following beauty: http://www.hobbyking.com/hobbyking/store/__28172__F330_Glass_Fiber_Mini_Quadcopter_Frame_330mm.html. It seemed big enough to make the controller run smoothly while being small enough to comfortably fly indoors (my flying domain of choice, for now). 

So I had a frame. To get from here to working quadrotor I need to spec out: 

• Frame
• 4 x Brushless Motors
• 4 x Electronic Speed Controllers (ESCs for short, one per motor)
• n x sets of Propellers (Banks Hunter let me know these break often, so I ordered like 5 sets)
• Battery Pack
• Battery Charger (I did not purchase one because I already have one)
• Programmable microcontroller board  (Arduino should do)
• IMU for angle feedback
• Other sensors for other kinds of feedback (usually come with the IMU and ucontroller board)
• XBee pair and USB interface, in order to give the quadrotor commands from my computer program. 

Looking at the F330 frame's listing on the HobbyKing website gave me the proper specifications I needed for the various components. I chose the components necessary simply based on what was least expensive. I picked the SK3 Aerodrive 2826 motors with a Kv of 1130 rpm/V and many spare sets of red 8", 4.5"pitch propellers (These break often, I hear), which are within the specification range recommended by the F330 frame page. I chose 15-amp ESCs, which is more than I need for this size copter, because they were also well-priced. I then chose the lightest 1800mAh 3s (11.1 V nominal) battery I could find.  In order to tie them all together with minimal wiring work, Banks suggested I use a nifty Power Distribution Board to get battery power to the ESCs.

As for the XBees, I chose to get this starter kit from Makershed, which was the least expensive option. It comes with a breadboard-able 5V-3.3V logic level shifter (Many electronics run on 5V, the XBees run on 3.3V) and a USB adapter so you can connect to a serial device (like a quadrotor!) wirelessly. 

I'll worry about the sensors and microcontroller board later, because I have Arduinos and IMU breakout boards readily available nearby. The ESCs each take a 5V servo-style PWM input signal, so an Arduino is be able to command all 4 brushless motors. Apparently the ESCs also generate 5V for the logic power, so no need to use a regular! 

Total cost? About $150.00. NOT BAD! 

Building the Quadrotor:

About a week after placing the order, I received the XBee starter kit from Makershed and the four SK3 2826 motors from the HobbyKing USA Warehouse in the mail. HobbyKing has both an International and USA warehouse, and you can isolate the online selection available from each right on the website. The International warehouse has a wider selection of products, but shipping to the USA is costly and takes forever. On the other hand, the USA HobbyKing warehouse does not have as wide a selection, but shipping to US destinations is both cheaper and more timely. For this build, I ordered everything but the motors from the International warehouse. 

After waiting a few more weeks, a package from China came!

And check out all the new toys! From left to right, I see two frames (I gave the extra one to Shane Colton), the ESCs, battery (above), and a shitton of props. 

Somewhere in the pile of new toys was the Power Distribution Board that Banks highly recommended I purchase. As you can see, it makes hooking everything together really easy. Kinda looks like a floating nervous/circulatory system quadcopter without a backbone...

Speaking of the backbone, let's put the sexy F330 frame together! The kit came with an instruct- I mean INSTRUCITON MANUAL! It was pretty well-written, though the kit was simple enough to figure out myself. 

The flip side of the sheet had the same thing, but written in Chinese and probably with better and more detailed instruct- I mean INSTRUCITONS. 

What really bugs me about the kit is there is only enough hardware (screws, etc) to put the thing together, and no extras should you lose any. Metric hardware isn't as easy to come by for me, so this may prove itself to be a pain. 

The frame consisted of four identical fiberglass arms placed in an equilateral X formation sandwiched by two carbon fiber plates. Lacking the room to hold the battery and control board (which will presumably occupy the very top) and attach the Power Distribution Board (PDB?), I decided to fasten the PDB to the underside of the vehicle. Sure, live power lines are just exposed for now, but I'll insulate the whole thing before trying to fly it. 

The tiny (D'AWW SO CUTE!) SK3 2826 motors each come with the proper mounting hardware (Again, no extras in case the tiny M3 screw wanders before it is secure...). I decided to leave off the top carbon fiber plate for now until I could figure out my microcontroller situation. 

The ESCs and extra wire folded up pretty nicely on each arm of the copter. I used small black zipties to keep everything down and prevent general self-destruction from the propellers from chopping things up.
The ESCs are sensorless, relying instead on current sensing to commute the motors.  For now, I plugged the 3-phase SK3 2826 motors into the ESCs in a random configuration. I will later need to shuffle the leads in order to ensure that each propeller is spinning in the proper direction, producing downward thrust. 

The propellers are not all identical. Each set of propellers comes with two pairs (4 total): one pair of clockwise and one pair of counterclockwise props. They are installed diagonally opposite each other on the quad, which is shown in the frame kit instructions. The SK3 2826 motors come with all the mounting hardware necessary to hold the props down. Yet again, no extra metric hardware included should you lose anything. I'm surprised the prop and motor are rotationally coupled by only by the friction force produced by the preloaded washer. I sure hope it doesn't go flying at my face. Or into my arm, for that matter...

Now, for the controller I was going to use an Arduino of some kind, an XBee attached to it, and a spare Sparkfun MPU6050 breakout board I picked up. Shane gave me something that was pretty much all of the above combined! The MultiWii 328P is a board that houses an atmega328 microcontroller (the same that is found on many an Arduino), an FTDI chip for USB-to-Serial communication and programming, and an array of sensors like an IMU, magnetometer, and barometer for stability and navigation feedback. When Shane used it, he wanted to communicate with a receiver via XBee, so he broke out the necessary UART pins on the MultiWii328P to connect to the XBee. He gave me this great looking all-in-one board in exchange for giving him one of the F330 quad frames (Apparently these are never in stock, and I was lucky to be able to find one). 

The ESC leads all plugged into the servo out pins of the MultiWii328P board. It's almost like they designed it to be easy to put together! After fastening the board onto the top carbon fiber plate and attaching the plate to the frame, I had a quadrotor! 

And there you have it! This was really freaking easy to choose parts for and put together. Next up: Write the software and get it flying! 

Filtered IMUs are so cool!

This video is from 2.12 (Intro to Robotics), a class I'm taking this semester. I got a Sparkfun MPU6050 Breakout Board to talk to an Arduino Nano via i2c, which then communicates with my Ubuntu laptop via Serial. A Python program interprets the Arduino serial signal as outputs of the sensor's Accelerometer[meters/(second^2)] and Gyroscope[degrees/second]. A second order complementary filter (More info on these style filters in Shane Colton's Blog) then interprets these signals as pitch, roll, and yaw angles. Which you can see on my computer screen represented as a line. 

Updates on Melonchopper's build and more details on 2.12 coming soon!

Quadrature Encoders in Arduino, done right. Done right.

Guys guys guys I should start doing things with my infinite motors! I will eventually, at some point, want to detect Quadrature Encoder feedback from multiple motors using an Arduino. Like in MASLAB or for Deltabot or something. So I'm doing it now. Because it's more fun than my homework.

http://www.hessmer.org/blog/2011/01/30/quadrature-encoder-too-fast-for-arduino/

Man, this is a great example. Like seriously read this guy's entire blog. It's not terribly long, and it's full of useful stuff. I'm a huge fun of everything he's done.

First thing I need to do to is get the provided code running. Taking a deeper look, it's unfortunately incomplete.

Let me explain:

Microcontrollers, like the AtXmega16a4u running TinyArmTroller, and the AtMega328 that's running in an Arduino Uno, have a feature called interrupts. An interrupt is a function that is called only when a certain event, usually pertaining to the voltage level at a certain pin, occurs. For example, suppose you want to make an LED turn on when a button is held down. You can approach this in two ways: 

Method 1 involves a loop that constantly checks whether or not a button is pressed. If the button is pressed, turn the light on. Otherwise, turn it off. This method is simple, and for the application of making a light turn on, is totally acceptable. 

However, if your microcontroller supports interrupts, you can create two interrupts. One of them will trigger when the button pin rises, that is, when it changes from low to high (Off to On). When the interrupt triggers, the interrupt handler function turning the LED on is immediately called. The second interrupt will trigger when the button pin undergoes a falling action (ie. The button goes from On to Off). When the interrupt triggers, The handler function turns the LED off. 

Now, in the context of a quadrature encoder, you traditionally need four interrupts: one to detect a channel A rising action, and channel A falling action, and a channel B rising and falling action. At each interrupt, you would know what the previous state of Channel A and Channel B were, and could iterate the odometry count appropriately (+1 or -1).

If you want some more reading on how Quadrature Encoders work, and how to use a Quadrature Decoder IC, check this page out:
http://www.robotoid.com/appnotes/circuits-quad-encoding.html
(The above diagram is grabbed from this page)

Also, let MIT Professor Harry Asada (the lecturer of 2.12: Intro to Robotics and my undergraduate academic adviser) teach you about encoders on page 13 of these free course notes: http://ocw.mit.edu/courses/mechanical-engineering/2-12-introduction-to-robotics-fall-2005/lecture-notes/chapter2.pdf

The code provided on Dr. Hessmer's site does not correctly implement a quadrature decoder. In that code, an interrupt triggers only when Channel A rises. He does achieve directionality, one of the primary advantages of using Quadrature encoders, by reading the state of Channel B each time the interrupt triggers, and either adding or subtracting to the count based on that reading. 

The issue with using this method is you achieve A QUARTER OF THE POSSIBLE RESOLUTION with a Quadrature encoder. Looking at the above diagram, suppose Channel A is on the outside and Channel B is on the inside. There are 12 black spaces per channel. If you account for both rising and falling of a single channel, you get a resolution of 24 counts per revolution [cpr]. Counting both Channels A and B, your resolution becomes 48 cpr! The code provided in the above example only takes into account when Channel A rises, cutting the resolution back down to 12. 

I want to correctly implement quadrature decoding, where interrupts trigger in both the A and B channel, and the resolution is four times that of that of just a single channel. Issue is, the Arduino Uno can only have two interrupts enabled at a time. I need four per motor, ideally.

Then I thought of some Code Magick: When one of four interrupts is triggered, I could reinitialize each interrupt to whatever two interrupts were possible at a current state!

Then I realized Arduino also could just trigger an interrupt when the state of a pin changed, not necessarily when it was Rising or Falling. That's good, because I can just check both pins at each interrupt, test it against the previous pin reading, and adjust the count accordingly. 

(If I wanted to keep only a single interrupt like in the above code, I could modify the interrupt to trigger when Channel A changes, read both channels A and B when this happens, and adjust the odometry count accordingly. This however would still keep me at half the total possible resolution. Using a second interrupt which triggers when Channel B changes is ideal).

Here's my setup: Arduino, Pittman 655 Motor, Computer with Arduino IDE. 

Next, I attached the encoder in the correct manner. Looking at the datasheet Tech Support Joe from Pittman scanned me from the 1990s, the lime green wire is ground, the red wire is Channel A, the black wire is +5V power (WTF?!), and the white wire is Channel B. The dark green wire is just shielding for the signal wire. 

I connected +5V and GND accordingly, and the Channel A and Channel B cables to pins 2 and 3 on my Arduino Uno. Pins 2 and 3 are the only pins able to trigger interrupts (Interrupt 0 and 1, respectively). 

Here's the code I used: 


/*
*Quadrature Decoder 
*/
#include "Arduino.h"
#include <digitalWriteFast.h>  // library for high performance reads and writes by jrraines
                               // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0
                               // and http://code.google.com/p/digitalwritefast/

// It turns out that the regular digitalRead() calls are too slow and bring the arduino down when
// I use them in the interrupt routines while the motor runs at full speed.

// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterruptA 0
#define c_LeftEncoderInterruptB 1
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 3
#define LeftEncoderIsReversed

volatile bool _LeftEncoderASet;
volatile bool _LeftEncoderBSet;
volatile bool _LeftEncoderAPrev;
volatile bool _LeftEncoderBPrev;
volatile long _LeftEncoderTicks = 0;

void setup()
{
  Serial.begin(9600);

  // Quadrature encoders
  // Left encoder
  pinMode(c_LeftEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_LeftEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_LeftEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_LeftEncoderInterruptA, HandleLeftMotorInterruptA, CHANGE);
  attachInterrupt(c_LeftEncoderInterruptB, HandleLeftMotorInterruptB, CHANGE);
}

void loop()
{ 
  Serial.print("Encoder Ticks: ");
  Serial.print(_LeftEncoderTicks);
  Serial.print("  Revolutions: ");
  Serial.print(_LeftEncoderTicks/4000.0);//4000 Counts Per Revolution
  Serial.print("\n");
}


// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA(){
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);
  
  _LeftEncoderTicks+=ParseEncoder();
  
  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

// Interrupt service routines for the right motor's quadrature encoder
void HandleLeftMotorInterruptB(){
  // Test transition;
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);
  
  _LeftEncoderTicks+=ParseEncoder();
  
  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

int ParseEncoder(){
  if(_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }
}


I pull up the Serial monitor in the Arduino IDE and see: 

Running the program, I see all is well. 

To keep track of my motor's angle, I just used a sharpie on a point of the shaft and on the motor housing. Here is where I defined theta = 0. 

I moved the shaft 90 degrees, and expected the encoder count to be 250 (As the motor states it is 1000cpr). 

Interesting. They must have meant that there are 1000 dark slots per channel of the encoder! This encoder is actually 4000 counts per revolution! 

4000 counts/revolution*340 rpm/volt*12 Volts*1min/60sconds = 272000 interrupts/s

(When being driven at 12 volts)

Whatever hardware is running the software running this (like my Arduino) should be ideally running at a clock speed an order of magnitude higher than this in order to keep up. 

One full rotation. Yup, makes sense. 

When I move the motor shaft really fast, though, it picks up a lot of error. I should write a filter to be able to keep up with really fast motor movements....

Not now though. I'm late for class -_-