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 | */
|
---|