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