/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter

« back to all changes in this revision

Viewing changes to test/gyroscopes/main.ino

  • Committer: dan
  • Date: 2014-04-02 20:11:38 UTC
  • mfrom: (31.1.2 bzr-project)
  • Revision ID: dan-20140402201138-5iy09kxg2pg0m4i5
Commit local changes

Show diffs side-by-side

added added

removed removed

Lines of Context:
test/gyroscopes/main.ino
 
1
//Arduino 1.0+ only
 
2
 
 
3
#include <Wire.h>
 
4
 
 
5
#define CTRL_REG1 0x20
 
6
#define CTRL_REG2 0x21
 
7
#define CTRL_REG3 0x22
 
8
#define CTRL_REG4 0x23
 
9
#define CTRL_REG5 0x24
 
10
 
 
11
int L3G4200D_Address = 104; //I2C address of the L3G4200D
 
12
 
 
13
int x;
 
14
int y;
 
15
int z;
 
16
 
 
17
 
 
18
void writeRegister(int deviceAddress, byte address, byte val) {
 
19
    Wire.beginTransmission(deviceAddress); // start transmission to device 
 
20
    Wire.write(address);       // send register address
 
21
    Wire.write(val);         // send value to write
 
22
    Wire.endTransmission();     // end transmission
 
23
}
 
24
 
 
25
int readRegister(int deviceAddress, byte address){
 
26
 
 
27
    int v;
 
28
    Wire.beginTransmission(deviceAddress);
 
29
    Wire.write(address); // register to read
 
30
    Wire.endTransmission();
 
31
 
 
32
    Wire.requestFrom(deviceAddress, 1); // read a byte
 
33
 
 
34
    while(!Wire.available()) {
 
35
        // waiting
 
36
    }
 
37
 
 
38
    v = Wire.read();
 
39
    return v;
 
40
}
 
41
 
 
42
void getGyroValues(){
 
43
 
 
44
  byte xMSB = readRegister(L3G4200D_Address, 0x29);
 
45
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
 
46
  x = ((xMSB << 8) | xLSB);
 
47
 
 
48
  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
 
49
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
 
50
  y = ((yMSB << 8) | yLSB);
 
51
 
 
52
  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
 
53
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
 
54
  z = ((zMSB << 8) | zLSB);
 
55
}
 
56
 
 
57
int setupL3G4200D(int scale){
 
58
  //From  Jim Lindblom of Sparkfun's code
 
59
 
 
60
  // Enable x, y, z and turn off power down:
 
61
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
 
62
 
 
63
  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
 
64
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
 
65
 
 
66
  // Configure CTRL_REG3 to generate data ready interrupt on INT2
 
67
  // No interrupts used on INT1, if you'd like to configure INT1
 
68
  // or INT2 otherwise, consult the datasheet:
 
69
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
 
70
 
 
71
  // CTRL_REG4 controls the full-scale range, among other things:
 
72
 
 
73
  if(scale == 250){
 
74
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
 
75
  }else if(scale == 500){
 
76
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
 
77
  }else{
 
78
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
 
79
  }
 
80
 
 
81
  // CTRL_REG5 controls high-pass filtering of outputs, use it
 
82
  // if you'd like:
 
83
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
 
84
}
 
85
 
 
86
 
 
87
void setup(){
 
88
 
 
89
  Wire.begin();
 
90
  Serial.begin(9600);
 
91
 
 
92
  Serial.println("starting up L3G4200D");
 
93
  setupL3G4200D(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec
 
94
 
 
95
  delay(1500); //wait for the sensor to be ready 
 
96
}
 
97
 
 
98
void loop(){
 
99
   getGyroValues();  // This will update x, y, and z with new values
 
100
 
 
101
  Serial.print("X:");
 
102
  Serial.print(x);
 
103
 
 
104
  Serial.print(" Y:");
 
105
  Serial.print(y);
 
106
 
 
107
  Serial.print(" Z:");
 
108
  Serial.println(z);
 
109
 
 
110
  delay(100); //Just here to slow down the serial to make it more readable
 
111
}