1 | #include "ArduinoRobotMotorBoard.h"
|
---|
2 | #include "EasyTransfer2.h"
|
---|
3 | #include "Multiplexer.h"
|
---|
4 | #include "LineFollow.h"
|
---|
5 |
|
---|
6 | RobotMotorBoard::RobotMotorBoard(){
|
---|
7 | //LineFollow::LineFollow();
|
---|
8 | }
|
---|
9 | /*void RobotMotorBoard::beginIRReceiver(){
|
---|
10 | IRrecv::enableIRIn();
|
---|
11 | }*/
|
---|
12 | void 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 |
|
---|
27 | void 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 | }
|
---|
43 | void RobotMotorBoard::pauseMode(bool onOff){
|
---|
44 | if(onOff){
|
---|
45 | isPaused=true;
|
---|
46 | }else{
|
---|
47 | isPaused=false;
|
---|
48 | }
|
---|
49 | stopCurrentActions();
|
---|
50 |
|
---|
51 | }
|
---|
52 | void 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 | }
|
---|
115 | uint8_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 | }
|
---|
127 | uint8_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 |
|
---|
140 | void 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 |
|
---|
155 | void RobotMotorBoard::stopCurrentActions(){
|
---|
156 | motorsStop();
|
---|
157 | //motorsWrite(0,0);
|
---|
158 | }
|
---|
159 |
|
---|
160 | void 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 | }
|
---|
189 | void RobotMotorBoard::motorsWritePct(int speedLpct, int speedRpct){
|
---|
190 | //speedLpct, speedRpct ranges from -100 to 100
|
---|
191 | motorsWrite(speedLpct*2.55,speedRpct*2.55);
|
---|
192 | }
|
---|
193 | void 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 | */
|
---|
209 | void RobotMotorBoard::_digitalWrite(uint8_t codename,bool value){
|
---|
210 | uint8_t pin=parseCodename(codename);
|
---|
211 | digitalWrite(pin,value);
|
---|
212 | }
|
---|
213 | void RobotMotorBoard::_analogWrite(uint8_t codename,int value){
|
---|
214 | //There's no PWM available on motor board
|
---|
215 | }
|
---|
216 | void 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 | }
|
---|
224 | void 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 | }
|
---|
232 | int RobotMotorBoard::IRread(uint8_t num){
|
---|
233 | return _IRread(num-1); //To make consistant with the pins labeled on the board
|
---|
234 | }
|
---|
235 |
|
---|
236 | int RobotMotorBoard::_IRread(uint8_t num){
|
---|
237 | IRs.selectPin(num);
|
---|
238 | return IRs.getAnalogValue();
|
---|
239 | }
|
---|
240 |
|
---|
241 |
|
---|
242 | void 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 |
|
---|
252 | void RobotMotorBoard::_readTrim(){
|
---|
253 | int value=analogRead(TRIM);
|
---|
254 | messageOut.writeByte(COMMAND_READ_TRIM_RE);
|
---|
255 | messageOut.writeInt(value);
|
---|
256 | messageOut.sendData();
|
---|
257 | }
|
---|
258 |
|
---|
259 | void RobotMotorBoard::_refreshMotorAdjustment(){
|
---|
260 | motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
|
---|
261 | }
|
---|
262 |
|
---|
263 | void RobotMotorBoard::reportActionDone(){
|
---|
264 | setMode(MODE_SIMPLE);
|
---|
265 | messageOut.writeByte(COMMAND_ACTION_DONE);
|
---|
266 | messageOut.sendData();
|
---|
267 | }
|
---|
268 |
|
---|
269 | RobotMotorBoard RobotMotor=RobotMotorBoard();
|
---|