From ea9a7a11c7e6434b407912e68a0dd10068f7016d Mon Sep 17 00:00:00 2001 From: Federico Fissore Date: Wed, 22 Apr 2015 16:40:00 +0200 Subject: [PATCH] Converted Robot_Control files to unix format for better git diffs --- libraries/Robot_Control/src/Motors.cpp | 114 +++++++++++++++++- libraries/Robot_Control/src/communication.cpp | 30 ++++- 2 files changed, 142 insertions(+), 2 deletions(-) diff --git a/libraries/Robot_Control/src/Motors.cpp b/libraries/Robot_Control/src/Motors.cpp index 12096fd50..3555886a2 100644 --- a/libraries/Robot_Control/src/Motors.cpp +++ b/libraries/Robot_Control/src/Motors.cpp @@ -1 +1,113 @@ -#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; } */ \ No newline at end of file +#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; +} +*/ \ No newline at end of file diff --git a/libraries/Robot_Control/src/communication.cpp b/libraries/Robot_Control/src/communication.cpp index eaf5346c4..9e796c373 100644 --- a/libraries/Robot_Control/src/communication.cpp +++ b/libraries/Robot_Control/src/communication.cpp @@ -1 +1,29 @@ -#include 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(); } \ No newline at end of file +#include + +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(); +}