[136] | 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 | */
|
---|