Step 23: Code
Our code was written in a modified form of C++ that is described on the Arduino website.
Our code represents a feedback control system known as PID (proportional integral derivative). Currently, it only employs use of the proportional and derivative components. With our current code, the quadrotor self-stabilizes quite well in the air, but is a little unstable on takeoff. However, this instability can be mitigated by taking off quickly.
To find the current amounts of tilt on the X and Y axis from accelerometer and gyro data, we used an algorithm that would average previous accelerometer data and combine it with gyro data to reach an angle measurement that was fairly resilient to linear acceleration.
We only do 2 Pulsin commands per loop (instead of 4) to cut the loop time in half, which makes the quadrotor control system much more responsive.
//neutral accelerometer/gyro positions
#define X_ZERO 332
#define Y_ZERO 324
#define Z_ZERO 396
#define PITCH_ZERO 249
#define ROLL_ZERO 249
#define YAW_ZERO 248
#define GYRO_CON 1.47
#define ACCEL_CON 0.93
#define TIME_CON 0.02
#define SEN_CON 0.95
//motor speed vars
int speeds[4];
//gyro inputs - current tilt vars
float pitch, roll, yaw;
int pitchzero, rollzero;
//accelerometer inputs - current acceleration vars
float xin, yin, zin;
//human inputs - control info vars
float pitchin, rollin, yawin, zhuman;
//random other vars
float xaverage=0, yaverage=0;
int y=0;
int blah;
//proportionality constants
float p=2.5; // P proportionality constant
float d=0.5; // D proportionality constant
void setup() {
zhuman=0;
rollin=0;
Serial.begin(9600);
for(int x=6; x<10; x++) {
pinMode(x, OUTPUT);
}
//send upper bound for human inputs to the motor speed controllers
for(int x=6; x<10; x++) {
pulsout(x,2000);
}
delay(5000);
//get zeros for pitch and roll human inputs
for(int x=0; x<10; x++) {
y=y+analogRead(3);
}
pitchzero=y/10;
y=0;
for(int x=0; x<10; x++) {
y=y+analogRead(4);
}
rollzero=y/10;
}
void loop () {
//accelerometer and gyro inputs ranged -232 to 232?
xin=(analogRead(0)-X_ZERO)*ACCEL_CON;
yin=(analogRead(1)-Y_ZERO)*ACCEL_CON;
zin=(analogRead(2)-Z_ZERO)*ACCEL_CON;
pitch=(pitchzero-analogRead(3))*GYRO_CON;
roll=(rollzero-analogRead(4))*GYRO_CON;
yaw=(analogRead(5)-YAW_ZERO)*GYRO_CON;
//get human inputs through radio here range of -30 to 30 except for zhuman which has an ideal range of 1000-2000, only 2 pulses per loop
if(blah==0) {
yawin=0.06*((signed int) pulseIn(2,HIGH)-1500);
pitchin=0.06*((signed int) pulseIn(3,HIGH)-1500);
blah=1;
}
else {
zhuman=(signed int) pulseIn(4,HIGH);
rollin=0.06*((signed int) pulseIn(5,HIGH)-1400); //1400 instead of 1500 is to correct for the underpowered motor #4 by trimming it in code
blah=0;
}
//averaging, etc.
xaverage= SEN_CON *( xaverage + TIME_CON * pitch) + ( 1 - SEN_CON ) * xin;
yaverage= SEN_CON *( yaverage + TIME_CON * roll) + ( 1 - SEN_CON ) * yin;
//calculate the motor speeds
if(zhuman<1150) {
for(int x=0; x<4; x++) {
speeds[x]=zhuman;
}
}
else {
if(zhuman > 1450) {
zhuman = 1450;
}
speeds[0] = zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch;
speeds[1] = zhuman - p*(pitchin - xaverage) - p*(yawin) + d*pitch;
speeds[2] = zhuman - p*(yaverage - rollin) + p*(yawin) - d*roll;
speeds[3] = zhuman - p*(rollin - yaverage) + p*(yawin) + d*roll;
}
//set the upper and lower bounds for motor speeds (1000=no speed, 1600=upper speed limit, 2000=maximum possible speed)
for(int x=0; x<4; x++) {
//speed limit between 1000 and 1600
if(speeds[x]<1000) {
speeds[x]=1000;
}
if(speeds[x]>1600) {
speeds[x]=1600;
}
}
//pulsouts to motor speed controllers
for(int x=0; x<4; x++) {
pulsout(x+6,speeds[x]);
}
}
void pulsout (int pin, int duration) {
digitalWrite(pin, HIGH);
delayMicroseconds(duration);
digitalWrite(pin, LOW);
}
Our code represents a feedback control system known as PID (proportional integral derivative). Currently, it only employs use of the proportional and derivative components. With our current code, the quadrotor self-stabilizes quite well in the air, but is a little unstable on takeoff. However, this instability can be mitigated by taking off quickly.
To find the current amounts of tilt on the X and Y axis from accelerometer and gyro data, we used an algorithm that would average previous accelerometer data and combine it with gyro data to reach an angle measurement that was fairly resilient to linear acceleration.
We only do 2 Pulsin commands per loop (instead of 4) to cut the loop time in half, which makes the quadrotor control system much more responsive.
//neutral accelerometer/gyro positions
#define X_ZERO 332
#define Y_ZERO 324
#define Z_ZERO 396
#define PITCH_ZERO 249
#define ROLL_ZERO 249
#define YAW_ZERO 248
#define GYRO_CON 1.47
#define ACCEL_CON 0.93
#define TIME_CON 0.02
#define SEN_CON 0.95
//motor speed vars
int speeds[4];
//gyro inputs - current tilt vars
float pitch, roll, yaw;
int pitchzero, rollzero;
//accelerometer inputs - current acceleration vars
float xin, yin, zin;
//human inputs - control info vars
float pitchin, rollin, yawin, zhuman;
//random other vars
float xaverage=0, yaverage=0;
int y=0;
int blah;
//proportionality constants
float p=2.5; // P proportionality constant
float d=0.5; // D proportionality constant
void setup() {
zhuman=0;
rollin=0;
Serial.begin(9600);
for(int x=6; x<10; x++) {
pinMode(x, OUTPUT);
}
//send upper bound for human inputs to the motor speed controllers
for(int x=6; x<10; x++) {
pulsout(x,2000);
}
delay(5000);
//get zeros for pitch and roll human inputs
for(int x=0; x<10; x++) {
y=y+analogRead(3);
}
pitchzero=y/10;
y=0;
for(int x=0; x<10; x++) {
y=y+analogRead(4);
}
rollzero=y/10;
}
void loop () {
//accelerometer and gyro inputs ranged -232 to 232?
xin=(analogRead(0)-X_ZERO)*ACCEL_CON;
yin=(analogRead(1)-Y_ZERO)*ACCEL_CON;
zin=(analogRead(2)-Z_ZERO)*ACCEL_CON;
pitch=(pitchzero-analogRead(3))*GYRO_CON;
roll=(rollzero-analogRead(4))*GYRO_CON;
yaw=(analogRead(5)-YAW_ZERO)*GYRO_CON;
//get human inputs through radio here range of -30 to 30 except for zhuman which has an ideal range of 1000-2000, only 2 pulses per loop
if(blah==0) {
yawin=0.06*((signed int) pulseIn(2,HIGH)-1500);
pitchin=0.06*((signed int) pulseIn(3,HIGH)-1500);
blah=1;
}
else {
zhuman=(signed int) pulseIn(4,HIGH);
rollin=0.06*((signed int) pulseIn(5,HIGH)-1400); //1400 instead of 1500 is to correct for the underpowered motor #4 by trimming it in code
blah=0;
}
//averaging, etc.
xaverage= SEN_CON *( xaverage + TIME_CON * pitch) + ( 1 - SEN_CON ) * xin;
yaverage= SEN_CON *( yaverage + TIME_CON * roll) + ( 1 - SEN_CON ) * yin;
//calculate the motor speeds
if(zhuman<1150) {
for(int x=0; x<4; x++) {
speeds[x]=zhuman;
}
}
else {
if(zhuman > 1450) {
zhuman = 1450;
}
speeds[0] = zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch;
speeds[1] = zhuman - p*(pitchin - xaverage) - p*(yawin) + d*pitch;
speeds[2] = zhuman - p*(yaverage - rollin) + p*(yawin) - d*roll;
speeds[3] = zhuman - p*(rollin - yaverage) + p*(yawin) + d*roll;
}
//set the upper and lower bounds for motor speeds (1000=no speed, 1600=upper speed limit, 2000=maximum possible speed)
for(int x=0; x<4; x++) {
//speed limit between 1000 and 1600
if(speeds[x]<1000) {
speeds[x]=1000;
}
if(speeds[x]>1600) {
speeds[x]=1600;
}
}
//pulsouts to motor speed controllers
for(int x=0; x<4; x++) {
pulsout(x+6,speeds[x]);
}
}
void pulsout (int pin, int duration) {
digitalWrite(pin, HIGH);
delayMicroseconds(duration);
digitalWrite(pin, LOW);
}
Remove these ads by
Signing Up
Visit Our Store »
Go Pro Today »
This was the only guide to building a quadrotor with good code explanation , could you plz explain the whole code to me and how the direction input from the transmitter is decoded and what actions are taken to follow the direction of the joystick.
Thanks Could i pls get a quick response.
I've done a quick costing of the project using the links referenced, it's coming out at $320 plus postage (say another 20%) so $380
I'm looking to make this project but don't know where to find the plastic spacers nuts and screws you've specified. I'm in australia and don't have home depot :) any ideas where i can find this stuff on the internet?