1 | #include "ArduinoRobot.h"
|
---|
2 | #include "Multiplexer.h"
|
---|
3 | #include "Wire.h"
|
---|
4 | bool RobotControl::digitalRead(uint8_t port){
|
---|
5 | uint8_t type=_getTypeCode(port);
|
---|
6 | switch(type){
|
---|
7 | case TYPE_TOP_TK:
|
---|
8 | return _digitalReadTopMux(port);
|
---|
9 | break;
|
---|
10 | case TYPE_TOP_TKD:
|
---|
11 | return _digitalReadTopPin(port);
|
---|
12 | break;
|
---|
13 | case TYPE_BOTTOM_TK:
|
---|
14 | return _requestDigitalRead(port);
|
---|
15 | break;
|
---|
16 | }
|
---|
17 | }
|
---|
18 | int RobotControl::analogRead(uint8_t port){
|
---|
19 | uint8_t type=_getTypeCode(port);
|
---|
20 | switch(type){
|
---|
21 | case TYPE_TOP_TK:
|
---|
22 | return _analogReadTopMux(port);
|
---|
23 | break;
|
---|
24 | case TYPE_TOP_TKD:
|
---|
25 | return _analogReadTopPin(port);
|
---|
26 | break;
|
---|
27 | case TYPE_BOTTOM_TK:
|
---|
28 | return _requestAnalogRead(port);
|
---|
29 | break;
|
---|
30 | }
|
---|
31 | }
|
---|
32 | void RobotControl::digitalWrite(uint8_t port, bool value){
|
---|
33 | uint8_t type=_getTypeCode(port);
|
---|
34 | switch(type){
|
---|
35 | case TYPE_TOP_TK:
|
---|
36 | //Top TKs can't use digitalWrite?
|
---|
37 | break;
|
---|
38 | case TYPE_TOP_TKD:
|
---|
39 | _digitalWriteTopPin(port, value);
|
---|
40 | break;
|
---|
41 | case TYPE_BOTTOM_TK:
|
---|
42 | _requestDigitalWrite(port, value);
|
---|
43 | break;
|
---|
44 | }
|
---|
45 | }
|
---|
46 | void RobotControl::analogWrite(uint8_t port, uint8_t value){
|
---|
47 | if(port==TKD4)
|
---|
48 | ::analogWrite(port,value);
|
---|
49 | }
|
---|
50 |
|
---|
51 | uint8_t RobotControl::_getTypeCode(uint8_t port){
|
---|
52 | switch(port){
|
---|
53 | case TK0:
|
---|
54 | case TK1:
|
---|
55 | case TK2:
|
---|
56 | case TK3:
|
---|
57 | case TK4:
|
---|
58 | case TK5:
|
---|
59 | case TK6:
|
---|
60 | case TK7:
|
---|
61 | return TYPE_TOP_TK;
|
---|
62 | break;
|
---|
63 |
|
---|
64 | case TKD0:
|
---|
65 | case TKD1:
|
---|
66 | case TKD2:
|
---|
67 | case TKD3:
|
---|
68 | case TKD4:
|
---|
69 | case TKD5:
|
---|
70 | case LED1:
|
---|
71 | return TYPE_TOP_TKD;
|
---|
72 | break;
|
---|
73 |
|
---|
74 | case B_TK1:
|
---|
75 | case B_TK2:
|
---|
76 | case B_TK3:
|
---|
77 | case B_TK4:
|
---|
78 | return TYPE_BOTTOM_TK;
|
---|
79 | break;
|
---|
80 | }
|
---|
81 | }
|
---|
82 | uint8_t RobotControl::_portToTopMux(uint8_t port){
|
---|
83 | switch(port){
|
---|
84 | case TK0:
|
---|
85 | return 0;
|
---|
86 | case TK1:
|
---|
87 | return 1;
|
---|
88 | case TK2:
|
---|
89 | return 2;
|
---|
90 | case TK3:
|
---|
91 | return 3;
|
---|
92 | case TK4:
|
---|
93 | return 4;
|
---|
94 | case TK5:
|
---|
95 | return 5;
|
---|
96 | case TK6:
|
---|
97 | return 6;
|
---|
98 | case TK7:
|
---|
99 | return 7;
|
---|
100 | }
|
---|
101 | }
|
---|
102 | uint8_t RobotControl::_topDPortToAPort(uint8_t port){
|
---|
103 | switch(port){
|
---|
104 | case TKD0:
|
---|
105 | return A1;
|
---|
106 | case TKD1:
|
---|
107 | return A2;
|
---|
108 | case TKD2:
|
---|
109 | return A3;
|
---|
110 | case TKD3:
|
---|
111 | return A4;
|
---|
112 | case TKD4:
|
---|
113 | return A7;
|
---|
114 | case TKD5:
|
---|
115 | return A11;
|
---|
116 | }
|
---|
117 | }
|
---|
118 | int* RobotControl::parseMBDPort(uint8_t port){
|
---|
119 | //Serial.println(port);
|
---|
120 | switch(port){
|
---|
121 | case B_TK1:
|
---|
122 | return &motorBoardData._B_TK1;
|
---|
123 | case B_TK2:
|
---|
124 | return &motorBoardData._B_TK2;
|
---|
125 | case B_TK3:
|
---|
126 | return &motorBoardData._B_TK3;
|
---|
127 | case B_TK4:
|
---|
128 | return &motorBoardData._B_TK4;
|
---|
129 |
|
---|
130 | /*
|
---|
131 | case B_IR0:
|
---|
132 | return &motorBoardData._B_IR0;
|
---|
133 | case B_IR1:
|
---|
134 | return &motorBoardData._B_IR1;
|
---|
135 | case B_IR2:
|
---|
136 | return &motorBoardData._B_IR2;
|
---|
137 | case B_IR3:
|
---|
138 | return &motorBoardData._B_IR3;
|
---|
139 | case B_IR4:
|
---|
140 | return &motorBoardData._B_IR4;*/
|
---|
141 | }
|
---|
142 | }
|
---|
143 | int RobotControl::get_motorBoardData(uint8_t port){
|
---|
144 | return *parseMBDPort(port);
|
---|
145 | }
|
---|
146 | void RobotControl::set_motorBoardData(uint8_t port, int data){
|
---|
147 | *parseMBDPort(port)=data;
|
---|
148 | }
|
---|
149 |
|
---|
150 | bool RobotControl::_digitalReadTopMux(uint8_t port){
|
---|
151 | uint8_t num=_portToTopMux(port);
|
---|
152 | return Multiplexer::getDigitalValueAt(num);
|
---|
153 | }
|
---|
154 |
|
---|
155 | int RobotControl::_analogReadTopMux(uint8_t port){
|
---|
156 | uint8_t num=_portToTopMux(port);
|
---|
157 | return Multiplexer::getAnalogValueAt(num);
|
---|
158 | }
|
---|
159 |
|
---|
160 | bool RobotControl::_digitalReadTopPin(uint8_t port){
|
---|
161 | return ::digitalRead(port);
|
---|
162 | }
|
---|
163 | int RobotControl::_analogReadTopPin(uint8_t port){
|
---|
164 | uint8_t aPin=_topDPortToAPort(port);
|
---|
165 | return ::analogRead(aPin);
|
---|
166 | }
|
---|
167 | void RobotControl::_digitalWriteTopPin(uint8_t port, bool value){
|
---|
168 | ::digitalWrite(port, value);
|
---|
169 | }
|
---|
170 |
|
---|
171 | bool RobotControl::_requestDigitalRead(uint8_t port){
|
---|
172 | messageOut.writeByte(COMMAND_DIGITAL_READ);
|
---|
173 | messageOut.writeByte(port);//B_TK1 - B_TK4
|
---|
174 | messageOut.sendData();
|
---|
175 | delay(10);
|
---|
176 | if(messageIn.receiveData()){
|
---|
177 | //Serial.println("*************");
|
---|
178 | uint8_t cmd=messageIn.readByte();
|
---|
179 | //Serial.print("cmd: ");
|
---|
180 | //Serial.println(cmd);
|
---|
181 | if(!(cmd==COMMAND_DIGITAL_READ_RE))
|
---|
182 | return false;
|
---|
183 |
|
---|
184 | uint8_t pt=messageIn.readByte(); //Bottom TK port codename
|
---|
185 | //Serial.print("pt: ");
|
---|
186 | //Serial.println(pt);
|
---|
187 | set_motorBoardData(pt,messageIn.readByte());
|
---|
188 | return get_motorBoardData(port);
|
---|
189 | }
|
---|
190 | }
|
---|
191 | int RobotControl::_requestAnalogRead(uint8_t port){
|
---|
192 | messageOut.writeByte(COMMAND_ANALOG_READ);
|
---|
193 | messageOut.writeByte(port);//B_TK1 - B_TK4
|
---|
194 | messageOut.sendData();
|
---|
195 | delay(10);
|
---|
196 | if(messageIn.receiveData()){
|
---|
197 | uint8_t cmd=messageIn.readByte();
|
---|
198 | //Serial.println("*************");
|
---|
199 | //Serial.print("cmd: ");
|
---|
200 | //Serial.println(cmd);
|
---|
201 | if(!(cmd==COMMAND_ANALOG_READ_RE))
|
---|
202 | return false;
|
---|
203 |
|
---|
204 | uint8_t pt=messageIn.readByte();
|
---|
205 | //Serial.print("pt: ");
|
---|
206 | //Serial.println(pt);
|
---|
207 | set_motorBoardData(pt,messageIn.readInt());
|
---|
208 | return get_motorBoardData(port);
|
---|
209 | }
|
---|
210 | }
|
---|
211 | void RobotControl::_requestDigitalWrite(uint8_t selector, uint8_t value){
|
---|
212 | messageOut.writeByte(COMMAND_DIGITAL_WRITE);
|
---|
213 | messageOut.writeByte(selector);//B_TK1 - B_TK4
|
---|
214 | messageOut.writeByte(value);
|
---|
215 | messageOut.sendData();
|
---|
216 | }
|
---|
217 |
|
---|
218 |
|
---|
219 |
|
---|
220 |
|
---|
221 |
|
---|
222 | void RobotControl::updateIR(){
|
---|
223 | messageOut.writeByte(COMMAND_READ_IR);
|
---|
224 | messageOut.sendData();
|
---|
225 | delay(10);
|
---|
226 | if(messageIn.receiveData()){
|
---|
227 | if(messageIn.readByte()==COMMAND_READ_IR_RE){
|
---|
228 | for(int i=0;i<5;i++){
|
---|
229 | IRarray[i]=messageIn.readInt();
|
---|
230 | }
|
---|
231 | }
|
---|
232 | }
|
---|
233 | }
|
---|
234 |
|
---|
235 | int RobotControl::knobRead(){
|
---|
236 | return ::analogRead(POT);
|
---|
237 | }
|
---|
238 |
|
---|
239 | int RobotControl::trimRead(){
|
---|
240 | messageOut.writeByte(COMMAND_READ_TRIM);
|
---|
241 | messageOut.sendData();
|
---|
242 | delay(10);
|
---|
243 | if(messageIn.receiveData()){
|
---|
244 | uint8_t cmd=messageIn.readByte();
|
---|
245 | if(!(cmd==COMMAND_READ_TRIM_RE))
|
---|
246 | return false;
|
---|
247 |
|
---|
248 | uint16_t pt=messageIn.readInt();
|
---|
249 | return pt;
|
---|
250 | }
|
---|
251 | }
|
---|
252 |
|
---|
253 | uint16_t RobotControl::compassRead(){
|
---|
254 | return Compass::getReading();
|
---|
255 | }
|
---|
256 |
|
---|
257 | /*
|
---|
258 | void RobotControl::beginUR(uint8_t pinTrigger, uint8_t pinEcho){
|
---|
259 | pinTrigger_UR=pinTrigger;
|
---|
260 | pinEcho_UR=pinEcho;
|
---|
261 |
|
---|
262 | pinMode(pinEcho_UR, INPUT);
|
---|
263 | pinMode(pinTrigger_UR, OUTPUT);
|
---|
264 | }
|
---|
265 | uint16_t RobotControl::getDistance(){
|
---|
266 | digitalWrite(pinTrigger_UR, LOW); // Set the trigger pin to low for 2uS
|
---|
267 | delayMicroseconds(2);
|
---|
268 | digitalWrite(pinTrigger_UR, HIGH); // Send a 10uS high to trigger ranging
|
---|
269 | delayMicroseconds(10);
|
---|
270 | digitalWrite(pinTrigger_UR, LOW); // Send pin low again
|
---|
271 | uint16_t distance = pulseIn(pinEcho_UR, HIGH); // Read in times pulse
|
---|
272 | distance= distance/58; // Calculate distance from time of pulse
|
---|
273 | return distance;
|
---|
274 | }*/
|
---|