http://mbed.org/users/okini3939/notebook/daisen-edes/
Dependencies:
mbed
eDES2WD.cpp@1:3c8a6bd50779, 2012-02-23 (annotated)
- Committer:
- okini3939
- Date:
- Thu Feb 23 05:05:23 2012 +0000
- Revision:
- 1:3c8a6bd50779
- Parent:
-
0:141734449aeb
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|
| okini3939 |
0:141734449aeb
|
1
|
#include "mbed.h"
|
| okini3939 |
0:141734449aeb
|
2
|
#include "eDES2WD.h"
|
| okini3939 |
0:141734449aeb
|
3
|
|
| okini3939 |
0:141734449aeb
|
4
|
I2C i2c(p28, p27);
|
| okini3939 |
0:141734449aeb
|
5
|
|
| okini3939 |
0:141734449aeb
|
6
|
void eDES2WD_reset (int id) {
|
| okini3939 |
0:141734449aeb
|
7
|
char cmd[] = {1, I2C_WCMD_RESET};
|
| okini3939 |
0:141734449aeb
|
8
|
int adrs;
|
| okini3939 |
1:3c8a6bd50779
|
9
|
|
| okini3939 |
0:141734449aeb
|
10
|
if (0 < id){
|
| okini3939 |
0:141734449aeb
|
11
|
adrs = SUB_ADRS + (id - 1) * 2;
|
| okini3939 |
0:141734449aeb
|
12
|
} else {
|
| okini3939 |
0:141734449aeb
|
13
|
adrs = 0;
|
| okini3939 |
0:141734449aeb
|
14
|
}
|
| okini3939 |
0:141734449aeb
|
15
|
i2c.write(adrs, cmd, 2);
|
| okini3939 |
0:141734449aeb
|
16
|
}
|
| okini3939 |
0:141734449aeb
|
17
|
|
| okini3939 |
0:141734449aeb
|
18
|
void eDES2WD_motor (int id, int l_speed, int r_speed) {
|
| okini3939 |
0:141734449aeb
|
19
|
char cmd[] = {3, I2C_WCMD_MOTOR, 0, 0};
|
| okini3939 |
0:141734449aeb
|
20
|
int adrs;
|
| okini3939 |
1:3c8a6bd50779
|
21
|
|
| okini3939 |
0:141734449aeb
|
22
|
cmd[2] = l_speed;
|
| okini3939 |
0:141734449aeb
|
23
|
cmd[3] = r_speed;
|
| okini3939 |
0:141734449aeb
|
24
|
if (0 < id){
|
| okini3939 |
0:141734449aeb
|
25
|
adrs = SUB_ADRS + (id - 1) * 2;
|
| okini3939 |
0:141734449aeb
|
26
|
} else {
|
| okini3939 |
0:141734449aeb
|
27
|
adrs = 0;
|
| okini3939 |
0:141734449aeb
|
28
|
}
|
| okini3939 |
0:141734449aeb
|
29
|
i2c.write(adrs, cmd, 4);
|
| okini3939 |
0:141734449aeb
|
30
|
}
|
| okini3939 |
0:141734449aeb
|
31
|
|
| okini3939 |
0:141734449aeb
|
32
|
void eDES2WD_set_led (int id, int led, int sw) {
|
| okini3939 |
0:141734449aeb
|
33
|
char cmd[] = {3, I2C_WCMD_LED, 0, 0};
|
| okini3939 |
0:141734449aeb
|
34
|
int adrs;
|
| okini3939 |
1:3c8a6bd50779
|
35
|
|
| okini3939 |
0:141734449aeb
|
36
|
cmd[2] = led;
|
| okini3939 |
0:141734449aeb
|
37
|
cmd[3] = sw;
|
| okini3939 |
0:141734449aeb
|
38
|
if (0 < id){
|
| okini3939 |
0:141734449aeb
|
39
|
adrs = SUB_ADRS + (id - 1) * 2;
|
| okini3939 |
0:141734449aeb
|
40
|
} else {
|
| okini3939 |
0:141734449aeb
|
41
|
adrs = 0;
|
| okini3939 |
0:141734449aeb
|
42
|
}
|
| okini3939 |
0:141734449aeb
|
43
|
i2c.write(adrs, cmd, 4);
|
| okini3939 |
0:141734449aeb
|
44
|
}
|
| okini3939 |
1:3c8a6bd50779
|
45
|
|
| okini3939 |
1:3c8a6bd50779
|
46
|
unsigned int eDES2WD_get_sensor(int id, int cn, unsigned int *dat) {
|
| okini3939 |
1:3c8a6bd50779
|
47
|
char cmd[] = {2, I2C_RCMD_SENSOR, 0};
|
| okini3939 |
1:3c8a6bd50779
|
48
|
char recv[MAX_CN];
|
| okini3939 |
1:3c8a6bd50779
|
49
|
unsigned int d;
|
| okini3939 |
1:3c8a6bd50779
|
50
|
int adrs;
|
| okini3939 |
1:3c8a6bd50779
|
51
|
int nmax, n;
|
| okini3939 |
1:3c8a6bd50779
|
52
|
|
| okini3939 |
1:3c8a6bd50779
|
53
|
if (id < 1 || 9 < id) return(0);
|
| okini3939 |
1:3c8a6bd50779
|
54
|
adrs = SUB_ADRS + (id - 1) * 2;
|
| okini3939 |
1:3c8a6bd50779
|
55
|
nmax = cn < MAX_CN ? 2 : MAX_CN * 2 + 2;
|
| okini3939 |
1:3c8a6bd50779
|
56
|
|
| okini3939 |
1:3c8a6bd50779
|
57
|
cmd[2] = cn;
|
| okini3939 |
1:3c8a6bd50779
|
58
|
i2c.write(adrs, cmd, 3);
|
| okini3939 |
1:3c8a6bd50779
|
59
|
i2c.read(adrs | 1, recv, nmax);
|
| okini3939 |
1:3c8a6bd50779
|
60
|
if (nmax == 2){
|
| okini3939 |
1:3c8a6bd50779
|
61
|
d = recv[0] | (recv[1] << 8);
|
| okini3939 |
1:3c8a6bd50779
|
62
|
return d;
|
| okini3939 |
1:3c8a6bd50779
|
63
|
} else {
|
| okini3939 |
1:3c8a6bd50779
|
64
|
for (n = 0; n < MAX_CN; n++){
|
| okini3939 |
1:3c8a6bd50779
|
65
|
d = recv[0] | (recv[1] << 8);
|
| okini3939 |
1:3c8a6bd50779
|
66
|
dat[n] = d;
|
| okini3939 |
1:3c8a6bd50779
|
67
|
}
|
| okini3939 |
1:3c8a6bd50779
|
68
|
// gSUB_LED_G = recv[20];
|
| okini3939 |
1:3c8a6bd50779
|
69
|
// gSUB_LED_R = recv[21];
|
| okini3939 |
1:3c8a6bd50779
|
70
|
}
|
| okini3939 |
1:3c8a6bd50779
|
71
|
return 0;
|
| okini3939 |
1:3c8a6bd50779
|
72
|
}
|
| okini3939 |
1:3c8a6bd50779
|
73
|
|
| okini3939 |
1:3c8a6bd50779
|
74
|
int eDES2WD_get_lat(int id, int cno) {
|
| okini3939 |
1:3c8a6bd50779
|
75
|
char cmd[] = {2, I2C_RCMD_LAT, 0};
|
| okini3939 |
1:3c8a6bd50779
|
76
|
char recv[MAX_CN];
|
| okini3939 |
1:3c8a6bd50779
|
77
|
int adrs;
|
| okini3939 |
1:3c8a6bd50779
|
78
|
|
| okini3939 |
1:3c8a6bd50779
|
79
|
if (id < 1 || 9 < id) return(false);
|
| okini3939 |
1:3c8a6bd50779
|
80
|
if (MAX_CN - 1 < cno) return(false);
|
| okini3939 |
1:3c8a6bd50779
|
81
|
adrs = SUB_ADRS + (id - 1) * 2;
|
| okini3939 |
1:3c8a6bd50779
|
82
|
|
| okini3939 |
1:3c8a6bd50779
|
83
|
cmd[2] = cno;
|
| okini3939 |
1:3c8a6bd50779
|
84
|
i2c.write(adrs, cmd, 3);
|
| okini3939 |
1:3c8a6bd50779
|
85
|
i2c.read(adrs | 1, recv, 1);
|
| okini3939 |
1:3c8a6bd50779
|
86
|
return recv[0];
|
| okini3939 |
1:3c8a6bd50779
|
87
|
}
|
Suga koubou
DAISEN eDES 2WD