Okay, so I'm trying to write a more or less simple code for having different pwm signals sent out to motors when I receive certain accelerometer readings in order to stabilize a quadcopter but am having a bit of an issue...
Goal:
I would like a single motor to increase every fourth of a second if the arm that the motor is on, is dipping lower than a certain programmed value. I would then like the motor to compensate its speed to stabilize once the accelerometer sends back a certain value.
My problem:
Every time I test my code, the motors seem to react very slowly to the degree of tilt of the quadcopter. Also, the motors will keep increasing and never stabilize out. In other words, if I were to turn on my quadcopter and let it go, it would take off and accelerate upward. I am pretty sure this is a programming issue.
If anyone can point me in the right direction in how I need to modify my code, I would be extremely appreciative of your time. Thank you very much.
Here is the code I am trying to work with:
// Quadcopter Sketch as of 11/14/12
#include <Wire.h>
#define DEVICE (0x53) // Device address as specified in data sheet
#include <Servo.h>
Servo myservo1; // first motor to be controlled by potentiometer (back)
Servo myservo2; // second motor to be controlled by potentiometer (front)
Servo myservo3; // third motor to be controlled by potentiometer (right)
Servo myservo4; // fourth motor to be controlled by potentiometer (left)
int incomingByte; // data received over serial from computer
int servovalprimey1 = 0;
int servovalprimex1 = 0;
int servovalprimey2 = 0;
int servovalprimex2 = 0;
int servovalprime1 = 0; // variable for servo variable to be stored on
int servovalprime2 = 0;
int servovalprime3 = 0;
int servovalprime4 = 0;
int val1; // servo value reading variable
byte _buff[6];
char POWER_CTL = 0x2D; //Power Control Register
char DATA_FORMAT = 0x31;
char DATAX0 = 0x32; //X-Axis Data 0
char DATAX1 = 0x33; //X-Axis Data 1
char DATAY0 = 0x34; //Y-Axis Data 0
char DATAY1 = 0x35; //Y-Axis Data 1
char DATAZ0 = 0x36; //Z-Axis Data 0
char DATAZ1 = 0x37; //Z-Axis Data 1
int x = (((int)_buff[1]) << 8) | _buff[0]; //x value equals that reading of accel
int y = (((int)_buff[3]) << 8) | _buff[2]; //y value equals that reading of accel
int z = (((int)_buff[5]) << 8) | _buff[4]; //z value equals that reading of accel
// Reads num bytes starting from address register on device in to _buff array
void readFrom(byte address, int num, byte _buff[]) {
Wire.beginTransmission(DEVICE); // start transmission to device
Wire.write(address); // sends address to read from
Wire.endTransmission(); // end transmission
Wire.beginTransmission(DEVICE); // start transmission to device
Wire.requestFrom(DEVICE, num); // request 6 bytes from device
int i = 0;
while(Wire.available()) // device may send less than requested (abnormal)
{
_buff[i] = Wire.read(); // receive a byte
i++;
}
Wire.endTransmission(); // end transmission
}
void setup()
{
Wire.begin(); // join i2c bus (address optional for master)
//Start Serial
Serial.begin(9600);
myservo1.attach(6); // back motor
myservo2.attach(9); // front motor
myservo3.attach(10); // right motor
myservo4.attach(11); // left motor
delay(500);
//Put the ADXL345 into +/- 4G range by writing the value 0x01 to the DATA_FORMAT register.
writeTo(DATA_FORMAT, 0x01);
//Put the ADXL345 into Measurement Mode by writing 0x08 to the POWER_CTL register.
writeTo(POWER_CTL, 0x08);
}
void readAccel() {
uint8_t howManyBytesToRead = 6;
readFrom( DATAX0, howManyBytesToRead, _buff); //read the acceleration data from the ADXL345
// each axis reading comes in 10 bit resolution, ie 2 bytes. Least Significat Byte first!!
// thus we are converting both bytes in to one int
int x = (((int)_buff[1]) << 8) | _buff[0];
int y = (((int)_buff[3]) << 8) | _buff[2];
int z = (((int)_buff[5]) << 8) | _buff[4];
if (x > 15) {
servovalprime1 = servovalprimex1++;
myservo1.write(servovalprime1);
delay(250);
}
if (x < -15) {
servovalprime2 = servovalprimex2++;
myservo2.write(servovalprime2);
delay(250);
}
if ((x < 15) && (x > -15)) {
myservo1.write(servovalprimex1);
myservo1.write(servovalprimex2);
delay(250);
}
if (y > 15) {
servovalprime3 = servovalprimey1++;
myservo3.write(servovalprime3);
delay(250);
}
if (y < -15) {
servovalprime4 = servovalprimey2++;
myservo4.write(servovalprime4);
delay(250);
}
if ((y < 15) && (y > -15)) {
myservo3.write(servovalprimey1);
myservo4.write(servovalprimey2);
delay(250);
}
}
void writeTo(byte address, byte val) {
Wire.beginTransmission(DEVICE); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
void loop()
{
readAccel(); // read the x/y/z tilt
delay(250); // only read every 0,5 seconds
}