forked from alliekins/BalancingRobot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
gyro.c
146 lines (106 loc) · 2.79 KB
/
gyro.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#include <avr/io.h>
#include <stdint.h>
#include <util/delay.h>
#include <stdio.h>
#include "debug.h"
#include "adc.h"
#include "gyro.h"
#include "spi.h"
#include "usb_serial.h"
//write to multiple registers on the device.
int L3G4200D_write(char* buffer, unsigned char address, int length){
SS_ON;
unsigned char address_byte=(0<<7); //specify that we are writing
if (address < 0x0E || address > 0x38){//check that the address is in range
return -1;
}
if (length > 1){
address_byte |= (1<<6); //specify to advance the pointer
}
address_byte += address;
SPI_MasterTransmit(address_byte);
for (int i=0;i<length;i++){
SPI_MasterTransmit(buffer[i]);
}
SS_OFF;
return length;
}
//write to one register on the device
int L3G4200D_writec(unsigned char address, char tx){
SS_ON;
unsigned char address_byte=(0<<7); //specify that we are writing
if (address < 0x0E || address > 0x38){//check that the address is in range
SS_OFF;
return 0;
}
address_byte += address;
SPI_MasterTransmit(address_byte);
SPI_MasterTransmit(tx);
SS_OFF;
return 1;
}
//initialize the device
int L3G4200D_init(void ){
//check the device id
if (L3G4200D_readc(0x0F) != 0xD3) return 0;
//initialize device with all axes reporting at 800hz, low-pass @ 35 hz)
if (!L3G4200D_writec(0x20,0b00011111)) return 0;
//low pass at 30Hz
if (!L3G4200D_writec(0x21,0b00000001)) return 0;
return 1;
}
//read one register from the device
char L3G4200D_readc( unsigned char address){
SS_ON;
unsigned char address_byte=(1<<7); //specify that we are reading
if (address < 0x0E || address > 0x38){
return -1;
}
address_byte += address;
SPI_MasterTransmit(address_byte);
char ret= SPI_MasterTransmit(0xFF); //doesn't matter what we transmit
SS_OFF;
return ret;
}
//read multiple registers from the device
int L3G4200D_read(char* buffer, unsigned char address, int length){
SS_ON;
unsigned char address_byte=(1<<7); //specify that we are reading
if (address < 0x0E || address > 0x38){
return -1;
}
if (length > 1){
address_byte |= (1<<6); //specify to advance the pointer
}
address_byte += address;
SPI_MasterTransmit(address_byte);
for (int i=0;i<length;i++){
buffer[i]=SPI_MasterTransmit(0x00); //doesn't matter what we transmit
}
SS_OFF;
return length;
}
//get the x axis count
int L3G4200D_getX(void){
char buf [2];
L3G4200D_read(buf, 0x28, 2); //read the two bytes into buf;
int ret=buf[0];
ret += buf[1] << 8;
return ret;
}
//get the y axis count
int L3G4200D_getY(void){
char buf [2];
L3G4200D_read(buf, 0x2A, 2); //read the two bytes into buf;
int ret=buf[0];
ret += buf[1] << 8;
return ret;
}
//get the z axis count
int L3G4200D_getZ(void){
char buf [2];
L3G4200D_read(buf, 0x2C, 2); //read the two bytes into buf;
int ret=buf[0];
ret += buf[1] << 8;
return ret;
}