EngineClient::EngineClient( ds::App& app, const ds::cfg::Settings& settings,
ds::EngineData& ed, const ds::RootList& roots)
: inherited(app, settings, ed, roots)
, mLoadImageService(*this, mIpFunctions)
, mRenderTextService(mRenderTextThread)
, mSender(mSendConnection)
, mReceiver(mReceiveConnection)
, mBlobReader(mReceiver.getData(), *this)
, mSessionId(0)
, mConnectionRenewed(false)
, mServerFrame(-1)
, mState(nullptr)
, mIoInfo(*this)
{
// NOTE: Must be EXACTLY the same items as in EngineServer, in same order,
// so that the BLOB ids match.
HEADER_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveHeader(r.mDataBuffer);});
COMMAND_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveCommand(r.mDataBuffer);});
DELETE_SPRITE_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveDeleteSprite(r.mDataBuffer);});
CLIENT_STATUS_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveClientStatus(r.mDataBuffer); });
CLIENT_INPUT_BLOB = mBlobRegistry.add([this](BlobReader& r) {receiveClientInput(r.mDataBuffer); });
mReceiver.setHeaderAndCommandIds(HEADER_BLOB, COMMAND_BLOB);
try {
if (settings.getBool("server:connect", 0, true)) {
mSendConnection.initialize(true, settings.getText("server:ip"), ds::value_to_string(settings.getInt("server:listen_port")));
mReceiveConnection.initialize(false, settings.getText("server:ip"), ds::value_to_string(settings.getInt("server:send_port")));
}
} catch(std::exception &e) {
DS_LOG_ERROR_M("EngineClient::EngineClient() initializing UDP: " << e.what(), ds::ENGINE_LOG);
}
setState(mClientStartedState);
}
Connection::Connection(QTcpSocket *socket)
: QObject()
, m_connection(socket)
, m_command(0)
{
connect(m_connection, SIGNAL(readyRead()), this, SLOT(receiveCommand()));
connect(m_connection, SIGNAL(disconnected()), this, SLOT(closedConnection()));
}
void NMEAServer::deliver(Command_ptr command){
if((*command).getReceiver()=="server"){
receiveCommand(command);
}
else{
receive(command);
}
}
STATUS fineTuneX(int argc)
{
do
{
IplImage* frame = 0;
// Load image from IP camera
if(argc == 1)
getimage();
frame = cvLoadImage("image.jpg");
if(!frame)
{
printf("\n No input frame found !!!");
break;
}
// Step #1: Cutter detection and processing
// Detect cutter
if (cutterProcessing(frame) != STATUS_OK)
break;
// Step #2: Fruit detection and processing
fruitProcessing(frame);
// Step #3: Find nearby fruits
findNearbyFruits();
if(count == 0) {
break;
}
printf("\nFine tunnig along X axis....");
printf("\n fruitX=%d cutterX=%d",fruitX,cutterX);
if(fruitX - cutterX > 10){
sendCommand(COMMAND_RIGHT,1,0);
receiveCommand(&cmd,NULL,NULL);
if(cmd == COMMAND_FAIL)
printf("\Error right send command");
}
else if(cutterX - fruitX > 10){
sendCommand(COMMAND_LEFT,1,0);
receiveCommand(&cmd,NULL,NULL);
if(cmd == COMMAND_FAIL)
printf("\Error right send command");
}
Pager::Pager(KWMModuleApplication *a, char *name) : QWidget(NULL, "kwmpager")
{
kwmmapp = a;
a->setMainWidget(this);
kwmmapp -> connectToKWM();
KWM::setSticky(winId(), true);
KWM::setDecoration(winId(), KWM::tinyDecoration | KWM::noFocus);
KWM::setWmCommand(winId(), name);
KWM::setUnsavedDataHint(winId(), false);
int count = KWM::numberOfDesktops();
desktop_font = new QFont();
Desktop *desk;
desktops.resize(count);
for (int i = 0; i < count; i++) {
desk = new Desktop(a, i + 1, this);
desktops[i] = desk;
}
activeDesktop = desktops.at(KWM::currentDesktop() - 1);
activeDesktop->activate(true);
connect(kwmmapp, SIGNAL(desktopChange(int)) ,
SLOT(changeDesktop(int)));
connect(kwmmapp, SIGNAL(init()),
SLOT(initDesktops()));
connect(kwmmapp, SIGNAL( desktopNumberChange(int)),
SLOT(changeNumber(int)));
connect(kwmmapp, SIGNAL( windowAdd(Window)),
SLOT(addWindow(Window)));
connect(kwmmapp, SIGNAL( windowRemove(Window)),
SLOT(removeWindow(Window)));
connect(kwmmapp, SIGNAL( windowChange(Window)),
SLOT(windowChange(Window)));
connect(kwmmapp, SIGNAL( windowRaise(Window)),
SLOT(raiseWindow(Window)));
connect(kwmmapp, SIGNAL( windowLower(Window)),
SLOT(lowerWindow(Window)));
connect(kwmmapp, SIGNAL( windowActivate(Window)),
SLOT(windowActivate(Window)));
connect(kwmmapp, SIGNAL( commandReceived(QString)),
SLOT(receiveCommand(QString)));
moved = false;
readSettings();
initDesktops();
show();
placeIt();
}
void listenToCommands() {
initCOMPort();
initUSBRadio();
lastSpeedSentTimer.start();
HANDLE waitHandles[3] = { startSignal, osStatusDongle.hEvent };
stateGoal = goalOnRight;
stateBallSeen = ballOnRight;
while (true) {
float floorX, floorY;
int currentx, currenty;
findNearestFloorBall(floorX, floorY, currentx, currenty);
float angle = atanf(floorY / floorX) * 180.0 / PI;
swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle);
SetWindowText(infoGUI, buffer);
swprintf_s(buffer, L"greencount %d", fieldGreenPixelCountShare);
SetWindowText(infoGUI3, buffer);
if (hCOMDongle != INVALID_HANDLE_VALUE) {
if (lastSpeedSentTimer.time() > 0.5) { //if we send the speeds too often we get communication timeouts
setSpeedBoth(currentDrivingState);
lastSpeedSentTimer.start();
}
}
switch (WaitForMultipleObjects(2, waitHandles, FALSE, 50)) {
//case WAIT_OBJECT_0:
// prints(L"radio event %X\n", dwCommEvent);
// //receiveCommand();
// //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus);
// break;
case WAIT_OBJECT_0:
prints(L"Start signal arrived.\n");
SetWindowText(stateStatusGUI, L"started");
dribblerON();
play();
dribblerOFF();
//discharge();
break;
case WAIT_OBJECT_0 + 1: //info from the main board controller
//prints(L"main board COM event %X\n", dwCommEventDongle);
handleMainBoardCommunication();
WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character
break;
}
receiveCommand();
}
}
void Connection::receiveCommand()
{
QByteArray arr = m_connection->readAll();
debugOutput(1, QString::fromLatin1("Command received: ") + (arr));
QList<CommandInfo> commands = availableCommands();
for(QList<CommandInfo>::iterator it = commands.begin(); it != commands.end(); ++it) {
if (it->commandName == QString::fromLatin1(arr)) {
debugOutput(1, "Found command in list");
disconnect(m_connection, SIGNAL(readyRead()), this, SLOT(receiveCommand()));
AbstractCommand* command = (*it).commandFunc();
command->setSocket(m_connection);
m_command = command;
return;
}
}
debugOutput(2, QString::fromLatin1("Unknown command received: ") + (arr));
}
int llclose(int fd){
alarm(0);
if(!sendByte(fd, A_SEND, C_DISC))
return -1;
//puts("sent disc");
Command command = receiveCommand(fd);
//puts("eu recebi um comandito\n");
if(command.command != DISC){
//puts("didnt receive a disc");
return -1;
}
//puts("received a disc");
if(!sendByte(fd, A_RECEIVE, C_UA))
return -1;
//puts("confirmed");
return 1;
}
void GUIServer::run() {
if (receiveCommand()) {
m_UdpServer.beginPacket(m_UdpServer.remoteIP(),
m_UdpServer.remotePort());
m_UdpServer.write(m_Buffer[1]);
m_UdpServer.write(m_Buffer[0]);
Serial.print(F("Request ID: "));
Serial.println(m_Buffer[0]);
Serial.print(F("Method ID: "));
Serial.println(m_Buffer[1]);
Serial.print(F("value: "));
Serial.println(m_Buffer[2]);
switch (m_Buffer[1]) {
case GETVERSION:
m_UdpServer.write((uint8_t) 0);
m_UdpServer.write(1);
break;
case GETALLSENSORS:
getAllSensors();
break;
case GETSENSORDATA:
getSensorData(m_Buffer[2]);
break;
case SETSENSORCONFIG:
switch (m_Buffer[3]) {
case 1:
break;
case 2:
setSensorConfig(m_Buffer[2], m_Buffer[3], (char*) &m_Buffer[4]);
break;
case 3:
setSensorConfig(m_Buffer[2], m_Buffer[3], (char*) &m_Buffer[4]);
break;
case 4:
setSensorConfig(m_Buffer[2], m_Buffer[3],
(uint8_t) m_Buffer[4]);
break;
case 5:
setSensorConfig(m_Buffer[2], m_Buffer[3],
(uint8_t) m_Buffer[4]);
break;
}
break;
case GETALLACTUATORS:
getAllActuators();
break;
case GETACTUATORDATA:
getActuatorData(m_Buffer[2]);
break;
case SETACTUATORDATA:
switch (m_Buffer[3]) {
case 1:
case 3:
case 4:
case 5:
case 6:
setActuatorData(m_Buffer[2], m_Buffer[3],
(uint8_t) m_Buffer[4]);
break;
case 2:
setActuatorData(m_Buffer[2], m_Buffer[3], (char*) m_Buffer[4]);
}
break;
case SETACTUATORCONFIG:
switch (m_Buffer[3]) {
case 1:
break;
case 2:
setActuatorConfig(m_Buffer[2], m_Buffer[3],
(char*) &m_Buffer[4]);
break;
case 3:
break;
case 4:
break;
case 5:
break;
}
break;
case GETCLOCKTIMERS:
getClockTimers(m_Buffer[2]);
break;
default:
break;
}
m_UdpServer.endPacket();
}
}
int llwrite(int fd, unsigned char* buffer, int length){
//printf("entrei no llwrite\n");
char *bufferStuffed;
char header[] = { FLAG, 0x03, I(ll.sequenceNumber), header[1]^header[2] };
char dataBCC = generateBCC((char*)buffer, length);
char* toStuff = malloc(length+1);
memcpy(toStuff, buffer, length);
toStuff[length] = dataBCC;
int n= byteStuffing(toStuff, length+1, &bufferStuffed);
//int k;
//puts("");
//for (k=0;k<n;k++){
//printf("\n%d\n", bufferStuffed[k]);
//}
// puts("");
char* message = (char*) malloc(n+6);
memcpy(message, header, 4);
memcpy(message+4, bufferStuffed, n);
message[n+4] = FLAG;
int wrote = 0;
while(!wrote)
wrote = write(fd, message, n+5);
stats.dataFramesTransmitted++;
//puts("message sent\n");
free(message);
free(bufferStuffed);
alarm(ll.timeOut);
Command command = receiveCommand(fd);
//was asked for new frame
if (command.command == RR(!ll.sequenceNumber)){
ll.sequenceNumber = (!ll.sequenceNumber);
retries = 0;
//puts("fui confirmado");
alarm(0);
return length;
}
//frame was rejected, resend
if (command.command == REJ(ll.sequenceNumber) || (command.command == NONE && ll.numTransmissions > retries)){
if (command.command == NONE){
stats.timeouts++;
//printf("\nNumber of retries: %d\n", retries);
alarm(0);
}
//puts("byte was rejected,resending or no response\n");
stats.rejs++;
return llwrite(fd, buffer, length);
}
else{
//puts("eu esperei mas o reader nao me quis responder :(");
return -1;
}
return length;
}
int llopen(const char* port, int mode){
srand(time(NULL));
stats.dataFramesTransmitted = 0;
stats.timeouts=0;
stats.rejs=0;
installAlarm();
int fd;
char fileName[15];
fd = open(port, O_RDWR|O_NOCTTY);
if(fd <0){
perror(fileName);
exit(-1);
}
struct termios oldTio, newTio;
if(tcgetattr(fd, &oldTio) == -1){
perror("tcgetattr error");
exit(-1);
}
ll.oldtio = oldTio;
memset(&newTio, 0, sizeof(newTio)); //possible error here. if error use bzero instead
newTio.c_cflag = ll.baudRate | CS8 | CLOCAL | CREAD;
newTio.c_iflag = IGNPAR;
newTio.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
if(mode == RECEIVE){
newTio.c_lflag = 0;
newTio.c_cc[VTIME] = 0; /* inter-character timer unused */
newTio.c_cc[VMIN] = 1; /* blocking read until 5 chars received */
tcflush(fd, TCIOFLUSH);
if ( tcsetattr(fd,TCSANOW,&newTio) == -1) {
perror("tcsetattr");
exit(-1);
}
//printf("Ready to read\n");
Command command = receiveCommand(fd);
if(command.command == SET){
//puts("llopen_receive: received SET message\n");
while(!sendByte(fd, UA, 0x03)){}
return fd;
}
return E_GENERIC;
}
else if(mode == SEND){
newTio.c_lflag = 0;
newTio.c_cc[VTIME] = 10; /* inter-character timer unused */
newTio.c_cc[VMIN] = 0; /* blocking read until 5 chars received */
tcflush(fd, TCIOFLUSH);
if ( tcsetattr(fd,TCSANOW,&newTio) == -1) {
perror("tcsetattr");
exit(-1);
}
send: ;
resend = 0;
unsigned char SET[5] = {FLAG, A_SEND, C_SET, SET[1]^SET[2], FLAG};
if(write(fd, SET, 5) != 5)
return -1;
//printf("escrevi\n");
alarm(ll.timeOut);
Command command = receiveCommand(fd);
//printf("0x%02x command\n", command.command);
if(command.command == NONE){
if (ll.numTransmissions < retries){
puts("llopen_writer timeouts exceeded");
return -1;
}
else{
//puts("llopen_writer timeout");
goto send;
}
}
if (command.command == UA){
alarm(0);
return fd;
}
return -1;
}
return 0;
}
int llread(int fd, unsigned char **buffer){
//printf("preparing to read frame\n");
Command command = receiveCommand(fd);
//puts("llread:received command");
int repeated;
if (command.command == I(ll.sequenceNumber))
repeated = 0;
else if (command.command == I(!ll.sequenceNumber))
repeated = 1;
else if (command.command == DISC){
//printf("llread: disconnecting\n");
while(!sendByte(fd, 0x01, DISC)){}
//puts("llread: disc confirmation sent\n");
command = receiveCommand(fd);
if (command.command != UA){
//puts("llread: didnt receive UA after disc confirmation\n");
return E_GENERIC;
}
else{
//puts("llread: connection successfully closed\n");
return E_CLOSED;
}
}
if (command.command == I(0) || command.command == I(1)){
//puts("llread: received a data frame\n");
//if we never saw this frame before, consider it
stats.dataFramesTransmitted++;
if(!repeated){
//puts("llread: new data frame\n");
int length = byteDeStuffing(&(command.data), command.size);
//puts("llread: destuffing succeeded\n");
int bccOK = verifyBCC(command.data, length, command.data[length-1]);
if(getRand() < RANDOM_REJ_CHANCE){
bccOK = 0;
// puts("\n randomly rejecting good packet");
}
//Reject frames with wrong BCC
if(!bccOK){
//puts("llread: frame was damaged, rejecting and rereading\n");
stats.rejs++;
while(!sendByte(fd, 0x03, REJ(ll.sequenceNumber) )){}
return llread(fd, buffer);
}
//accept the frame and confirm it
//puts("llread: frame bcc ok, accepting\n");
*buffer = (unsigned char*) malloc(length);
memcpy(*buffer, command.data, length);
while(!sendByte(fd,0x03, RR(!ll.sequenceNumber))){}
//puts("llread: receiver ready sent, message confirmed\n");
ll.sequenceNumber = !ll.sequenceNumber;
free(command.data);
//printf("%d length\n", length);
return length;
}
//this frame is repeated. Confirm it and ask for the new frame
else{
//puts("llread: message repeated");
while(!sendByte(fd, 0x03, RR(ll.sequenceNumber))){}
return E_GENERIC;
}
}
else{
//printf("received unexpected command 0x%02x\n",command.command);
return E_GENERIC;
}
return 0;
}
void play() {
HANDLE waitHandles[5] = { stopSignal, osStatusDongle.hEvent, calibratingSignal, newImageAnalyzed };
int currentx = 320, currenty = 0, FPS2Count = 0;
int timeOut = 30;
float movingTime; //timeout after sending each command
objectCollection goals, goals2;
float nearestBallFloorX = 0, nearestBallFloorY = 0, initialBallDistance = 0;
ignoreX = 0, ignoreY = 0;
Timer ignoreTimer, ballSearchRotationSpeedTimer, chargingTimer, wanderingTimer, frameTimer;
float lastBallFoundTime = 0, lastGoalFoundTime = 0, wanderingTime = 0;
bool rotateFast = true;
FPS2Timer.start();
FPS2Count = 0;
frameTimer.start();
bool turnFast = true;
state = lookForBall;
SetWindowText(goalGuessState, L"Goal on right");
isBallInDribbler = false;
prints(L"Started, driving to ball\n");
SetWindowText(stateStatusGUI, L"Driving to ball");
charged = false, ignoreBall = false;
charge();
chargingTimer.start();
sendString(hCOMDongle, "9:bl\n");
lastBallFoundTimer.start();
lastGoalFoundTimer.start();
movingTimer.start();
while (true) {
//display test stuff
float floorX, floorY;
int currentx, currenty;
findNearestFloorBall(floorX, floorY, currentx, currenty);
float angle = atanf(floorY / floorX) * 180.0 / PI;
swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle);
SetWindowText(infoGUI, buffer);
//end display test stuff
switch (WaitForMultipleObjects(4, waitHandles, FALSE, timeOut)) {
//case WAIT_OBJECT_0: //start of a new command from the radio detected
// //ResetEvent(osStatus.hEvent);
// prints(L"radio event %X\n", dwCommEvent);
// //receiveCommand(); //receive and interpret the command
// //WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus); //listen to new events, ie beginning character
// continue;
case WAIT_OBJECT_0: //stop signal
prints(L"Stop signal arrived.\n");
SetWindowText(stateStatusGUI, L"stopped");
currentDrivingState.angle = 0, currentDrivingState.speed = 0, currentDrivingState.vx = 0,
currentDrivingState.vy = 0, currentDrivingState.angularVelocity = 0;
setSpeedAngle(currentDrivingState);
ResetEvent(startSignal);
return;
case WAIT_OBJECT_0 + 1: //info from the main board controller
//prints(L"main board COM event %X\n", dwCommEvent);
handleMainBoardCommunication();
WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character
continue;
case WAIT_OBJECT_0 + 2: //calibrating signal
SetWindowText(stateStatusGUI, L"calibrating");
rotateAroundCenter(0);
//WaitForSingleObject(calibratingEndSignal, INFINITE);
continue;
case WAIT_OBJECT_0 + 3: //new image analyzed
//update FPS rate after every 5 images
++FPS2Count;
if (FPS2Count % 5 == 0) {
float FPS = 5.0 / FPS2Timer.time();
swprintf_s(buffer, L"FPS2: %.2f", FPS);
SetWindowText(statusFPS2, buffer);
FPS2Timer.start();
}
if (FPS2Count % 10 == 0) {
receiveCommand();
}
break;
default:
continue;
}
if (attackBlue) { //set the side
goals = goalsBlueShare;
goals2 = goalsYellowShare;
}
else {
goals = goalsYellowShare;
goals2 = goalsBlueShare;
}
if (ignoreBall) { //ignore the ball just kicked for 1s
if (ignoreTimer.time() > 1.0) {
ignoreBall = false;
ignoreX = 0, ignoreY = 0;
}
else {
float ignoreXNew = ignoreX, ignoreYNew = ignoreY;
findNearestFloorObjectToOldObject(ignoreXNew, ignoreYNew, ballsShare);
//if we cant see the ball behind the dribbler, don't start ignoring a ball too far away
ignoreX = ignoreXNew;
ignoreY = ignoreYNew;
swprintf_s(buffer, L"IgX %.2f IgY %.2f\n NX %.2f NY %.2f", ignoreX, ignoreY, ignoreXNew, ignoreYNew);
SetWindowText(infoGUI2, buffer);
}
}
else {
SetWindowText(infoGUI2, L"Ignore OFF");
}
//keep track of the likely direction to turn to to find the goal
if ((goalsBlueShare.count > 0 || goalsYellowShare.count > 0) && !(goalsBlueShare.count == 1 && goalsYellowShare.count == 1)) {
int goalX, goalY;
State stateGoalNew;
if (goals.count > 0) {
findLargestObject(goalX, goalY, goals);
stateGoalNew = goalX > 320 ? goalOnRight : goalOnLeft;
}
else if (goals2.count > 0) {
findLargestObject(goalX, goalY, goals2);
stateGoalNew = goalX > 320 ? goalOnLeft : goalOnRight;
}
if (stateGoalNew != stateGoal && state != lookForGoal) {
stateGoal = stateGoalNew;
if (stateGoal == goalOnRight) {
SetWindowText(goalGuessState, L"Goal on the right");
}
else {
SetWindowText(goalGuessState, L"Goal on the left");
}
}
}
if (ballsOnFieldInSight() > 0) { //keep track of the likely direction to turn to to find the ball
for (int i = 0; i < ballsShare.count; ++i) {
if (!ballsShare.data[i].isObjectAcrossLine) {
if (ballsShare.data[i].x > 320) {
stateBallSeen = ballOnRight;
}
else {
stateBallSeen = ballOnLeft;
}
}
}
}
float frameTime = frameTimer.time();
frameTimer.start();
if (ballsOnFieldInSight() == 0 && state == lookForBall && !isBallInDribbler) {
lastBallFoundTime += frameTime;
}
else if (ballsOnFieldInSight > 0) {
lastBallFoundTime = 0;
}
if (goals.count == 0 && state == lookForGoal) {
lastGoalFoundTime += frameTime;
}
else if (goals.count > 0) {
lastGoalFoundTime = 0;
}
if (lastBallFoundTime > 3.0 || lastGoalFoundTime > 3.0) {
prints(L"Wandering for ball\n");
state = wanderForBall;
movingTimer.start();
wanderingTimer.start();
wanderingTime = 0;
lastBallFoundTime = 0;
lastGoalFoundTime = 0;
}
if (fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD && state != rotate90 && state != wanderForBall) {
state = rotate90;
movingTimer.start();
}
if (state == lookForBall) {
if (isBallInDribbler) {
prints(L"Looking for goal\n");
state = lookForGoal;
movingTimer.start();
}
else {
//prints(L"Looking for ball\n");
SetWindowText(stateStatusGUI, L"Looking for ball");
nearestBallFloorX = 0;
int sign = stateBallSeen == ballOnRight ? -1 : 1;
if (ballsOnFieldInSight() == 0) {
if (fmodf(movingTimer.time(), 0.3) < 0.2) {
rotateAroundCenter(sign * 200);
}
else {
rotateAroundCenter(sign * 40);
}
}
else {
findNearestFloorBall(nearestBallFloorX, nearestBallFloorY, currentx, currenty);
initialBallDistance = pow(nearestBallFloorX*nearestBallFloorX + nearestBallFloorY*nearestBallFloorY, 0.5);
//prints(L"found nearest ball X: %.2f, Y: %.2f, currentx: %d, currenty: %d\n", nearestBallFloorX, nearestBallFloorY, currentx, currenty);
prints(L"Driving to ball\n");
state = driveToBall;
rotateAroundCenter(0);
movingTimer.start();
}
}
}
if (state == driveToBall) {
if (isBallInDribbler) {
rotateAroundCenter(0);
Sleep(0);
prints(L"Looking for goal\n");
state = lookForGoal;
movingTimer.start();
}
else {
//prints(L"Driving to ball\n");
SetWindowText(stateStatusGUI, L"driving to ball");
float floorX, floorY;
//findNearestObject(currentx, currenty, ballsShare);
//prints(L"Floor coordinates x: %.2f, y: %.2f, currentx: %d, currenty: %d\n", floorX, floorY, currentx, currenty);
if (findNearestFloorBall(floorX, floorY, currentx, currenty) == 0) {
//prints(L"Drive state, but ballcount zero, starting to look for ball.\n");
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
continue;
}
driveToFloorXYPID(floorX, floorY, initialBallDistance);
}
}
if (state == lookForGoal) {
//prints(L"Looking for goal\n");
SetWindowText(stateStatusGUI, L"Looking for goal\n");
if (isBallInDribbler) {
int x = 0, y = 0;
float floorX, floorY;
findLargestObject(x, y, goals);
convertToFloorCoordinates(x, y, floorX, floorY, GOALMIDHEIGHT);
//prints(L"Goal x: %d\n", x);
if (!(goalsBlueShare.count == 1 && goalsYellowShare.count == 1) &&
(y > 0 && floorX > 0 && fabs(floorY) < 18.0 / 100.0 || abs(x-320) <= 35)) {
state = kickBall;
rotateAroundFront(0);
prints(L"Kicking\n");
}
else {
int sign;
if (stateGoal == goalOnRight) {
sign = -1;
}
else {
sign = 1;
}
if (goals.count == 0) { //no goals in sight, turn faster
rotateAroundFront(sign * 200);
}
else{
//around 120 degs/s is max speed so that the goal doesn't go out, less than 30 degs/s is pointless
float angle = atanf(tanf(angleOfView)*(x - 320.0) / 320.0); //roughly, in degrees
float speedMultiplier = 1 - expf(-fabs(pow((x - 320.0) / 320.0, 4)));
speedMultiplier = speedMultiplier > 1 ? 1 : speedMultiplier;
float turningSpeed = 50 + (160 - 50)*speedMultiplier;
rotateAroundFront(sign * turningSpeed);
}
}
}
else {
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
}
}
if (state == kickBall) {
SetWindowText(stateStatusGUI, L"Kicking");
if (!isBallInDribbler) {
state = lookForBall;
movingTimer.start();
}
if (!charged) {//not charged, wait more
if (chargingTimer.time() > 3.5) { //by this time we definitely should have gotten a charge command, charge again
prints(L"No charge info received in time\n");
chargingTimer.start();
charge();
}
continue;
}
kick();
Sleep(20);
charged = false;
//isBallInDribbler = false;
//sendString(hCOMDongle, "9:bl\n");
ignoreBall = true;
ignoreX = 25.0 / 100.0, ignoreY = 0;
ignoreTimer.start();
state = lookForBall;
movingTimer.start();
prints(L"Looking for ball\n");
//Sleep(80);
charge();
chargingTimer.start();
}
if (state == rotate90) {
SetWindowText(stateStatusGUI, L"Rotating 90");
if (movingTimer.time() < 0.5) {
rotateAroundCenter(180);
continue;
}
else {
prints(L"Looking for ball\n");
stateBallSeen = ballOnLeft;
stateGoal = goalOnLeft;
state = lookForBall;
movingTimer.start();
}
}
//wander around, try to drive towards a goal far enough away, if can't find one, drive to a direction where there are no goals, lines and there is enough green
if (state == wanderForBall) {
wanderingTime += frameTime;
SetWindowText(stateStatusGUI, L"Wandering");
if (wanderingTime > 4.0) { //if we didn't find a far away goal in 4 seconds
int goalBlueX, goalBlueY;
int goalYellowX, goalYellowY;
findLargestObject(goalBlueX, goalBlueY, goalsBlueShare);
findLargestObject(goalYellowX, goalYellowY, goalsYellowShare);
if (goalsBlueShare.count != 0 && goalBlueX >= 150 && goalBlueX <= 500
|| goalsYellowShare.count != 0 && goalYellowX >= 150 && goalYellowX <= 500 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
rotateAroundCenter(100);
wanderingTimer.start();
}
else {
if (wanderingTimer.time() > 1.0) {
movingTimer.start();
state = lookForBall;
prints(L"Looking for ball\n");
continue;
}
driveForward(0.5);
}
}
else {
if (goalsBlueShare.count == 0 && goalsYellowShare.count == 0 || isLineStraightAhead || fieldGreenPixelCountShare < FIELDGREENCOUNTTHRESHOLD) {
rotateAroundCenter(120);
wanderingTimer.start();
}
else {
if (wanderingTimer.time() > 1.0) {
movingTimer.start();
state = lookForBall;
prints(L"Looking for ball\n");
continue;
}
int goalX = 0, goalY = 0;
findLargestObject(goalX, goalY, goalsBlueShare);
if (goalY > 450) {
driveForward(0.5);
continue;
}
else {
goalX = 0, goalY = 0;
findLargestObject(goalX, goalY, goalsYellowShare);
if (goalY > 450) {
driveForward(0.5);
}
else {
rotateAroundCenter(100);
wanderingTimer.start();
}
}
}
}
}
}
}
void NMEAServer::run(){
shouldRun=true;
while(shouldRun){
std::list<Message_ptr> msgs_cpy;
{
boost::mutex::scoped_lock lock(msgsMutex);
if(msgs.empty()){
msgsCond.wait(lock);
}
msgs_cpy = std::list<Message_ptr>(msgs);
}
while(!msgs_cpy.empty()){
std::list<Endpoint_ptr> online_cpy;
{
boost::mutex::scoped_lock lock(onlineMutex);
online_cpy = std::list<Endpoint_ptr>(online);
}
if(NMEAmsg_ptr tmp_msg = boost::dynamic_pointer_cast<NMEAmsg>(msgs_cpy.front())){
for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) {
NMEAEndpoint_ptr nmeaEnd = boost::dynamic_pointer_cast<NMEAEndpoint>(*endpoint);
if(nmeaEnd){
nmeaEnd->deliver(tmp_msg);
}
}
}
else if(Command_ptr tmp_cmd = boost::dynamic_pointer_cast<Command>(msgs_cpy.front())){
if(tmp_cmd->getReceiver()=="*"){
receiveCommand(tmp_cmd);
}
if(tmp_cmd->getReceiver()=="server"){
receiveCommand(tmp_cmd);
}
else{
bool found=false;
for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) {
std::string receiver = tmp_cmd->getReceiver();
boost::replace_all(receiver, "*", "(.*)");
boost::regex reg("^"+receiver+"$");
boost::cmatch matches;
CommandEndpoint_ptr commandEnd = boost::dynamic_pointer_cast<CommandEndpoint>(*endpoint);
if(commandEnd){
if(boost::regex_search(commandEnd->getId().c_str(), matches, reg)){
if(tmp_cmd->getSender()!=commandEnd){
commandEnd->receive(tmp_cmd);
found=true;
}
}
}
}
if(!found){
tmp_cmd->answer(Answer::UNKNOWN_RECEIVER, "The receiver "+tmp_cmd->getReceiver()+" was not found \n", this->shared_from_this());
}
}
}
else if(Answer_ptr tmp_answer = boost::dynamic_pointer_cast<Answer>(msgs_cpy.front())){
if(tmp_answer->getReceiver()=="*"){
deliver(tmp_answer);
}
if(tmp_answer->getReceiver()=="server"){
deliver(tmp_answer);
}
else{
bool found=false;
for (std::list<Endpoint_ptr>::const_iterator endpoint = online_cpy.begin(), end = online_cpy.end(); endpoint != end; ++endpoint) {
std::string receiver = tmp_answer->getReceiver();
boost::replace_all(receiver, "*", "(.*)");
boost::regex reg("^"+receiver+"$");
boost::cmatch matches;
CommandEndpoint_ptr commandEnd = boost::dynamic_pointer_cast<CommandEndpoint>(*endpoint);
if(commandEnd){
if(boost::regex_search(commandEnd->getId().c_str(), matches, reg)){
if(tmp_answer->getSender()!=commandEnd){
commandEnd->deliver(tmp_answer);
found=true;
}
}
}
}
}
}
{
boost::mutex::scoped_lock lock(msgsMutex);
msgs.remove(msgs_cpy.front());
}
msgs_cpy.pop_front();
}
}
}
void checkCommands(char *buffer){
//maybe for changing physical layer probabilities
//printf("Getting data from link layer\n");
int size = dataLinkRecv(buffer);
receiveCommand(buffer, size);
}
