void apply_power(int left_power, int right_power)
{
if (direction > 0)
{
drive_custom(left_power, right_power);
}
else
{
drive_custom(-right_power, -left_power);
}
fprintf(stderr, "left: %d right: %d", left_power, right_power);
// sleep(0.1);
// all_stop();
// sleep(5);
}
int turn_right_reverse_speed(int speed){
return drive_custom(0, -speed);
}
int turn_right_speed(int speed){
return drive_custom(speed, 0);
}
int pivot_right_speed(int speed){
return drive_custom(speed, -speed);
}
int turn_left_reverse_speed(int speed){
return drive_custom(-speed, 0);
}
int turn_left_speed(int speed){
return drive_custom(0, speed);
}
int pivot_left_speed(int speed){
return drive_custom(-speed, speed);
}
int drive_speed (int speed){
return drive_custom(speed, speed);
}
void approachTargetLocation(struct RoboAI* ai, int mode){
int targetProximity = 200;
double angleThreshold = 20;
// The idea behind this version of approaching the ball
// is simple: I turn and check the angle between our facing
// and the ball. If it's decreasing, we are turning the right way;
// if not, we start turning the opposite way.
// This is to avoid the problem with acos: its always positive.
struct vector selfToTarget, selfDirection, selfToBall, goalToBall, goalToSelf, selfToKickPos;
struct vector selfToGoal;
calculateInitialVectors(&selfDirection, &selfToBall, &goalToBall, &goalToSelf, ai, SELF);
switch(mode) {
case TO_BALL: // for chase mode, or after oriented to goal - go directly to ball
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
break;
case TO_KICK_POS: // we are still approaching the optimal kicking position
selfToGoal.x = -goalToSelf.x;
selfToGoal.y = -goalToSelf.y;
if (calculateAngle(&selfToBall, &selfToGoal) < 5) {
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
} else {
calculateKickPosition(&selfToBall, &goalToBall, &goalToSelf, &selfToKickPos, ai);
selfToTarget.x = selfToKickPos.x;
selfToTarget.y = selfToKickPos.y;
}
break;
case TO_DEFENSE: // assume defensive positions
getInterceptPos(ai, &selfToTarget);
break;
default: // this should never happen, but otherwise, just go for ball
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
break;
}
printf("[%d|%d] CURRENT TARGET: [%f, %f], DIST: [%f]\n", ai->st.state, mode, selfToTarget.x, selfToTarget.y, magnitude(&selfToTarget));
// Note how the quadrant problem still exists at this point.
// I will fix this by forcing the robot to reset the motion vector
// whenever our angles start increasing in distance.
//////////////////// ! RESET FLAGGED ANGLE ! ////////////////////
// First, we check if the oldAngle variable is still valid.
// If it is flagged for a renewal, we move forwards and reset it.
if (oldAngle == -999999){
drive_speed(35);
oldAngle = calculateAngle(&selfToTarget, &selfDirection);
return;
}
//////////////////// ! HANDLE TURNING ! ////////////////////
// If we, instead, have a valid old angle, we must check if after
// turning we are increasing the angle.
double currentAngle = calculateAngle(&selfToTarget, &selfDirection);
// printf("ANGLE: %f from %f, difference of %f\n", currentAngle, oldAngle, currentAngle - oldAngle);
if (currentAngle > oldAngle && currentAngle > 5){
adjustOverturn = 1;
// There are two possible circumstances that we are concerned with.
// a) We just jumped into a different quadrant from the motion vector.
// b) We are really just turning away from the target.
// Let me first address the quadrant problem first.
//////////////////// ! QUADRANT SOLUTION ! ////////////////////
if (attemptedQuadrantSolution < 5){
// The solution to this is rather simple:
// I continue turning in the direction I already am, and
// reset the oldAngle to update it in the new quadrant.
// IF I was indeed turning in the right direction but the
// angle was messed up because of the bajanxed quadrants,
// then the (currentAngle > oldAngle) boolean above would
// then return false and we will continue on.
printf("ATTEMPTED QUADRANT SOLUTION\n");
if (attemptedQuadrantSolution < 1)
pivotRobot(25);
else if (attemptedQuadrantSolution < 2){
oldAngle = -999999;
}
else if (attemptedQuadrantSolution < 3){
drive_speed(50);
} else {
// drive_speed(25);
all_stop();
}
// if (attemptedQuadrantSolution == 4)
attemptedQuadrantSolution++;
return;
} else {
//////////////////// ! CHANGE DIRECTION ! ////////////////////
// If I got into here, I have already attempted a quadrant
// problem solution, and thus am most likely turning in the
// wrong direction. I will then change the direction I have
// been turning in, and reset the attemptedQuadrantSolution
// flag in anticipation of further quadrant problems.
printf("CHANGED DIRECTION\n");
toggleTurn = !toggleTurn;
attemptedQuadrantSolution = 0;
pivotRobot(20);
oldAngle = currentAngle; // Store the old angle to keep checking.
return;
}
} else {
// If we reached this part of the code, that means we are actually
// turning towards the target location. This is great! We can
// continue turning towards the target until the angle is within
// an acceptable range.
if (currentAngle > angleThreshold){
//////////////////// ! CONTINUE TURNING ! ////////////////////
// We need to continue turning until an optimal angle is achieved.
attemptedQuadrantSolution = 0;
if (mode == TO_BALL) {
printf("SLOW YO ROLL\n");
pivotRobot(17);
} else {
pivotRobot(20);
}
oldAngle = currentAngle;
return;
} else {
//////////////////// ! APPROACH TARGET ! ////////////////////
// We are facing the target location within an acceptable range.
if (magnitude(&selfToTarget) > targetProximity){
// Let us start moving forwards if we are not within range.
if (mode == TO_BALL) {
if (adjustOverturn){
adjustOverturn = 0;
if (toggleTurn){
drive_custom(38, 18);
} else {
drive_custom(18, 38);
}
} else {
drive_speed(20);
}
} else {
if (adjustOverturn){
adjustOverturn = 0;
if (toggleTurn){
drive_custom(18 + driveRelative(magnitude(&selfToTarget)), driveRelative(magnitude(&selfToTarget)));
} else {
drive_custom(driveRelative(magnitude(&selfToTarget)), 18 + driveRelative(magnitude(&selfToTarget)));
}
} else {
drive_speed(driveRelative(magnitude(&selfToTarget)));
}
}
attemptedQuadrantSolution = 5;
oldAngle = currentAngle;
return;
} else {
if (mode == TO_DEFENSE){
all_stop();
return;
}
// We are in an acceptable range of the target!
// Let us then increase the state of the AI directive and
// handle the different cases for different game modes.
ai->st.state ++;
return;
}
}
}
}
