source: rtos_arduino/trunk/arduino_lib/libraries/Robot_Motor/src/ArduinoRobotMotorBoard.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 "ArduinoRobotMotorBoard.h"
2#include "EasyTransfer2.h"
3#include "Multiplexer.h"
4#include "LineFollow.h"
5
6RobotMotorBoard::RobotMotorBoard(){
7 //LineFollow::LineFollow();
8}
9/*void RobotMotorBoard::beginIRReceiver(){
10 IRrecv::enableIRIn();
11}*/
12void RobotMotorBoard::begin(){
13 //initialze communication
14 Serial1.begin(9600);
15 messageIn.begin(&Serial1);
16 messageOut.begin(&Serial1);
17
18 //init MUX
19 uint8_t MuxPins[]={MUXA,MUXB,MUXC};
20 this->IRs.begin(MuxPins,MUX_IN,3);
21 pinMode(MUXI,INPUT);
22 digitalWrite(MUXI,LOW);
23
24 isPaused=false;
25}
26
27void RobotMotorBoard::process(){
28 if(isPaused)return;//skip process if the mode is paused
29
30 if(mode==MODE_SIMPLE){
31 //Serial.println("s");
32 //do nothing? Simple mode is just about getting commands
33 }else if(mode==MODE_LINE_FOLLOW){
34 //do line following stuff here.
35 LineFollow::runLineFollow();
36 }else if(mode==MODE_ADJUST_MOTOR){
37 //Serial.println('a');
38 //motorAdjustment=analogRead(POT);
39 //setSpeed(255,255);
40 //delay(100);
41 }
42}
43void RobotMotorBoard::pauseMode(bool onOff){
44 if(onOff){
45 isPaused=true;
46 }else{
47 isPaused=false;
48 }
49 stopCurrentActions();
50
51}
52void RobotMotorBoard::parseCommand(){
53 uint8_t modeName;
54 uint8_t codename;
55 int value;
56 int speedL;
57 int speedR;
58 if(this->messageIn.receiveData()){
59 //Serial.println("data received");
60 uint8_t command=messageIn.readByte();
61 //Serial.println(command);
62 switch(command){
63 case COMMAND_SWITCH_MODE:
64 modeName=messageIn.readByte();
65 setMode(modeName);
66 break;
67 case COMMAND_RUN:
68 if(mode==MODE_LINE_FOLLOW)break;//in follow line mode, the motor does not follow commands
69 speedL=messageIn.readInt();
70 speedR=messageIn.readInt();
71 motorsWrite(speedL,speedR);
72 break;
73 case COMMAND_MOTORS_STOP:
74 motorsStop();
75 break;
76 case COMMAND_ANALOG_WRITE:
77 codename=messageIn.readByte();
78 value=messageIn.readInt();
79 _analogWrite(codename,value);
80 break;
81 case COMMAND_DIGITAL_WRITE:
82 codename=messageIn.readByte();
83 value=messageIn.readByte();
84 _digitalWrite(codename,value);
85 break;
86 case COMMAND_ANALOG_READ:
87 codename=messageIn.readByte();
88 _analogRead(codename);
89 break;
90 case COMMAND_DIGITAL_READ:
91 codename=messageIn.readByte();
92 _digitalRead(codename);
93 break;
94 case COMMAND_READ_IR:
95 _readIR();
96 break;
97 case COMMAND_READ_TRIM:
98 _readTrim();
99 break;
100 case COMMAND_PAUSE_MODE:
101 pauseMode(messageIn.readByte());//onOff state
102 break;
103 case COMMAND_LINE_FOLLOW_CONFIG:
104 LineFollow::config(
105 messageIn.readByte(), //KP
106 messageIn.readByte(), //KD
107 messageIn.readByte(), //robotSpeed
108 messageIn.readByte() //IntegrationTime
109 );
110 break;
111 }
112 }
113 //delay(5);
114}
115uint8_t RobotMotorBoard::parseCodename(uint8_t codename){
116 switch(codename){
117 case B_TK1:
118 return TK1;
119 case B_TK2:
120 return TK2;
121 case B_TK3:
122 return TK3;
123 case B_TK4:
124 return TK4;
125 }
126}
127uint8_t RobotMotorBoard::codenameToAPin(uint8_t codename){
128 switch(codename){
129 case B_TK1:
130 return A0;
131 case B_TK2:
132 return A1;
133 case B_TK3:
134 return A6;
135 case B_TK4:
136 return A11;
137 }
138}
139
140void RobotMotorBoard::setMode(uint8_t mode){
141 if(mode==MODE_LINE_FOLLOW){
142 LineFollow::calibIRs();
143 }
144 /*if(mode==SET_MOTOR_ADJUSTMENT){
145 save_motor_adjustment_to_EEPROM();
146 }
147 */
148 /*if(mode==MODE_IR_CONTROL){
149 beginIRReceiver();
150 }*/
151 this->mode=mode;
152 //stopCurrentActions();//If line following, this should stop the motors
153}
154
155void RobotMotorBoard::stopCurrentActions(){
156 motorsStop();
157 //motorsWrite(0,0);
158}
159
160void RobotMotorBoard::motorsWrite(int speedL, int speedR){
161 /*Serial.print(speedL);
162 Serial.print(" ");
163 Serial.println(speedR);*/
164 //motor adjustment, using percentage
165 _refreshMotorAdjustment();
166
167 if(motorAdjustment<0){
168 speedR*=(1+motorAdjustment);
169 }else{
170 speedL*=(1-motorAdjustment);
171 }
172
173 if(speedR>0){
174 analogWrite(IN_A1,speedR);
175 analogWrite(IN_A2,0);
176 }else{
177 analogWrite(IN_A1,0);
178 analogWrite(IN_A2,-speedR);
179 }
180
181 if(speedL>0){
182 analogWrite(IN_B1,speedL);
183 analogWrite(IN_B2,0);
184 }else{
185 analogWrite(IN_B1,0);
186 analogWrite(IN_B2,-speedL);
187 }
188}
189void RobotMotorBoard::motorsWritePct(int speedLpct, int speedRpct){
190 //speedLpct, speedRpct ranges from -100 to 100
191 motorsWrite(speedLpct*2.55,speedRpct*2.55);
192}
193void RobotMotorBoard::motorsStop(){
194 analogWrite(IN_A1,255);
195 analogWrite(IN_A2,255);
196
197 analogWrite(IN_B1,255);
198 analogWrite(IN_B2,255);
199}
200
201
202/*
203*
204*
205* Input and Output ports
206*
207*
208*/
209void RobotMotorBoard::_digitalWrite(uint8_t codename,bool value){
210 uint8_t pin=parseCodename(codename);
211 digitalWrite(pin,value);
212}
213void RobotMotorBoard::_analogWrite(uint8_t codename,int value){
214 //There's no PWM available on motor board
215}
216void RobotMotorBoard::_digitalRead(uint8_t codename){
217 uint8_t pin=parseCodename(codename);
218 bool value=digitalRead(pin);
219 messageOut.writeByte(COMMAND_DIGITAL_READ_RE);
220 messageOut.writeByte(codename);
221 messageOut.writeByte(value);
222 messageOut.sendData();
223}
224void RobotMotorBoard::_analogRead(uint8_t codename){
225 uint8_t pin=codenameToAPin(codename);
226 int value=analogRead(pin);
227 messageOut.writeByte(COMMAND_ANALOG_READ_RE);
228 messageOut.writeByte(codename);
229 messageOut.writeInt(value);
230 messageOut.sendData();
231}
232int RobotMotorBoard::IRread(uint8_t num){
233 return _IRread(num-1); //To make consistant with the pins labeled on the board
234}
235
236int RobotMotorBoard::_IRread(uint8_t num){
237 IRs.selectPin(num);
238 return IRs.getAnalogValue();
239}
240
241
242void RobotMotorBoard::_readIR(){
243 int value;
244 messageOut.writeByte(COMMAND_READ_IR_RE);
245 for(int i=0;i<5;i++){
246 value=_IRread(i);
247 messageOut.writeInt(value);
248 }
249 messageOut.sendData();
250}
251
252void RobotMotorBoard::_readTrim(){
253 int value=analogRead(TRIM);
254 messageOut.writeByte(COMMAND_READ_TRIM_RE);
255 messageOut.writeInt(value);
256 messageOut.sendData();
257}
258
259void RobotMotorBoard::_refreshMotorAdjustment(){
260 motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
261}
262
263void RobotMotorBoard::reportActionDone(){
264 setMode(MODE_SIMPLE);
265 messageOut.writeByte(COMMAND_ACTION_DONE);
266 messageOut.sendData();
267}
268
269RobotMotorBoard RobotMotor=RobotMotorBoard();
Note: See TracBrowser for help on using the repository browser.