Skip to content

Commit ea9a7a1

Browse files
author
Federico Fissore
committed
Converted Robot_Control files to unix format for better git diffs
1 parent 269a8b1 commit ea9a7a1

File tree

2 files changed

+142
-2
lines changed

2 files changed

+142
-2
lines changed

‎libraries/Robot_Control/src/Motors.cpp

+113-1
Original file line numberDiff line numberDiff line change
@@ -1 +1,113 @@
1-
#include "ArduinoRobot.h"#include "EasyTransfer2.h"void RobotControl::motorsStop(){ messageOut.writeByte(COMMAND_MOTORS_STOP); messageOut.sendData();}void RobotControl::motorsWrite(int speedLeft,int speedRight){ messageOut.writeByte(COMMAND_RUN); messageOut.writeInt(speedLeft); messageOut.writeInt(speedRight); messageOut.sendData();}void RobotControl::motorsWritePct(int speedLeftPct, int speedRightPct){ int16_t speedLeft=255*speedLeftPct/100.0; int16_t speedRight=255*speedRightPct/100.0; motorsWrite(speedLeft,speedRight);}void RobotControl::pointTo(int angle){ int target=angle; uint8_t speed=80; target=target%360; if(target<0){ target+=360; } int direction=angle; while(1){ int currentAngle=compassRead(); int diff=target-currentAngle; direction=180-(diff+360)%360; if(direction>0){ motorsWrite(speed,-speed);//right delay(10); }else{ motorsWrite(-speed,speed);//left delay(10); } //if(diff<-180) // diff += 360; //else if(diff> 180) // diff -= 360; //direction=-diff; if(abs(diff)<5){ motorsStop(); return; } }}void RobotControl::turn(int angle){ int originalAngle=compassRead(); int target=originalAngle+angle; pointTo(target); /*uint8_t speed=80; target=target%360; if(target<0){ target+=360; } int direction=angle; while(1){ if(direction>0){ motorsWrite(speed,speed);//right delay(10); }else{ motorsWrite(-speed,-speed);//left delay(10); } int currentAngle=compassRead(); int diff=target-currentAngle; if(diff<-180) diff += 360; else if(diff> 180) diff -= 360; direction=-diff; if(abs(diff)<5){ motorsWrite(0,0); return; } }*/}void RobotControl::moveForward(int speed){ motorsWrite(speed,speed);}void RobotControl::moveBackward(int speed){ motorsWrite(speed,speed);}void RobotControl::turnLeft(int speed){ motorsWrite(speed,255);}void RobotControl::turnRight(int speed){ motorsWrite(255,speed);}/*int RobotControl::getIRrecvResult(){ messageOut.writeByte(COMMAND_GET_IRRECV); messageOut.sendData(); //delay(10); while(!messageIn.receiveData()); if(messageIn.readByte()==COMMAND_GET_IRRECV_RE){ return messageIn.readInt(); } return -1;}*/
1+
#include "ArduinoRobot.h"
2+
#include "EasyTransfer2.h"
3+
4+
5+
void RobotControl::motorsStop(){
6+
messageOut.writeByte(COMMAND_MOTORS_STOP);
7+
messageOut.sendData();
8+
}
9+
void RobotControl::motorsWrite(int speedLeft,int speedRight){
10+
messageOut.writeByte(COMMAND_RUN);
11+
messageOut.writeInt(speedLeft);
12+
messageOut.writeInt(speedRight);
13+
messageOut.sendData();
14+
}
15+
void RobotControl::motorsWritePct(int speedLeftPct, int speedRightPct){
16+
int16_t speedLeft=255*speedLeftPct/100.0;
17+
int16_t speedRight=255*speedRightPct/100.0;
18+
motorsWrite(speedLeft,speedRight);
19+
}
20+
void RobotControl::pointTo(int angle){
21+
int target=angle;
22+
uint8_t speed=80;
23+
target=target%360;
24+
if(target<0){
25+
target+=360;
26+
}
27+
int direction=angle;
28+
while(1){
29+
int currentAngle=compassRead();
30+
int diff=target-currentAngle;
31+
direction=180-(diff+360)%360;
32+
if(direction>0){
33+
motorsWrite(speed,-speed);//right
34+
delay(10);
35+
}else{
36+
motorsWrite(-speed,speed);//left
37+
delay(10);
38+
}
39+
//if(diff<-180)
40+
// diff += 360;
41+
//else if(diff> 180)
42+
// diff -= 360;
43+
//direction=-diff;
44+
45+
if(abs(diff)<5){
46+
motorsStop();
47+
return;
48+
}
49+
}
50+
}
51+
void RobotControl::turn(int angle){
52+
int originalAngle=compassRead();
53+
int target=originalAngle+angle;
54+
pointTo(target);
55+
/*uint8_t speed=80;
56+
target=target%360;
57+
if(target<0){
58+
target+=360;
59+
}
60+
int direction=angle;
61+
while(1){
62+
if(direction>0){
63+
motorsWrite(speed,speed);//right
64+
delay(10);
65+
}else{
66+
motorsWrite(-speed,-speed);//left
67+
delay(10);
68+
}
69+
int currentAngle=compassRead();
70+
int diff=target-currentAngle;
71+
if(diff<-180)
72+
diff += 360;
73+
else if(diff> 180)
74+
diff -= 360;
75+
direction=-diff;
76+
77+
if(abs(diff)<5){
78+
motorsWrite(0,0);
79+
return;
80+
}
81+
}*/
82+
}
83+
84+
void RobotControl::moveForward(int speed){
85+
motorsWrite(speed,speed);
86+
}
87+
void RobotControl::moveBackward(int speed){
88+
motorsWrite(speed,speed);
89+
}
90+
void RobotControl::turnLeft(int speed){
91+
motorsWrite(speed,255);
92+
}
93+
void RobotControl::turnRight(int speed){
94+
motorsWrite(255,speed);
95+
}
96+
97+
98+
99+
/*
100+
int RobotControl::getIRrecvResult(){
101+
messageOut.writeByte(COMMAND_GET_IRRECV);
102+
messageOut.sendData();
103+
//delay(10);
104+
while(!messageIn.receiveData());
105+
106+
if(messageIn.readByte()==COMMAND_GET_IRRECV_RE){
107+
return messageIn.readInt();
108+
}
109+
110+
111+
return -1;
112+
}
113+
*/

‎libraries/Robot_Control/src/communication.cpp

+29-1
Original file line numberDiff line numberDiff line change
@@ -1 +1,29 @@
1-
#include <ArduinoRobot.h>bool RobotControl::isActionDone(){ if(messageIn.receiveData()){ if(messageIn.readByte()==COMMAND_ACTION_DONE){ return true; } } return false;}void RobotControl::pauseMode(uint8_t onOff){ messageOut.writeByte(COMMAND_PAUSE_MODE); if(onOff){ messageOut.writeByte(true); }else{ messageOut.writeByte(false); } messageOut.sendData();}void RobotControl::lineFollowConfig(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){ messageOut.writeByte(COMMAND_LINE_FOLLOW_CONFIG); messageOut.writeByte(KP); messageOut.writeByte(KD); messageOut.writeByte(robotSpeed); messageOut.writeByte(intergrationTime); messageOut.sendData();}
1+
#include <ArduinoRobot.h>
2+
3+
bool RobotControl::isActionDone(){
4+
if(messageIn.receiveData()){
5+
if(messageIn.readByte()==COMMAND_ACTION_DONE){
6+
return true;
7+
}
8+
}
9+
return false;
10+
}
11+
12+
void RobotControl::pauseMode(uint8_t onOff){
13+
messageOut.writeByte(COMMAND_PAUSE_MODE);
14+
if(onOff){
15+
messageOut.writeByte(true);
16+
}else{
17+
messageOut.writeByte(false);
18+
}
19+
messageOut.sendData();
20+
}
21+
22+
void RobotControl::lineFollowConfig(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
23+
messageOut.writeByte(COMMAND_LINE_FOLLOW_CONFIG);
24+
messageOut.writeByte(KP);
25+
messageOut.writeByte(KD);
26+
messageOut.writeByte(robotSpeed);
27+
messageOut.writeByte(intergrationTime);
28+
messageOut.sendData();
29+
}

0 commit comments

Comments
 (0)