void UBGraphicsRuler::mouseMoveEvent(QGraphicsSceneMouseEvent *event)
{
if (!mResizing && !mRotating)
{
QGraphicsItem::mouseMoveEvent(event);
}
else
{
if (mResizing)
{
QPointF delta = event->pos() - event->lastPos();
if (rect().width() + delta.x() < sMinLength)
delta.setX(sMinLength - rect().width());
if (rect().width() + delta.x() > sMaxLength)
delta.setX(sMaxLength - rect().width());
setRect(QRectF(rect().topLeft(), QSizeF(rect().width() + delta.x(), rect().height())));
}
else
{
QLineF currentLine(rotationCenter(), event->pos());
QLineF lastLine(rotationCenter(), event->lastPos());
rotateAroundCenter(currentLine.angleTo(lastLine));
}
event->accept();
}
}
void UBGraphicsTriangle::rotateAroundCenter(qreal angle)
{
qreal oldAngle = this->angle;
this->angle = angle;
QTransform transform;
rotateAroundCenter(transform, rotationCenter());
setTransform(transform, true);
this->angle = oldAngle + angle; // We have to store absolute value for FLIP case
}
void UBGraphicsTriangle::mouseMoveEvent(QGraphicsSceneMouseEvent *event)
{
if (!mResizing1 && !mResizing2 && !mRotating)
{
QGraphicsItem::mouseMoveEvent(event);
}
else
{
QPoint currPos = sceneTransform().inverted().map(event->screenPos());
if (mResizing1)
{
if (mOrientation == TopLeft || mOrientation == BottomLeft)
{
int deltaX = currPos.x() - lastPos.x();
if (lastRect.width() + deltaX < sMinWidth)
deltaX = sMinWidth - lastRect.width();
setRect(QRectF(lastRect.left(), lastRect.top(),
lastRect.width() + deltaX, lastRect.height()), mOrientation);
}
else
{
int deltaX = lastPos.x() - currPos.x();
if (lastRect.width() + deltaX < sMinWidth)
deltaX = sMinWidth - lastRect.width();
setRect(QRectF(lastRect.left() - deltaX, lastRect.top(),
lastRect.width() + deltaX, lastRect.height()), mOrientation);
}
}
//-----------------------------------------------//
if (mResizing2)
{
if (mOrientation == BottomRight || mOrientation == BottomLeft)
{
int deltaY = lastPos.y() - currPos.y();
if (lastRect.height() + deltaY < sMinHeight)
deltaY = sMinHeight - lastRect.height();
setRect(QRectF(lastRect.left(), lastRect.top() - deltaY,
lastRect.width(), lastRect.height() + deltaY), mOrientation);
}
else
{
int deltaY = currPos.y() - lastPos.y();
if (lastRect.height() + deltaY < sMinHeight)
deltaY = sMinHeight - lastRect.height();
setRect(QRectF(lastRect.left(), lastRect.top(),
lastRect.width(), lastRect.height() + deltaY), mOrientation);
}
}
//-----------------------------------------------//
if (mRotating)
{
QLineF currentLine(rotationCenter(), event->pos());
QLineF lastLine(rotationCenter(), event->lastPos());
rotateAroundCenter(currentLine.angleTo(lastLine));
}
//-----------------------------------------------//
event->accept();
}
}
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();
}
}
}
}
}
}
}
