-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathproxi2c_generated.ino
183 lines (181 loc) · 4.71 KB
/
proxi2c_generated.ino
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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
#include "proxi2c_generated.h"
enum i2c_registers
{
REG_MOVE=1,
REG_ROTATE=2,
REG_GOTO=3,
REG_GETPOSITION=4,
REG_SETODO=5,
REG_SETDISTKPKD=6,
REG_GETDISTKPKD=7,
REG_SETROTKPKD=8,
REG_GETROTKPKD=9,
REG_GETTICKS=10,
REG_SETTICKS=11,
REG_GETULTRASOUNDS=12,
REG_GETSTATUS=13,
REG_SETSERVO=14,
REG_RATATOUILLE=15,
REG_LAUNCHNET=16,
REG_SETTICKRATIO=17,
REG_SETBRAS=18,
};
void i2c_runCmd(int i2c_reg,uint8_t * data_in,uint8_t * data_out,uint8_t * data_out_len)
{
switch(i2c_reg) {
case REG_MOVE :
Serial.println("cmd_move");
cmd_move(
MAKEUINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEINT8_T(data_in[4])
);
break;
case REG_ROTATE :
Serial.println("cmd_rotate");
cmd_rotate(
MAKEDOUBLE(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEINT8_T(data_in[4])
);
break;
case REG_GOTO :
Serial.println("cmd_goto");
cmd_goto(
MAKEINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEINT32_T(data_in[4],data_in[5],data_in[6],data_in[7]),
MAKEINT32_T(data_in[8],data_in[9],data_in[10],data_in[11])
);
break;
case REG_GETPOSITION :
Serial.println("cmd_getPosition");
int32_t ret_getPosition_x;
int32_t ret_getPosition_y;
double ret_getPosition_angle;
cmd_getPosition(
&ret_getPosition_x,
&ret_getPosition_y,
&ret_getPosition_angle
);
SPLITINT32_T(ret_getPosition_x,data_out,0);
SPLITINT32_T(ret_getPosition_y,data_out,4);
SPLITDOUBLE(ret_getPosition_angle,data_out,8);
*data_out_len = 12;
break;
case REG_SETODO :
Serial.println("cmd_setOdo");
cmd_setOdo(
MAKEINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEINT32_T(data_in[4],data_in[5],data_in[6],data_in[7]),
MAKEDOUBLE(data_in[8],data_in[9],data_in[10],data_in[11]),
MAKEINT8_T(data_in[12])
);
break;
case REG_SETDISTKPKD :
Serial.println("cmd_setDistKpKd");
cmd_setDistKpKd(
MAKEUINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEUINT32_T(data_in[4],data_in[5],data_in[6],data_in[7])
);
break;
case REG_GETDISTKPKD :
Serial.println("cmd_getDistKpKd");
uint32_t ret_getDistKpKd_kp;
uint32_t ret_getDistKpKd_kd;
cmd_getDistKpKd(
&ret_getDistKpKd_kp,
&ret_getDistKpKd_kd
);
SPLITUINT32_T(ret_getDistKpKd_kp,data_out,0);
SPLITUINT32_T(ret_getDistKpKd_kd,data_out,4);
*data_out_len = 8;
break;
case REG_SETROTKPKD :
Serial.println("cmd_setRotKpKd");
cmd_setRotKpKd(
MAKEUINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEUINT32_T(data_in[4],data_in[5],data_in[6],data_in[7])
);
break;
case REG_GETROTKPKD :
Serial.println("cmd_getRotKpKd");
uint32_t ret_getRotKpKd_kp;
uint32_t ret_getRotKpKd_kd;
cmd_getRotKpKd(
&ret_getRotKpKd_kp,
&ret_getRotKpKd_kd
);
SPLITUINT32_T(ret_getRotKpKd_kp,data_out,0);
SPLITUINT32_T(ret_getRotKpKd_kd,data_out,4);
*data_out_len = 8;
break;
case REG_GETTICKS :
Serial.println("cmd_getTicks");
int32_t ret_getTicks_left;
int32_t ret_getTicks_right;
cmd_getTicks(
&ret_getTicks_left,
&ret_getTicks_right
);
SPLITINT32_T(ret_getTicks_left,data_out,0);
SPLITINT32_T(ret_getTicks_right,data_out,4);
*data_out_len = 8;
break;
case REG_SETTICKS :
Serial.println("cmd_setTicks");
cmd_setTicks(
MAKEINT32_T(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEINT32_T(data_in[4],data_in[5],data_in[6],data_in[7])
);
break;
case REG_GETULTRASOUNDS :
Serial.println("cmd_getUltrasounds");
int32_t ret_getUltrasounds_dist;
cmd_getUltrasounds(
&ret_getUltrasounds_dist
);
SPLITINT32_T(ret_getUltrasounds_dist,data_out,0);
*data_out_len = 4;
break;
case REG_GETSTATUS :
Serial.println("cmd_getStatus");
int8_t ret_getStatus_bfr;
int8_t ret_getStatus_bfl;
int8_t ret_getStatus_bbr;
int8_t ret_getStatus_bbl;
int8_t ret_getStatus_cmdhack;
cmd_getStatus(
&ret_getStatus_bfr,
&ret_getStatus_bfl,
&ret_getStatus_bbr,
&ret_getStatus_bbl,
&ret_getStatus_cmdhack
);
SPLITINT8_T(ret_getStatus_bfr,data_out,0);
SPLITINT8_T(ret_getStatus_bfl,data_out,1);
SPLITINT8_T(ret_getStatus_bbr,data_out,2);
SPLITINT8_T(ret_getStatus_bbl,data_out,3);
SPLITINT8_T(ret_getStatus_cmdhack,data_out,4);
*data_out_len = 5;
break;
case REG_SETSERVO :
Serial.println("cmd_setServo");
cmd_setServo(
MAKEUINT8_T(data_in[0]),
MAKEUINT8_T(data_in[1])
);
break;
case REG_SETTICKRATIO :
Serial.println("cmd_setTickRatio");
cmd_setTickRatio(
MAKEDOUBLE(data_in[0],data_in[1],data_in[2],data_in[3]),
MAKEDOUBLE(data_in[4],data_in[5],data_in[6],data_in[7])
);
break;
case REG_SETBRAS :
Serial.println("cmd_setBras");
cmd_setBras(
MAKEINT8_T(data_in[0]),
MAKEINT8_T(data_in[1])
);
break;
};
};