/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: Tim Marston
  • Date: 2012-11-28 23:18:38 UTC
  • Revision ID: tim@ed.am-20121128231838-r2t1dyawkzb6nxhe
initial commit, and a couple of receiver test programs

Show diffs side-by-side

added added

removed removed

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
 
}