void smoothForward(int distance) {
nMotorEncoder[LeftBackMotor1] = nMotorEncoder[RightBackMotor1] = 0;
// divided up into the first fourth, the middle half, the last fourth
int x1 = distance/4;
int x2 = distance*3/4;
int currentSpeed;
int newSpeed = currentSpeed;
while(isUnderValue(x1)) {
currentSpeed = motor[LeftBackMotor1] + motor[RightBackMotor1]/2;
newSpeed = (currentSpeed <= 117) ? currentSpeed + 10 : 127;
basicMove(0, newSpeed, 0);
wait1Msec(20);
}
while(isUnderValue(x2)) {
currentSpeed = motor[LeftBackMotor1] + motor[RightBackMotor1]/2;
basicMove(0, 127, 0);
}
while(isUnderValue(distance)) {
currentSpeed = motor[LeftBackMotor1] + motor[RightBackMotor1]/2;
newSpeed = (currentSpeed >= 10) ? currentSpeed - 10 : 0;
basicMove(0, newSpeed, 0);
//basicMove(0, currentSpeed - 10, 0);
wait1Msec(20);
}
}
void strafeUntilLine() {
if(morpheus == 0)
setMorpheus();
while(SensorValue[RightLine] < 2600) {
basicMove(0, 0, -67);
}
while(SensorValue[LeftLine] > 2600) {
basicMove(0, 0, -40);
}
basicMove(0, 0, -127);
wait1Msec(20);
setTank();
}
void slolRof() {
setTank();
powerArm(0);
raiseArm(650);
powerArm(10);
forward(480);
wait1Msec(1000);
back(500);
basicMove(0, 0, 0);
wait1Msec(2000);
forward(470);
wait1Msec(1000);
back(450);
basicMove(0, 0, 0);
}
void forMuricaAndForAslan() {
setTank();
forward(1600);
setMorpheus();
basicMove(0, 10, -127);
wait1Msec(1000);
basicMove(0, 0, 0);
raiseArm(1200);
powerArm(10);
setTank();
forward(470);
basicMove(0, 0, 0);
outtakeT(1200);
back(600);
basicMove(0, 0, 0);
}
void followLine() {
//int defaultLine = 2800;
int leftMotor = 60, rightMotor = 60;
//int firstDetected = 1;
//int rightOfRight = false;
//int leftOfLeft = true;
int isLeft = true;
//int timesHitLine = 0;
nMotorEncoder[LeftBackMotor1] = nMotorEncoder[RightBackMotor1] = 0;
while(isUnderValue(1400)) {
if(SensorValue[LeftLine] < 2600)
{
if(isLeft) {
leftMotor = 35;
rightMotor = 70;
isLeft = false;
if(SensorValue[RightLine] > 2600)
wait1Msec(40);
}
}
if(SensorValue[RightLine] < 2600) {
if(!isLeft) {
rightMotor = 35;
leftMotor = 70;
isLeft = true;
wait1Msec(40);
}
}
motor(LeftBackMotor1) = motor(LeftBackMotor2) = motor(LeftFrontMotor1) = leftMotor;
motor(RightBackMotor1) = motor(RightBackMotor2) = motor(RightFrontMotor1) = rightMotor;
}
basicMove(0, 0, 0);
}
void back(int distance) {
nMotorEncoder[LeftBackMotor1] = 0;
nMotorEncoder[RightBackMotor1] = 0;
while(isUnderValue(distance)) {
move(0, -127, 0);
}
move(0, 0, 0);
basicMove(0, 0, 0);
}
void hangSide() {
motor(Intake1) =motor(Intake2) = 127;
forward(34);
wait1Msec(1500);
back(60);
basicMove(0, 0, 0);
wait1Msec(2000);
/*raiseArm(300);
outtakeT(2000);*/
motor(Intake1) = motor(Intake2) = 0;
powerArm(0);
basicMove(0, 0, 0);
raiseArm(600);
powerArm(20);
waitForButton();
outtakeT(3500);
basicMove(0, 0, 0);
powerArm(0);
basicMove(0, 0, 0);
}
void forLols() {
setTank();
powerArm(0);
raiseArm(650);
powerArm(10);
motor[Intake1] = motor[Intake2] = -127;
forward(430);
wait1Msec(1000);
back(430);
basicMove(0, 0, 0);
//waitForButton();
wait1Msec(4000);
forward(480);
wait1Msec(1000);
back(500);
basicMove(0, 0, 0);
lowerArm(0);
motor[Intake1] = motor[Intake2] = 0;
basicMove(0, 0, 0);
}
void move(float targetPosition[3]) {
// This function is designed to get from place to place, using basicMove(), but to account for the asteroid.
float wayPoint[3]; // The current way point to be used.
bool isWayPoint = false; // Is the point you end up going to a wayPoint?
closestPointInIntervalToPoint(wayPoint, origin, position, targetPosition); // The wayPoint is the closes point to the asteroid.
// But, if it is inside the raidus, it needs to be adjusted.
int x = 0;
while (mathVecMagnitude(wayPoint, 3) <= dangerZoneRadius + sphereRadius + 0.02f && x < 5) {
x ++;
isWayPoint = true;
setMagnitude(wayPoint, dangerZoneRadius + sphereRadius + 0.02f);
closestPointInIntervalToPoint(wayPoint, origin, position, wayPoint); // Setting to check if there is a point along this line.
}
// If the path passes through the origin for instance, the wayPoint will be null
// Don't use paths which pass through the origin.
if (isWayPoint && mathVecMagnitude(wayPoint, 3)) {
basicMove(wayPoint, 0.04f);
} else {
basicMove(targetPosition, 0.0f);
}
}
// Most basic, scores preload, knocks over big balls.
void forGreaterJustice() {
setTank();
forward(1100);
raiseArm(1200);
powerArm(10);
//back(50);
basicMove(0, 0, 0);
motor(Intake1) =motor(Intake2) = -127;
wait1Msec(800);
back(400);
motor(Intake1) =motor(Intake2) = 0;
lowerArm(10);
back(600);
basicMove(0, 0, 0);
waitForButton();
// Push over big balls
//raiseArm(650);
powerArm(0);
//stopArm();
motor[Arm1] = motor[Arm2] = 0;
forLols();
}
/// Getters
Movement SmartAIController::getMove()
{
const auto& snake = m_pkSnake.lock();
const auto& foodManager = m_pkFoodManager.lock();
const auto& obstacleManager = m_pkObstacleManager.lock();
const auto& snakeManager = m_pkSnakeManager.lock();
if (snake && foodManager && obstacleManager && snakeManager)
{
m_lastMove = snake->getLastMove();
const auto head = snake->getHead();
Rectangle food = head;
const unsigned int avoid = m_kSetup.getGridWidth() * m_kSetup.getGridHeight() * 0.01;
if (snake->getSize() <= avoid)
{
// Only look for FatFood to prevent death
food = foodManager->findNearestFood (head, true);
}
else
{
food = foodManager->findNearestFood (head, false);
}
if (head != food)
{
m_move = chaseFood (head, food);
}
// No edible food available
else
{
m_move = safeMove();
}
}
else // weak_ptrs are invalid
{
m_move = basicMove();
}
m_lastMove = m_move;
return m_move;
}
