Skip to content

Commit ea5299e

Browse files
Fede85Fede85
committed
SPI library to the new format and moved Robot_Motor and Robot_Control libraries
1 parent 141de7a commit ea5299e

File tree

10 files changed

+915
-0
lines changed

10 files changed

+915
-0
lines changed

‎ArduinoRobotMotorBoard.cpp

Lines changed: 265 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,265 @@
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(speedL>0){
174+
analogWrite(IN_A1,speedL);
175+
analogWrite(IN_A2,0);
176+
}else{
177+
analogWrite(IN_A1,0);
178+
analogWrite(IN_A2,-speedL);
179+
}
180+
181+
if(speedR>0){
182+
analogWrite(IN_B1,speedR);
183+
analogWrite(IN_B2,0);
184+
}else{
185+
analogWrite(IN_B1,0);
186+
analogWrite(IN_B2,-speedR);
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+
IRs.selectPin(num-1); //To make consistant with the pins labeled on the board
234+
return IRs.getAnalogValue();
235+
}
236+
237+
void RobotMotorBoard::_readIR(){
238+
//Serial.println("readIR");
239+
int value;
240+
messageOut.writeByte(COMMAND_READ_IR_RE);
241+
for(int i=1;i<6;i++){
242+
value=IRread(i);
243+
messageOut.writeInt(value);
244+
}
245+
messageOut.sendData();
246+
}
247+
248+
void RobotMotorBoard::_readTrim(){
249+
int value=analogRead(TRIM);
250+
messageOut.writeByte(COMMAND_READ_TRIM_RE);
251+
messageOut.writeInt(value);
252+
messageOut.sendData();
253+
}
254+
255+
void RobotMotorBoard::_refreshMotorAdjustment(){
256+
motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
257+
}
258+
259+
void RobotMotorBoard::reportActionDone(){
260+
setMode(MODE_SIMPLE);
261+
messageOut.writeByte(COMMAND_ACTION_DONE);
262+
messageOut.sendData();
263+
}
264+
265+
RobotMotorBoard RobotMotor=RobotMotorBoard();

‎ArduinoRobotMotorBoard.h

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
#ifndef ArduinoRobot_h
2+
#define ArduinoRobot_h
3+
4+
#include "EasyTransfer2.h"
5+
#include "Multiplexer.h"
6+
#include "LineFollow.h"
7+
//#include "IRremote.h"
8+
9+
#if ARDUINO >= 100
10+
#include "Arduino.h"
11+
#else
12+
#include "WProgram.h"
13+
#endif
14+
15+
//Command code
16+
#define COMMAND_SWITCH_MODE 0
17+
#define COMMAND_RUN 10
18+
#define COMMAND_MOTORS_STOP 11
19+
#define COMMAND_ANALOG_WRITE 20
20+
#define COMMAND_DIGITAL_WRITE 30
21+
#define COMMAND_ANALOG_READ 40
22+
#define COMMAND_ANALOG_READ_RE 41
23+
#define COMMAND_DIGITAL_READ 50
24+
#define COMMAND_DIGITAL_READ_RE 51
25+
#define COMMAND_READ_IR 60
26+
#define COMMAND_READ_IR_RE 61
27+
#define COMMAND_ACTION_DONE 70
28+
#define COMMAND_READ_TRIM 80
29+
#define COMMAND_READ_TRIM_RE 81
30+
#define COMMAND_PAUSE_MODE 90
31+
#define COMMAND_LINE_FOLLOW_CONFIG 100
32+
33+
34+
//component codename
35+
#define CN_LEFT_MOTOR 0
36+
#define CN_RIGHT_MOTOR 1
37+
#define CN_IR 2
38+
39+
//motor board modes
40+
#define MODE_SIMPLE 0
41+
#define MODE_LINE_FOLLOW 1
42+
#define MODE_ADJUST_MOTOR 2
43+
#define MODE_IR_CONTROL 3
44+
45+
//bottom TKs, just for communication purpose
46+
#define B_TK1 201
47+
#define B_TK2 202
48+
#define B_TK3 203
49+
#define B_TK4 204
50+
51+
/*
52+
A message structure will be:
53+
switch mode (2):
54+
byte COMMAND_SWITCH_MODE, byte mode
55+
run (5):
56+
byte COMMAND_RUN, int speedL, int speedR
57+
analogWrite (3):
58+
byte COMMAND_ANALOG_WRITE, byte codename, byte value;
59+
digitalWrite (3):
60+
byte COMMAND_DIGITAL_WRITE, byte codename, byte value;
61+
analogRead (2):
62+
byte COMMAND_ANALOG_READ, byte codename;
63+
analogRead _return_ (4):
64+
byte COMMAND_ANALOG_READ_RE, byte codename, int value;
65+
digitalRead (2):
66+
byte COMMAND_DIGITAL_READ, byte codename;
67+
digitalRead _return_ (4):
68+
byte COMMAND_DIGITAL_READ_RE, byte codename, int value;
69+
read IR (1):
70+
byte COMMAND_READ_IR;
71+
read IR _return_ (9):
72+
byte COMMAND_READ_IR_RE, int valueA, int valueB, int valueC, int valueD;
73+
74+
75+
*/
76+
77+
class RobotMotorBoard:public LineFollow{
78+
public:
79+
RobotMotorBoard();
80+
void begin();
81+
82+
void process();
83+
84+
void parseCommand();
85+
86+
int IRread(uint8_t num);
87+
88+
void setMode(uint8_t mode);
89+
void pauseMode(bool onOff);
90+
91+
void motorsWrite(int speedL, int speedR);
92+
void motorsWritePct(int speedLpct, int speedRpct);//write motor values in percentage
93+
void motorsStop();
94+
private:
95+
float motorAdjustment;//-1.0 ~ 1.0, whether left is lowered or right is lowered
96+
97+
//convert codename to actual pins
98+
uint8_t parseCodename(uint8_t codename);
99+
uint8_t codenameToAPin(uint8_t codename);
100+
101+
void stopCurrentActions();
102+
//void sendCommand(byte command,byte codename,int value);
103+
104+
void _analogWrite(uint8_t codename, int value);
105+
void _digitalWrite(uint8_t codename, bool value);
106+
void _analogRead(uint8_t codename);
107+
void _digitalRead(uint8_t codename);
108+
void _readIR();
109+
void _readTrim();
110+
111+
void _refreshMotorAdjustment();
112+
113+
Multiplexer IRs;
114+
uint8_t mode;
115+
uint8_t isPaused;
116+
EasyTransfer2 messageIn;
117+
EasyTransfer2 messageOut;
118+
119+
//Line Following
120+
void reportActionDone();
121+
};
122+
123+
extern RobotMotorBoard RobotMotor;
124+
125+
#endif

0 commit comments

Comments
 (0)