Long time reader, first time poster.
I bought my Arduino not too long ago and aside from a few leds and motors my quadrotor is my first project.
My issue is not in interfacing with the hardware. iI already got the code set up enough to get the serial readout for each of my sensors, which are an Ultrasonic Distance Sensor (HC-SR04), 3 Axis Gyro (L3G4200D) and a 3 Axis Accelerometer(MMA7361).
The code is made up of 3 existing codes for each module and my own written serial display code.
My questions are simple:
Does my code look proper? Structured correctly?
Is there any unnecessary elements aside from the comment tags?
How can i Graph this data?
And the code:
#define trigPin 12
#define echoPin 13
int sleepPin = 4;
int xpin = A0;
int ypin = A1;
int zpin = A2;
#include <Wire.h>
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
int L3G4200D_Address = 105;
int x;
int y;
int z;
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("Starting Up Sensors");
setupL3G4200D(250); // 150, 500, 2000
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(sleepPin,OUTPUT);
digitalWrite(sleepPin, HIGH);
pinMode(xpin, INPUT);
digitalWrite(xpin, HIGH);
pinMode(ypin, INPUT);
digitalWrite(ypin, HIGH);
pinMode(zpin, INPUT);
digitalWrite(zpin, HIGH);
delay(1000);
}
void loop(){
int duration, distance, xmath, xdisp, ymath, ydisp, zmath, zdisp;
digitalWrite(trigPin, HIGH);
delayMicroseconds(100);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2)/29.10;
xmath = analogRead(xpin);
xdisp = (xmath-658);
ymath = analogRead(ypin);
ydisp = (ymath-664);
zmath = analogRead(zpin);
zdisp = (zmath-729);
getGyroValues();
delay(500); //Delay for readability
Serial.print("A-X : ");
Serial.print(xdisp);
Serial.print(" | A-Y : ");
Serial.print(ydisp);
Serial.print(" | A-Z : ");
Serial.print(zdisp);
Serial.print(" || G-X:");
Serial.print(x);
Serial.print(" | G-Y:");
Serial.print(y);
Serial.print(" | G-Z:");
Serial.print(z);
Serial.print(" | Altitude : ");
if (distance <= 5.08){Serial.println("TOO LOW!");}
else {
Serial.print(distance/2.54);
Serial.println(" in");}
}
void getGyroValues(){
byte xMSB = readRegister(L3G4200D_Address, 0x29);
byte xLSB = readRegister(L3G4200D_Address, 0x28);
x = ((xMSB << 8) | xLSB);
byte yMSB = readRegister(L3G4200D_Address, 0x2B);
byte yLSB = readRegister(L3G4200D_Address, 0x2A);
y = ((yMSB << 8) | yLSB);
byte zMSB = readRegister(L3G4200D_Address, 0x2D);
byte zLSB = readRegister(L3G4200D_Address, 0x2C);
z = ((zMSB << 8) | zLSB);
}
int setupL3G4200D(int scale){
writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
if(scale == 250){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
}else if(scale == 500){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
}else{
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
}
writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress);
Wire.write(address);
Wire.write(val);
Wire.endTransmission();
}
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1);
while(!Wire.available()) {
}
v = Wire.read();
return v;
}
[code/]
Thanks in advance to any help that is given.
Des