Hi all>>>> Some people may know about my project work on quadrotor with gsm control....
Any here is a small review >>
My project is about a quadrotor which should be controlled by gsm sms..
so in order to make this possible i wrote some of the few stuff except the GSM section ...
Here is the problem>>
When i upload this code my arduino doesnot show any response in either serial communication nor in the actual>>>
so i tried of uploading a simple code that is character analysis as in example section which worked fine in serial communication>>
what is the problem ??
here is the code>>
#include <Servo.h>
#include <L3G4200D.h>
#include <PID_v1.h>
L3G4200D gyro;
double Measuredx, Measuredy, Measuredz, Measureda, Outputx, Outputy, Outputz, Outputa, Setpointx, Setpointy, Setpointz, Setpointa;
PID X(&Measuredx, &Outputx, &Setpointx, 1.8, 0.42, 0.5, DIRECT);
PID Y(&Measuredy, &Outputy, &Setpointy, 1.8, 0.42, 0.5, DIRECT);
PID Z(&Measuredz, &Outputz, &Setpointz, 3.8, 1.3, 0.15, DIRECT);
PID A(&Measureda, &Outputa, &Setpointa, 3.8, 1.3, 0.15, DIRECT);
Servo FM;
Servo BM;
Servo LM;
Servo RM;
/* *(FM)
|
(RM)* -- + -- * (LM)
|
*(BM)
*/
#include <Wire.h>
#define MIN_THROTTLE 1060
#define MAX_THROTTLE 1860
#define AX A0
#define AY A1
#define AZ A2
#define gX (int)gyro.g.x
#define gY (int)gyro.g.y
#define gZ (int)gyro.g.z
#define trig_pin 12
#define echo_pin 13
int THROTTLE;
float ax,ay,az;
double zeroValue[6] = { 0 }; // gyroX, gyroY, gyroZ, accX, accY, accZ
/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;
// Complimentary filter
double compAngleX = 180;
double compAngleY = 180;
// Used for timing
unsigned long timer;
void measure_height(){
int duration;
digitalWrite(trig_pin, HIGH);
delayMicroseconds(1000);
digitalWrite(trig_pin, LOW);
duration = pulseIn(echo_pin, HIGH);
Measureda = (duration/2) / 29.1;
}
void esc_calib(){
Serial.println("ESC Calibration");
Serial.println("Front MOTOR");
delay(1000);
FRONT(MIN_THROTTLE);
Serial.println("Back MOTOR");
delay(1000);
BACK(MIN_THROTTLE);
Serial.println("Left MOTOR");
delay(1000);
LEFT(MIN_THROTTLE);
Serial.println("Right MOTOR");
delay(1000);
RIGHT(MIN_THROTTLE);
Serial.println("Calibration Success");
}
void FRONT(int x){
FM.writeMicroseconds(x);
}
void BACK(int x){
BM.writeMicroseconds(x);
}
void LEFT(int x){
LM.writeMicroseconds(x);
}
void RIGHT(int x){
RM.writeMicroseconds(x);
}
void attach_MOTORS(int i, int j, int k, int l){
delay(100);
FM.attach(i);
delay(100);
BM.attach(j);
delay(100);
LM.attach(k);
delay(100);
RM.attach(l);
delay(100);
}
void read_ACC(){
ax=analogRead(A0);
ay=analogRead(A1);
az=analogRead(A2);
}
void calibSensor(){
for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
gyro.read();
zeroValue[0] += gX;
zeroValue[1] += gY;
zeroValue[2] += gZ;
read_ACC();
zeroValue[3] += ax;
zeroValue[4] += ay;
zeroValue[5] += az;
delay(10);
}
zeroValue[0] /= 100;
zeroValue[1] /= 100;
zeroValue[2] /= 100;
zeroValue[3] /= 100;
zeroValue[4] /= 100;
zeroValue[5] /= 100;
zeroValue[5] /= 100; // Z value is -1g when facing upwards - Sensitivity = 0.33/3.3*1023=102.3
}
void measure_ANGLE(){
timer = micros();
gyro.read();
double gyroXrate = ((gX-zeroValue[0])/14.286); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = 0.00333/3.3*1023=1.0323
gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Without any filter
double gyroYrate = ((gY-zeroValue[1])/14.286);
gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
double gyroZrate = ((gZ-zeroValue[2])/14.286);
gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);
/* Serial.println(gyroZangle); // This is the yaw
*/
double accXval = (double)(analogRead(AX)-zeroValue[3]);
double accYval = (double)(analogRead(AY)-zeroValue[4]);
double accZval = (double)(analogRead(AZ)-zeroValue[5]);
// Convert to 360 degrees resolution
// atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
// We are then convert it to 0 to 2? and then from radians to degrees
double accXangle = (atan2(accXval, accZval)+PI)*(180/3.14);
double accYangle = (atan2(accYval, accZval)+PI)*(180/3.14);
timer = micros();
/* You might have to tune the filters to get the best values */
compAngleX = (0.965*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.035*(accXangle));
compAngleY = (0.965*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.035*(accYangle));
// reset timing
/* print data to processing */
//Serial.print(gyroXangle);Serial.print("\t");
//Serial.print(gyroYangle);Serial.print("\t");
// Serial.print(accXangle);Serial.print("\t");
// Serial.print(accYangle);Serial.print("\t");
Serial.print(compAngleX);Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(gyroZangle);
//Serial.print(xAngle);Serial.print("\t");
//Serial.print(yAngle);Serial.print("\t");
}
void roll_compute(){
measure_ANGLE();
Y.Compute();
}
void pitch_compute(){
measure_ANGLE();
X.Compute();
}
void yaw_compute(){
measure_ANGLE();
Z.Compute();
}
void attitude(){
A.Compute();
}
void setup(){
pinMode(12, OUTPUT);
pinMode(13, INPUT);
Serial.begin(9600);
gyro.enableDefault();
attach_MOTORS(6,9,10,11);
delay(2000);
esc_calib();
delay(1000);
X.SetOutputLimits(1060, 1860);
X.SetMode(AUTOMATIC);
Y.SetOutputLimits(1060, 1860);
Y.SetMode(AUTOMATIC);
Z.SetOutputLimits(1060, 1860);
Z.SetMode(AUTOMATIC);
A.SetOutputLimits(1060, 1860);
A.SetMode(AUTOMATIC);
}
void stabilize_x(){
if (Measuredx<Setpointx)
{
int i= (THROTTLE+(Outputx-MIN_THROTTLE));
RIGHT(i);
}
else if (Measuredx>Setpointx){
int i=(THROTTLE-(Outputx-MIN_THROTTLE));
LEFT(i);
}
else
{
RIGHT(THROTTLE);
LEFT(THROTTLE);
}
}
void stabilize_y(){
if (Measuredy<Setpointy){
int i=THROTTLE+(Outputy-MIN_THROTTLE);
BACK(i);
}
else if(Measuredy<Setpointy){
int i=(THROTTLE-(Outputy-MIN_THROTTLE));
FRONT(i);
}
else{
FRONT(THROTTLE);
BACK(THROTTLE);
}
}
void stabilize_height(){
if (Measureda<Setpointa){
int i=THROTTLE+(Outputa-MIN_THROTTLE);
FRONT(i);
BACK(i);
LEFT(i);
RIGHT(i);
}
else if (Measureda>Setpointa){
int i=THROTTLE-(Outputa-MIN_THROTTLE);
FRONT(i);
BACK(i);
LEFT(i);
RIGHT(i);
}
else{
THROTTLE=Outputz;
FRONT(THROTTLE);
BACK(THROTTLE);
LEFT(THROTTLE);
RIGHT(THROTTLE);
}
}
void stabilize_yaw(){
if (Measuredz<Setpointz){
int i=THROTTLE+(Outputz-MIN_THROTTLE);
int j=THROTTLE-(Outputz-MIN_THROTTLE);
FRONT(i);
BACK(i);
LEFT(j);
RIGHT(j);
}
else if (Measuredz>Setpointz){
int i=THROTTLE+(Outputz-MIN_THROTTLE);
int j=THROTTLE-(Outputz-MIN_THROTTLE);
FRONT(j);
BACK(j);
LEFT(i);
RIGHT(i);
}
else{
FRONT(THROTTLE);
BACK(THROTTLE);
LEFT(THROTTLE);
RIGHT(THROTTLE);
}
}
void loop(){
pitch_compute();
roll_compute();
yaw_compute();
Serial.println(Outputz);
Serial.print(compAngleX);
Serial.print('/t');
Serial.println(compAngleY);
}
Thanks in advance!! Pls help>> I have only short time to finish this project =( =( =( =( =(