source: rtos_arduino/trunk/arduino_lib/libraries/Robot_Control/src/Sensors.cpp@ 136

Last change on this file since 136 was 136, checked in by ertl-honda, 8 years ago

ライブラリとOS及びベーシックなサンプルの追加．
File size: 5.9 KB
Line 
1#include "ArduinoRobot.h"
2#include "Multiplexer.h"
3#include "Wire.h"
4bool 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}
18int 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}
32void 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}
46void RobotControl::analogWrite(uint8_t port, uint8_t value){
47 if(port==TKD4)
48 ::analogWrite(port,value);
49}
50
51uint8_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}
82uint8_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}
102uint8_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}
118int* 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}
143int RobotControl::get_motorBoardData(uint8_t port){
144 return *parseMBDPort(port);
145}
146void RobotControl::set_motorBoardData(uint8_t port, int data){
147 *parseMBDPort(port)=data;
148}
149
150bool RobotControl::_digitalReadTopMux(uint8_t port){
151 uint8_t num=_portToTopMux(port);
152 return Multiplexer::getDigitalValueAt(num);
153}
154
155int RobotControl::_analogReadTopMux(uint8_t port){
156 uint8_t num=_portToTopMux(port);
157 return Multiplexer::getAnalogValueAt(num);
158}
159
160bool RobotControl::_digitalReadTopPin(uint8_t port){
161 return ::digitalRead(port);
162}
163int RobotControl::_analogReadTopPin(uint8_t port){
164 uint8_t aPin=_topDPortToAPort(port);
165 return ::analogRead(aPin);
166}
167void RobotControl::_digitalWriteTopPin(uint8_t port, bool value){
168 ::digitalWrite(port, value);
169}
170
171bool 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}
191int 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}
211void 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
222void 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
235int RobotControl::knobRead(){
236 return ::analogRead(POT);
237}
238
239int 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
253uint16_t RobotControl::compassRead(){
254 return Compass::getReading();
255}
256
257/*
258void 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}
265uint16_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}*/
Note: See TracBrowser for help on using the repository browser.