void turnToDirection(int turnDirection) {
adjustAngle();
int currentDir;
currentDir = currentDirection();
double target = (currentDir + turnDirection) * M_PI / 2;
while(fabs(currentHeading() - target) > turnThreshold) {
if(fabs(currentHeading() - target) < slowThreshold) {
if(currentHeading() > target) {
set_motors(-5,5);
}
if(currentHeading() < target) {
set_motors(5,-5);
}
} else {
if(currentHeading() > target) {
set_motors(-15,15);
}
if(currentHeading() < target) {
set_motors(15,-15);
}
}
calcPos();
}
set_motors(0,0);
adjustAngle();
usleep(10000);
}
double CSDirection::getAngle(const short sum[4], short ma) {
int nbMax = ma == sum[0] ? 1 : 0;
nbMax = ma == sum[1] ? nbMax + 1 : nbMax;
nbMax = ma == sum[2] ? nbMax + 1 : nbMax;
nbMax = ma == sum[3] ? nbMax + 1 : nbMax;
double diff = 50.0;
if (nbMax == 1) {
double angle, corrAngle;
if (ma == sum[0]) {
angle = 0.0;
corrAngle = adjustAngle(sum, 0, 3, 1);
if (corrAngle < 0.0) angle = 180.0;
} else if (ma == sum[1]) {
angle = 45.0;
corrAngle = adjustAngle(sum, 1, 0, 2);
} else if (ma == sum[2]) {
angle = 90.0;
corrAngle = adjustAngle(sum, 2, 1, 3);
} else {
angle = 135.0;
corrAngle = adjustAngle(sum, 3, 2, 0);
}
return angle + corrAngle + diff;
}
///* ?????????????????????? kiprobalni
if (nbMax == 2) {
if (ma == sum[0] && ma == sum[1]) return 22.5 + diff;
if (ma == sum[0] && ma == sum[3]) return 157.5 + diff;
if (ma == sum[1] && ma == sum[2]) return 67.5 + diff;
if (ma == sum[2] && ma == sum[3]) return 112.5 + diff;
}
//*/
return 1.0;
}
void Straight()
{
adjustAngle();
// Going Up
int i;
int motorSpeed;
for (i = 0; i < 11; i++) {
set_motors(3 * i, 3 * i);
usleep(100000);
}
if(currentDirection() == 0 || currentDirection() == 4) {
idealY += 60;
printf("UP TargetY: %f\n",idealY);
while(fabs(currentY()-idealY) > 1) {
if(fabs(currentY()-idealY) < 10) {
if(currentY() < idealY) {
correctToStraight(2 * fabs(currentY()-idealY));
// set_motors(motorSpeed, motorSpeed);
} else {
correctToStraight(-2 * fabs(currentY()-idealY));
// set_motors(-5 -2 * fabs(currentY()-idealY),-5 -2 * fabs(currentY()-idealY));
}
} else if(currentY() < idealY) {
set_motors(40,40);
} else if(currentY() > idealY) {
set_motors(-40,-40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.y += 1;
}
// Going Down
if(currentDirection() == 2) {
idealY -= 60;
printf("DOWN TargetY: %f\n",idealY);
while(fabs(currentY()-idealY) > 1) {
if(fabs(currentY()-idealY) < 10) {
if(currentY() > idealY) {
correctToStraight(2 * fabs(currentY()-idealY));
// set_motors(5 + 2 * fabs(currentY()-idealY), 5 + 2 * fabs(currentY()-idealY));
} else {
correctToStraight(-2 * fabs(currentY()-idealY));
// set_motors(-5 -2 * fabs(currentY()-idealY),-5 -2 * fabs(currentY()-idealY));
}
} else if(currentY() < idealY) {
set_motors(-40,-40);
} else if(currentY() > idealY) {
set_motors(40,40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.y -= 1;
}
// Going Right
if(currentDirection() == 1) {
idealX += 60;
printf("RIGHT TargetX: %f\n",idealX);
while(fabs(currentX()-idealX) > 1) {
if(fabs(currentX()-idealX) < 10) {
if(currentX() < idealX) {
correctToStraight(2 * fabs(currentX()-idealX));
// set_motors(5 + 2 * fabs(currentX()-idealX),5 + 2 * fabs(currentX()-idealX));
} else {
correctToStraight(-2 * fabs(currentX()-idealX));
// set_motors(-5 -2 * fabs(currentX()-idealX),-5 -2 * fabs(currentX()-idealX));
}
} else if(currentX() < idealX) {
set_motors(40,40);
} else if(currentX() > idealX) {
set_motors(-40,-40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.x += 1;
}
// Going Left
if(currentDirection() == 3) {
idealX -= 60;
printf("LEFT TargetX: %f\n",idealX);
while(fabs(currentX()-idealX) > 1) {
if(fabs(currentX()-idealX) < 10) {
if(currentX() > idealX) {
correctToStraight(2 * fabs(currentX()-idealX));
// set_motors(5 + 2 * fabs(currentX()-idealX),5 + 2 * fabs(currentX()-idealX));
} else {
correctToStraight(-2 * fabs(currentX()-idealX));
// set_motors(-5 -2 * fabs(currentX()-idealX),-5 -2 * fabs(currentX()-idealX));
}
} else if(currentX() < idealX) {
set_motors(-40,-40);
} else if(currentX() > idealX) {
set_motors(40,40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.x -= 1;
}
}
void Entity::turnRight()
{
adjustAngle(-0.1);
}
void Entity::turnLeft()
{
adjustAngle(0.1);
}
void Ball::SetAngle(float angle)
{
this->angle = angle;
adjustAngle();
}
void Ball::calcAngle()
{
angle = std::atan2(vy, vx);
adjustAngle();
}
void Ball::Bounce(float wall_angle)
{
angle = 2 * (M_PI + wall_angle) - angle;
adjustAngle();
}
