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
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
|
#include "ArduinoRobot.h"
#include "Multiplexer.h"
#include "Wire.h"
bool RobotControl::digitalRead(uint8_t port){
uint8_t type=_getTypeCode(port);
switch(type){
case TYPE_TOP_TK:
return _digitalReadTopMux(port);
break;
case TYPE_TOP_TKD:
return _digitalReadTopPin(port);
break;
case TYPE_BOTTOM_TK:
return _requestDigitalRead(port);
break;
}
}
int RobotControl::analogRead(uint8_t port){
uint8_t type=_getTypeCode(port);
switch(type){
case TYPE_TOP_TK:
return _analogReadTopMux(port);
break;
case TYPE_TOP_TKD:
return _analogReadTopPin(port);
break;
case TYPE_BOTTOM_TK:
return _requestAnalogRead(port);
break;
}
}
void RobotControl::digitalWrite(uint8_t port, bool value){
uint8_t type=_getTypeCode(port);
switch(type){
case TYPE_TOP_TK:
//Top TKs can't use digitalWrite?
break;
case TYPE_TOP_TKD:
_digitalWriteTopPin(port, value);
break;
case TYPE_BOTTOM_TK:
_requestDigitalWrite(port, value);
break;
}
}
void RobotControl::analogWrite(uint8_t port, uint8_t value){
if(port==TKD4)
::analogWrite(port,value);
}
uint8_t RobotControl::_getTypeCode(uint8_t port){
switch(port){
case TK0:
case TK1:
case TK2:
case TK3:
case TK4:
case TK5:
case TK6:
case TK7:
return TYPE_TOP_TK;
break;
case TKD0:
case TKD1:
case TKD2:
case TKD3:
case TKD4:
case TKD5:
case LED1:
return TYPE_TOP_TKD;
break;
case B_TK1:
case B_TK2:
case B_TK3:
case B_TK4:
return TYPE_BOTTOM_TK;
break;
}
}
uint8_t RobotControl::_portToTopMux(uint8_t port){
switch(port){
case TK0:
return 0;
case TK1:
return 1;
case TK2:
return 2;
case TK3:
return 3;
case TK4:
return 4;
case TK5:
return 5;
case TK6:
return 6;
case TK7:
return 7;
}
}
uint8_t RobotControl::_topDPortToAPort(uint8_t port){
switch(port){
case TKD0:
return A1;
case TKD1:
return A2;
case TKD2:
return A3;
case TKD3:
return A4;
case TKD4:
return A7;
case TKD5:
return A11;
}
}
int* RobotControl::parseMBDPort(uint8_t port){
//Serial.println(port);
switch(port){
case B_TK1:
return &motorBoardData._B_TK1;
case B_TK2:
return &motorBoardData._B_TK2;
case B_TK3:
return &motorBoardData._B_TK3;
case B_TK4:
return &motorBoardData._B_TK4;
/*
case B_IR0:
return &motorBoardData._B_IR0;
case B_IR1:
return &motorBoardData._B_IR1;
case B_IR2:
return &motorBoardData._B_IR2;
case B_IR3:
return &motorBoardData._B_IR3;
case B_IR4:
return &motorBoardData._B_IR4;*/
}
}
int RobotControl::get_motorBoardData(uint8_t port){
return *parseMBDPort(port);
}
void RobotControl::set_motorBoardData(uint8_t port, int data){
*parseMBDPort(port)=data;
}
bool RobotControl::_digitalReadTopMux(uint8_t port){
uint8_t num=_portToTopMux(port);
return Multiplexer::getDigitalValueAt(num);
}
int RobotControl::_analogReadTopMux(uint8_t port){
uint8_t num=_portToTopMux(port);
return Multiplexer::getAnalogValueAt(num);
}
bool RobotControl::_digitalReadTopPin(uint8_t port){
return ::digitalRead(port);
}
int RobotControl::_analogReadTopPin(uint8_t port){
uint8_t aPin=_topDPortToAPort(port);
return ::analogRead(aPin);
}
void RobotControl::_digitalWriteTopPin(uint8_t port, bool value){
::digitalWrite(port, value);
}
bool RobotControl::_requestDigitalRead(uint8_t port){
messageOut.writeByte(COMMAND_DIGITAL_READ);
messageOut.writeByte(port);//B_TK1 - B_TK4
messageOut.sendData();
delay(10);
if(messageIn.receiveData()){
//Serial.println("*************");
uint8_t cmd=messageIn.readByte();
//Serial.print("cmd: ");
//Serial.println(cmd);
if(!(cmd==COMMAND_DIGITAL_READ_RE))
return false;
uint8_t pt=messageIn.readByte(); //Bottom TK port codename
//Serial.print("pt: ");
//Serial.println(pt);
set_motorBoardData(pt,messageIn.readByte());
return get_motorBoardData(port);
}
}
int RobotControl::_requestAnalogRead(uint8_t port){
messageOut.writeByte(COMMAND_ANALOG_READ);
messageOut.writeByte(port);//B_TK1 - B_TK4
messageOut.sendData();
delay(10);
if(messageIn.receiveData()){
uint8_t cmd=messageIn.readByte();
//Serial.println("*************");
//Serial.print("cmd: ");
//Serial.println(cmd);
if(!(cmd==COMMAND_ANALOG_READ_RE))
return false;
uint8_t pt=messageIn.readByte();
//Serial.print("pt: ");
//Serial.println(pt);
set_motorBoardData(pt,messageIn.readInt());
return get_motorBoardData(port);
}
}
void RobotControl::_requestDigitalWrite(uint8_t selector, uint8_t value){
messageOut.writeByte(COMMAND_DIGITAL_WRITE);
messageOut.writeByte(selector);//B_TK1 - B_TK4
messageOut.writeByte(value);
messageOut.sendData();
}
void RobotControl::updateIR(){
messageOut.writeByte(COMMAND_READ_IR);
messageOut.sendData();
delay(10);
if(messageIn.receiveData()){
if(messageIn.readByte()==COMMAND_READ_IR_RE){
for(int i=0;i<5;i++){
IRarray[i]=messageIn.readInt();
}
}
}
}
int RobotControl::knobRead(){
return ::analogRead(POT);
}
int RobotControl::trimRead(){
messageOut.writeByte(COMMAND_READ_TRIM);
messageOut.sendData();
delay(10);
if(messageIn.receiveData()){
uint8_t cmd=messageIn.readByte();
if(!(cmd==COMMAND_READ_TRIM_RE))
return false;
uint16_t pt=messageIn.readInt();
return pt;
}
}
uint16_t RobotControl::compassRead(){
return Compass::getReading();
}
/*
void RobotControl::beginUR(uint8_t pinTrigger, uint8_t pinEcho){
pinTrigger_UR=pinTrigger;
pinEcho_UR=pinEcho;
pinMode(pinEcho_UR, INPUT);
pinMode(pinTrigger_UR, OUTPUT);
}
uint16_t RobotControl::getDistance(){
digitalWrite(pinTrigger_UR, LOW); // Set the trigger pin to low for 2uS
delayMicroseconds(2);
digitalWrite(pinTrigger_UR, HIGH); // Send a 10uS high to trigger ranging
delayMicroseconds(10);
digitalWrite(pinTrigger_UR, LOW); // Send pin low again
uint16_t distance = pulseIn(pinEcho_UR, HIGH); // Read in times pulse
distance= distance/58; // Calculate distance from time of pulse
return distance;
}*/
|
