void Robot::calibrateGrid()
{
int i = 0;
for(i = 0; i < 25; i++)
{
gridSensors.calibrate();
delay(20);
}
turnTo(NWEST);
delay(500);
moveTicks(AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED);
for(i = 0; i < 25; i++)
{
gridSensors.calibrate();
delay(20);
}
moveTicks(-AFEW, APPROACHSPEED);
stopMoving(APPROACHSPEED, true);
delay(500);
turnTo(NORTH);
delay(500);
}
// TODO: This function may need to be re-coded because it is too specific for
// the code we're on. It might be best to have something like this in the AI part.
// It might be best to just have openClaws and closeClaws.
void Robot::dropBlock()
{
unsigned int sensors[8];
bool atPoint = false;
bool delivered = false;
int ticks_moved=0;
turnTo(SOUTH);
movePoints(1);
moveTicks(60, 25);
stopMoving(25);
openClaws();
delay(300);
moveTicks(-50, 25);
stopMoving(25, true);
delay(300);
turnTo(NORTH);
delay(300);
movePoints(1);
delay(500);
/*while (!delivered)
{
ticks_moved = 0;
while ((ticks_moved < 70) && !atPoint)
{
//Go Forward
moveTicks(2, 25);
ticks_moved = ticks_moved + wheelEnc.getCountsM1();
gridSensors.readCalibrated(sensors, QTR_EMITTERS_ON);
if ((sensors[0] < SENSORBLACK) && (sensors[7] < SENSORBLACK) && (ticks_moved >10))
{
motorStop();
delay(2000);
atPoint = true;
servo(SERVO_RIGHT,30); //Check required values.
servo(SERVO_LEFT,80);
moveTicks(-ticks_moved, 20);
delivered = true;
}
}
motorStop();
delay(2000);
atPoint = false;
moveTicks(-ticks_moved, 20);
}*/
}
/**************************************
* Definition: Turns the robot in a relative direction
* the specified number of radians
*
* Parameters: int specifying direction and a float
* specifying radians (0..2PI)
**************************************/
void Robot::turn(int relDirection, float radians) {
float goalTheta;
if (relDirection == DIR_LEFT) {
goalTheta = Util::normalizeTheta(_northStar->getTheta()+radians);
turnTo(goalTheta, MAX_THETA_ERROR);
}
else {
goalTheta = Util::normalizeTheta(_northStar->getTheta()-radians);
turnTo(goalTheta, MAX_THETA_ERROR);
}
}
/************************************
* Definition: Turns the robot in the cardinal direction
* given as the parameter
*
* Parameters: A cardinal direction constant as follows
* DIR_NORTH, DIR_SOUTH, DIR_EAST, DIR_WEST
***********************************/
void Robot::turn(int direction) {
switch (direction) {
case DIR_NORTH:
turnTo(DEGREE_90, MAX_THETA_ERROR);
break;
case DIR_SOUTH:
turnTo(DEGREE_270, MAX_THETA_ERROR);
break;
case DIR_EAST:
turnTo(DEGREE_0, MAX_THETA_ERROR);
break;
case DIR_WEST:
turnTo(DEGREE_180, MAX_THETA_ERROR);
break;
}
}
void executeStep(Step *step){
writeDebugStreamLine("Beginning execution of step \"%s\"", step->name);
switch (step->type)
{
case StepDrive:
moveTo(step->target, step->power, step->forward);
break;
case StepTurnDrive:
turnAndMoveTo(step->target, step->power, step->forward);
break;
case StepTurn:
turnTo(step->target, step->power, step->forward);
break;
case StepGrab:
grabGoal();
wait1Msec(500);
break;
case StepRelease:
releaseGoal();
wait1Msec(500);
break;
case StepDone:
writeDebugStreamLine("Finished executing Autonomous program");
StopAllTasks();
break;
default:
writeDebugStreamLine("Unknown step type detected (%d). Quitting now.", step->type);
StopAllTasks();
}
}
void Actor::updateWalk() {
if (_path.empty()) {
return;
}
Math::Vector3d destPos = _path.back();
Math::Vector3d dir = destPos - _pos;
float dist = dir.getMagnitude();
_walkedCur = true;
float walkAmt = g_grim->getPerSecond(_walkRate);
if (walkAmt >= dist) {
_path.pop_back();
if (_path.empty()) {
_walking = false;
_pos = destPos;
// It seems that we need to allow an already active turning motion to
// continue or else turning actors away from barriers won't work right
_turning = false;
return;
}
}
destPos = handleCollisionTo(_pos, destPos);
Math::Angle y = getYawTo(destPos);
turnTo(_pitch, y, _roll);
dir = destPos - _pos;
dir.normalize();
_pos += dir * walkAmt;
}
//this actually causes the ship to move to a waypoint
void AISteering::moveTo(const Position &destination)
{
turnTo(destination);
if (isFacing(destination))
bot->thrust(true);
}
void AiTankController::moveTo(const Point2f &pos, const Point2f &nextPos)
{
if (!isAttached())
return;
turnTo(pos);
bCheckMoveTo = true;
moveToPos = pos;
nextMoveToPos = nextPos;
}
void produceTrees_winter()
{
trees_wi = glGenLists(1);
glNewList(trees_wi, GL_COMPILE);
leafColor = WHITE;
leafScale = 1.8;
glColor3f(0.5, 0.5, 0);
moveTo(0.0, 0.0);
turnTo(90);
produceString(Fstr, 2, 1);
glEndList();
}
void produceTrees_autumn()
{
trees_au = glGenLists(1);
glNewList(trees_au, GL_COMPILE);
leafColor = ORANGE;
leafScale = 3.5;
glColor3f(0.5, 0.5, 0);
moveTo(0.0, 0.0);
turnTo(90);
produceString(Fstr, 2, 1);
glEndList();
}
void produceTrees_spring()
{
trees_sp = glGenLists(1);
glNewList(trees_sp, GL_COMPILE);
leafColor = PINK;
leafScale = 2.0;
glColor3f(0.5, 0.5, 0);
moveTo(0.0, 0.0);
turnTo(90);
produceString(Fstr, 2, 1);
glEndList();
}
void produceTrees_summer()
{
trees_su = glGenLists(1);
glNewList(trees_su, GL_COMPILE);
leafColor = GREEN;
leafScale = 3.5;
glColor3f(0.5, 0.5, 0);
moveTo(0.0, 0.0);
turnTo(90);
produceString(Fstr, 2, 1);
glEndList();
}
void AiTankController::stepMoveTo()
{
if (!isAttached())
{
bCheckMoveTo = false;
return;
}
Point2f posTank;
float orientionTank;
Entity &e = entity->getEntity();
if (e.getStatus(Entity::ESI_Position, &posTank) &&
e.getStatus(Entity::ESI_Orientation, &orientionTank))
{
//#ifndef NDEBUG
iRenderQueue::getSingleton().render(moveToPos, posTank, ARGB(255, 0, 0, 0));
//#endif
Point2f dir = moveToPos - posTank;
float len = dir.Length();
float len2 = (nextMoveToPos - posTank).Length();
if (len < 10 || len > len2)
{
bCheckMoveTo = false;
bForward = false;
sendCommand(Tank::TCI_Forward, (void *)bForward);
return;
}
dir.Normalize();
if (fabs(dir ^ Point2f(cos(orientionTank), sin(orientionTank))) < 0.3f)
{
if (!bForward)
{
bForward = true;
sendCommand(Tank::TCI_Forward, (void *)bForward);
}
}
else
{
float lerpValue = (float)rand() / RAND_MAX;
lerpValue *= lerpValue;
turnTo(moveToPos);//lerp(moveToPos, nextMoveToPos, lerpValue));
if (bForward)
{
bForward = false;
sendCommand(Tank::TCI_Forward, (void *)bForward);
}
}
}
else
{
bCheckMoveTo = false;
}
}
void Ship::approach()
{
target_angle = getAngleBetween(getPosition(), target);
turnTo(Angle(target_angle));
if (getDistanceBetween(getPosition(), target) < getGlobalBounds().width * 2.0f)
{
stop();
setRotation(target_angle);
}
else
{
correctSpeed();
}
}
void AdActor::initLine(const BasePoint &startPt, const BasePoint &endPt) {
_pFCount = MAX((abs(endPt.x - startPt.x)) , (abs(endPt.y - startPt.y)));
_pFStepX = (double)(endPt.x - startPt.x) / _pFCount;
_pFStepY = (double)(endPt.y - startPt.y) / _pFCount;
_pFX = startPt.x;
_pFY = startPt.y;
int angle = (int)(atan2((double)(endPt.y - startPt.y), (double)(endPt.x - startPt.x)) * (180 / 3.14));
_nextState = STATE_FOLLOWING_PATH;
turnTo(angleToDirection(angle));
}
// actions
int bt_scientist::actionAimToPos() {
//Look to next target position
stuck = false;
stuck_time = 0.f;
if (turnTo(obj_position)) {
SetMyEntity(); //needed in case address Entity moved by handle_manager
if (!myEntity) return KO;
TCompTransform *me_transform = myEntity->get<TCompTransform>();
VEC3 myPos = me_transform->getPosition();
getPath(myPos, obj_position);
SET_ANIM_SCI_BT(AST_IDLE);
return OK;
}
else {
SET_ANIM_SCI_BT(AST_MOVE);
return STAY;
}
}
int bt_scientist::actionNextWpt() {
PROFILE_FUNCTION("scientist: actionnextwpt");
if (!myParent.isValid()) return false;
//animController.setState(AST_TURN);
VEC3 dest = keyPoints[curkpt].pos;
//Look to waypoint
if (turnTo(dest)) {
SetMyEntity(); //needed in case address Entity moved by handle_manager
if (!myEntity) return KO;
TCompTransform *me_transform = myEntity->get<TCompTransform>();
VEC3 myPos = me_transform->getPosition();
getPath(myPos, dest);
return OK;
}
else {
SET_ANIM_SCI_BT(AST_MOVE);
return STAY;
}
}
void AdActor::followPath() {
// skip current position
_path->getFirst();
while (_path->getCurrent() != nullptr) {
if (_path->getCurrent()->x != _posX || _path->getCurrent()->y != _posY) {
break;
}
_path->getNext();
}
// are there points to follow?
if (_path->getCurrent() != nullptr) {
_state = STATE_FOLLOWING_PATH;
initLine(BasePoint(_posX, _posY), *_path->getCurrent());
} else {
if (_afterWalkDir != DI_NONE) {
turnTo(_afterWalkDir);
} else {
_state = STATE_READY;
}
}
}
int bt_scientist::actionSeekWpt() {
PROFILE_FUNCTION("scientist: actionseekwpt");
SetMyEntity(); //needed in case address Entity moved by handle_manager
if (!myEntity) return KO;
TCompTransform *me_transform = myEntity->get<TCompTransform>();
VEC3 myPos = me_transform->getPosition();
VEC3 dest = keyPoints[curkpt].pos;
//Go to waypoint
if (keyPoints[curkpt].type == Seek) {
//reach waypoint?
if (simpleDistXZ(myPos, dest) < DIST_REACH_PNT) {
curkpt = (curkpt + 1) % keyPoints.size();
return OK;
}
else {
//getPath(myPos, dest);
//animController.setState(AST_RUN);
goTo(dest);
SET_ANIM_SCI_BT(AST_MOVE);
return STAY;
}
}
//Look to waypoint
else if (keyPoints[curkpt].type == Look) {
//Look to waypoint
if (turnTo(dest)) {
curkpt = (curkpt + 1) % keyPoints.size();
return OK;
}
else {
SET_ANIM_SCI_BT(AST_MOVE);
return STAY;
}
}
return OK;
}
/**************************************
* Definition: Moves the robot to the specified location in the global
* coord system, disregarding cells
*
* Parameters: floats specifying x and y in global system
**************************************/
void Robot::moveTo(float x, float y, float distErrorLimit) {
printf("beginning move\n");
float thetaError;
do {
// move to the location until theta is off by too much
thetaError = moveToUntil(x, y, distErrorLimit, MAX_THETA_ERROR);
float goal = Util::normalizeTheta(_northStar->getTheta() + thetaError);
if (thetaError != 0) {
// if we're off in theta, turn to adjust
turnTo(goal, MAX_THETA_ERROR);
}
}
while (thetaError != 0);
_movePID->flushPID();
_turnPID->flushPID();
// reset wheel encoder pose to be Kalman pose since we hit our base
_wheelEncoders->resetPose(_pose);
_wheelEncoders->setTheta(_northStar->getTheta());
printf("done moving\n");
}
void Lapiz::lookAt(PV2D *p){
turnTo(0);//revisar
turn(atan2(p->getY()-pos->getY(),p->getX()-pos->getX()));
}
void HwSceneObject::turnTo(HwSceneObject *obj)
{
turnTo(obj->getPos());
}
//////////////////////////////////////////////////////////////////////////
// high level scripting interface
//////////////////////////////////////////////////////////////////////////
bool AdActor::scCallMethod(ScScript *script, ScStack *stack, ScStack *thisStack, const char *name) {
//////////////////////////////////////////////////////////////////////////
// GoTo / GoToAsync
//////////////////////////////////////////////////////////////////////////
if (strcmp(name, "GoTo") == 0 || strcmp(name, "GoToAsync") == 0) {
stack->correctParams(2);
int x = stack->pop()->getInt();
int y = stack->pop()->getInt();
goTo(x, y);
if (strcmp(name, "GoToAsync") != 0) {
script->waitForExclusive(this);
}
stack->pushNULL();
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// GoToObject / GoToObjectAsync
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "GoToObject") == 0 || strcmp(name, "GoToObjectAsync") == 0) {
stack->correctParams(1);
ScValue *val = stack->pop();
if (!val->isNative()) {
script->runtimeError("actor.%s method accepts an entity refrence only", name);
stack->pushNULL();
return STATUS_OK;
}
AdObject *obj = (AdObject *)val->getNative();
if (!obj || obj->getType() != OBJECT_ENTITY) {
script->runtimeError("actor.%s method accepts an entity refrence only", name);
stack->pushNULL();
return STATUS_OK;
}
AdEntity *ent = (AdEntity *)obj;
if (ent->getWalkToX() == 0 && ent->getWalkToY() == 0) {
goTo(ent->_posX, ent->_posY);
} else {
goTo(ent->getWalkToX(), ent->getWalkToY(), ent->getWalkToDir());
}
if (strcmp(name, "GoToObjectAsync") != 0) {
script->waitForExclusive(this);
}
stack->pushNULL();
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// TurnTo / TurnToAsync
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "TurnTo") == 0 || strcmp(name, "TurnToAsync") == 0) {
stack->correctParams(1);
int dir;
ScValue *val = stack->pop();
// turn to object?
if (val->isNative() && _gameRef->validObject((BaseObject *)val->getNative())) {
BaseObject *obj = (BaseObject *)val->getNative();
int angle = (int)(atan2((double)(obj->_posY - _posY), (double)(obj->_posX - _posX)) * (180 / 3.14));
dir = (int)angleToDirection(angle);
}
// otherwise turn to direction
else {
dir = val->getInt();
}
if (dir >= 0 && dir < NUM_DIRECTIONS) {
turnTo((TDirection)dir);
if (strcmp(name, "TurnToAsync") != 0) {
script->waitForExclusive(this);
}
}
stack->pushNULL();
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// IsWalking
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "IsWalking") == 0) {
stack->correctParams(0);
stack->pushBool(_state == STATE_FOLLOWING_PATH);
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// MergeAnims
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "MergeAnims") == 0) {
stack->correctParams(1);
stack->pushBool(DID_SUCCEED(mergeAnims(stack->pop()->getString())));
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// UnloadAnim
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "UnloadAnim") == 0) {
stack->correctParams(1);
const char *animName = stack->pop()->getString();
bool found = false;
for (uint32 i = 0; i < _anims.size(); i++) {
if (scumm_stricmp(_anims[i]->getName(), animName) == 0) {
// invalidate sprites in use
if (_anims[i]->containsSprite(_tempSprite2)) {
_tempSprite2 = nullptr;
}
if (_anims[i]->containsSprite(_currentSprite)) {
_currentSprite = nullptr;
}
if (_anims[i]->containsSprite(_animSprite2)) {
_animSprite2 = nullptr;
}
delete _anims[i];
_anims[i] = nullptr;
_anims.remove_at(i);
i--;
found = true;
}
}
stack->pushBool(found);
return STATUS_OK;
}
//////////////////////////////////////////////////////////////////////////
// HasAnim
//////////////////////////////////////////////////////////////////////////
else if (strcmp(name, "HasAnim") == 0) {
stack->correctParams(1);
const char *animName = stack->pop()->getString();
stack->pushBool(getAnimByName(animName) != nullptr);
return STATUS_OK;
} else {
return AdTalkHolder::scCallMethod(script, stack, thisStack, name);
}
}
void AdActor::getNextStep() {
if (_walkSprite) {
_currentSprite = _walkSprite->getSprite(_dir);
} else {
AdSpriteSet *anim = getAnimByName(_walkAnimName);
if (anim) {
_currentSprite = anim->getSprite(_dir);
}
}
if (!_currentSprite) {
return;
}
_currentSprite->getCurrentFrame(_zoomable ? ((AdGame *)_gameRef)->_scene->getZoomAt(_posX, _posY) : 100, _zoomable ? ((AdGame *)_gameRef)->_scene->getZoomAt(_posX, _posY) : 100);
if (!_currentSprite->isChanged()) {
return;
}
int maxStepX, maxStepY;
maxStepX = abs(_currentSprite->_moveX);
maxStepY = abs(_currentSprite->_moveY);
maxStepX = MAX(maxStepX, maxStepY);
maxStepX = MAX(maxStepX, 1);
while (_pFCount > 0 && maxStepX >= 0) {
_pFX += _pFStepX;
_pFY += _pFStepY;
_pFCount--;
maxStepX--;
}
if (((AdGame *)_gameRef)->_scene->isBlockedAt((int)_pFX, (int) _pFY, true, this)) {
if (_pFCount == 0) {
_state = _nextState;
_nextState = STATE_READY;
return;
}
goTo(_targetPoint->x, _targetPoint->y);
return;
}
_posX = (int)_pFX;
_posY = (int)_pFY;
afterMove();
if (_pFCount == 0) {
if (_path->getNext() == nullptr) {
_posX = _targetPoint->x;
_posY = _targetPoint->y;
_path->reset();
if (_afterWalkDir != DI_NONE) {
turnTo(_afterWalkDir);
} else {
_state = _nextState;
_nextState = STATE_READY;
}
} else {
initLine(BasePoint(_posX, _posY), *_path->getCurrent());
}
}
}
void Actor::walkForward() {
float dist = g_grim->getPerSecond(_walkRate);
// Limit the amount of the movement per frame, otherwise with low fps
// scripts that use WalkActorForward and proximity may break.
if ((dist > 0 && dist > _walkRate / 5.f) || (dist < 0 && dist < _walkRate / 5.f))
dist = _walkRate / 5.f;
_walking = false;
if (! _constrain) {
Math::Vector3d forwardVec(-_moveYaw.getSine() * _pitch.getCosine(),
_moveYaw.getCosine() * _pitch.getCosine(), _pitch.getSine());
// EMI: Y is up-down, sectors use an X-Z plane for movement
if (g_grim->getGameType() == GType_MONKEY4) {
float temp = forwardVec.z();
forwardVec.z() = forwardVec.y();
forwardVec.y() = temp;
}
_pos += forwardVec * dist;
_walkedCur = true;
return;
}
bool backwards = false;
if (dist < 0) {
dist = -dist;
backwards = true;
}
int tries = 0;
while (dist > 0.0f) {
Math::Vector3d forwardVec(-_moveYaw.getSine() * _pitch.getCosine(),
_moveYaw.getCosine() * _pitch.getCosine(), _pitch.getSine());
// EMI: Y is up-down, sectors use an X-Z plane for movement
if (g_grim->getGameType() == GType_MONKEY4) {
float temp = forwardVec.z();
forwardVec.z() = forwardVec.y();
forwardVec.y() = temp;
}
if (backwards)
forwardVec = -forwardVec;
Sector *currSector = NULL, *prevSector = NULL, *startSector = NULL;
Sector::ExitInfo ei;
g_grim->getCurrSet()->findClosestSector(_pos, &currSector, &_pos);
if (!currSector) { // Shouldn't happen...
moveTo(_pos + forwardVec * dist);
_walkedCur = true;
return;
}
startSector = currSector;
float oldDist = dist;
while (currSector) {
prevSector = currSector;
Math::Vector3d puckVec = currSector->getProjectionToPuckVector(forwardVec);
puckVec /= puckVec.getMagnitude();
currSector->getExitInfo(_pos, puckVec, &ei);
float exitDist = (ei.exitPoint - _pos).getMagnitude();
if (dist < exitDist) {
moveTo(_pos + puckVec * dist);
_walkedCur = true;
return;
}
_pos = ei.exitPoint;
dist -= exitDist;
if (exitDist > 0.0001)
_walkedCur = true;
// Check for an adjacent sector which can continue
// the path
currSector = g_grim->getCurrSet()->findPointSector(ei.exitPoint + (float)0.0001 * puckVec, Sector::WalkType);
// EMI: some sectors are significantly higher/lower than others.
if (currSector && g_grim->getGameType() == GType_MONKEY4) {
float planeDist = currSector->distanceToPoint(_pos);
if (fabs(planeDist) < 1.f)
_pos -= planeDist * currSector->getNormal();
}
if (currSector == prevSector || currSector == startSector)
break;
}
int turnDir = 1;
if (ei.angleWithEdge > 90) {
ei.angleWithEdge = 180 - ei.angleWithEdge;
ei.edgeDir = -ei.edgeDir;
turnDir = -1;
}
if (ei.angleWithEdge > _reflectionAngle)
return;
ei.angleWithEdge += (float)1.0f;
turnTo(0, _moveYaw + ei.angleWithEdge * turnDir, 0);
if (oldDist <= dist + 0.001f) {
// If we didn't move at all, keep trying a couple more times
// in case we can move in the new direction.
tries++;
if (tries > 3)
break;
}
}
}
//Aux
bool CPlayerBase::turnTo(TCompTransform * t)
{
return turnTo(t->getPosition());
}
void PTGObject::turnTo(PTGObject *obj)
{
turnTo(obj->pos);
}
/**************************************
* Definition: Strafes the robot until it is considered centered
* between two squares in a corridor
**************************************/
void Robot::center() {
moveHead(RI_HEAD_MIDDLE);
int turnAttempts = 0;
Camera::prevTagState = -1;
while (true) {
bool turn = false;
float centerError = _camera->centerError(COLOR_PINK, &turn);
if (turn) {
if (_centerTurn(centerError)) {
break;
}
turnAttempts++;
}
else {
if (_centerStrafe(centerError)) {
break;
}
}
if (turnAttempts > 2) {
moveHead(RI_HEAD_DOWN);
// make sure we are close to our desired heading, in case we didn't move correctly
updatePose(false);
float thetaHeading;
switch (_heading) {
case DIR_NORTH:
thetaHeading = DEGREE_90;
break;
case DIR_SOUTH:
thetaHeading = DEGREE_270;
break;
case DIR_EAST:
thetaHeading = DEGREE_0;
break;
case DIR_WEST:
thetaHeading = DEGREE_180;
break;
}
float theta = _pose->getTheta();
float thetaError = thetaHeading - theta;
thetaError = Util::normalizeThetaError(thetaError);
if (fabs(thetaError) > DEGREE_45) {
// if we turned beyond 45 degrees from our
// heading, this was a mistake. let's turn
// back just enough so we're 45 degrees from
// our heading.
float thetaGoal = Util::normalizeTheta(theta + (DEGREE_45 + thetaError));
turnTo(thetaGoal, MAX_THETA_ERROR);
}
turnAttempts = 0;
moveHead(RI_HEAD_MIDDLE);
}
}
moveHead(RI_HEAD_DOWN);
_centerTurnPID->flushPID();
_centerStrafePID->flushPID();
}
