int turn(float heading){
float currentHeading = getHeading(fd);
float deltaHeading = wrap180(heading - currentHeading); // deltaheading: negative means turn left, positive means turn right
const int tolerance = 5;
printf("deltaHeading: %f, currentHeading: %f, tolerance: %d\n", deltaHeading, currentHeading, tolerance);
//while(abs((int) deltaHeading) > tolerance && keepGoing){
while(keepGoing && abs(deltaHeading) > tolerance){
if(deltaHeading > 0){ //turn right
printf("**turning right**deltaHeading positive: %f, currentHeading: %f\n", deltaHeading, currentHeading);
gpio_set_value(LEFT_FWD_GPIO, 1);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_BCK_GPIO, 1);
gpio_set_value(RIGHT_FWD_GPIO, 0);
} else { // turn left
printf("**turning left**deltaHeading negative: %f, currentHeading: %f\n", deltaHeading, currentHeading);
gpio_set_value(LEFT_BCK_GPIO, 1);
gpio_set_value(LEFT_FWD_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 1);
gpio_set_value(RIGHT_BCK_GPIO, 0);
}
currentHeading = getHeading(fd);
deltaHeading = wrap180(heading - currentHeading);
}
gpio_set_value(LEFT_FWD_GPIO, 0);
gpio_set_value(LEFT_BCK_GPIO, 0);
gpio_set_value(RIGHT_FWD_GPIO, 0);
gpio_set_value(RIGHT_BCK_GPIO, 0);
}
void Hero::shoot()
{
if (munitions[currentMunition] > 0)
{
Projectile *projectile = NULL;
switch (currentMunition){
case 0: projectile = new Neutrophile(engine);
projectile->setHeading(getHeading());
break;
case 1: projectile = shootMonocyte();
break;
case 2: projectile = new Lymphocyte(engine, lymphocyteTag);
projectile->setHeading(getHeading());
break;
default: return;
break;
}
if (projectile != NULL)
{
engine->addObject(projectile);
munitions[currentMunition]--;
engine->getParentEngine()->getSoundEngine()->playSound(SoundEngine::TEST_SOUND_ID);
}
}
}
void HMC5883L::calibrate()
{
int i = 0;
float nSumMinX = 0.0f;
float nSumMaxX = 0.0f;
float nSumMinY = 0.0f;
float nSumMaxY = 0.0f;
const int nLoopCnt = 10;
for(i=0 ; i<5 ; i++)
{
int16_t x, y, z;
getHeading(&x, &y, &z);
delay(20);
}
for(i=0 ; i<nLoopCnt ; i++)
{
float nMinX = 0.0f;
float nMaxX = 0.0f;
float nMinY = 0.0f;
float nMaxY = 0.0f;
int16_t x, y, z;
getHeading(&x, &y, &z);
// Determine Min / Max values
if(x < nMinX)
nMinX = x;
if(x > nMaxX)
nMaxX = x;
if(y < nMinY)
nMinY = y;
if(y > nMaxY)
nMaxY = y;
nSumMinX += nMinX;
nSumMaxX += nMaxX;
nSumMinY += nMinY;
nSumMaxY += nMaxY;
delay(20);
}
// Calculate offsets
magoffset[0] = (int16_t)((nSumMaxX + nSumMinX) / 2 / nLoopCnt);
magoffset[1] = (int16_t)((nSumMaxY + nSumMinY) / 2 / nLoopCnt);
}
/** Sets the pitch and the roll of this vector to follow the normal given. The
* heading is taken from this vector.
* \param normal The normal vector to which pitch and roll should be aligned.
*/
void Vec3::setPitchRoll(const Vec3 &normal)
{
const float X = sin(getHeading());
const float Z = cos(getHeading());
// Compute the angle between the normal of the plane and the line to
// (x,0,z). (x,0,z) is normalised, so are the coordinates of the plane,
// which simplifies the computation of the scalar product.
float pitch = ( normal.getX()*X + normal.getZ()*Z ); // use ( x,0,z)
float roll = (-normal.getX()*Z + normal.getZ()*X ); // use (-z,0,x)
// The actual angle computed above is between the normal and the (x,y,0)
// line, so to compute the actual angles 90 degrees must be subtracted.
setPitch(-acosf(pitch) + NINETY_DEGREE_RAD);
setRoll (-acosf(roll) + NINETY_DEGREE_RAD);
} // setPitchRoll
inline void FileParse::getHead(std::ifstream& fin) {
if (fin.eof()) return;
// Create a new head node
HeadNode *node = new HeadNode;
// Look for the heading. Ends with ":", or "{". If "{" is encountered, body = true.
bool body = false;
getHeading(fin, node->heading, body);
// Message
message += (tabs() + "Level " + toStr(level) + ": Heading: [" + node->heading + "]\n");
// Set node as the current head
node->parent = currentHead;
currentHead = node;
// Look for parameters
bool newLine = passSpaces(fin);
// Get the parameters - adds them to the current head node
if (!fin.eof() && !newLine && !body)
body = getParameters(fin);
++level;
if (body && !fin.eof()) getBody(fin);
--level;
// Add this node to the parent node
node->parent->subHeads.push_back(node);
// Return to parent node
currentHead = node->parent;
}
void AISteering::turnTo(const Position &destination)
{
float uncorrectedAngleDifference = getAngleDifference(destination);
float correctedAngleDifference = uncorrectedAngleDifference;
//angle corrections
while(correctedAngleDifference <= -M_PI)
correctedAngleDifference += 2*M_PI;
while(correctedAngleDifference >= M_PI)
correctedAngleDifference -= 2*M_PI;
//check the uncorrected angle to see if you're facing the target
//if not, then steer accordingly
if( abs(correctedAngleDifference) < getDt()*bot->type->getTurnSpeed() )
{
botPosition->setR(getHeading(*botPosition, destination));
bot->turnLeft(false);
bot->turnRight(false);
}
/* if(isFacing(destination))
{
bot->turnLeft(false);
bot->turnRight(false);
}*/
else if(correctedAngleDifference > 0)
{
bot->turnLeft(false);
bot->turnRight(true);
}
else
{
bot->turnLeft(true);
bot->turnRight(false);
}
}
bool ai_agent::LOSCheck(long x1, long y1, long x2, long y2) {
//Must supply tile coordinates
if (x1 == x2) {
int aaaaaa = 0;
}
float heading = getHeading(x1*ts+(ts/2),y1*ts+(ts/2),x2*ts+(ts/2),y2*ts+(ts/2));
float distance = getDistance(x1*ts+(ts/2),y1*ts+(ts/2),x2*ts+(ts/2),y2*ts+(ts/2));
//heading += 0.00000001f;
if (distance <= ts) {
return true;
}
for (float tempDist = 0; tempDist < distance; tempDist += ts/4) {
//long tile = getRelX(physPtr1,heading,tempDist);
long xx2 = (x1*ts)+(ts/2);
long yy2 = (y1*ts)+(ts/2);
int xx1 = (int)(this->ptrWorld->ptrPhysics->getRelX(xx2,heading,tempDist)/ts);
int yy1 = (int)(this->ptrWorld->ptrPhysics->getRelY(yy2,heading,tempDist)/ts);
BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(xx1,yy1);
if (blockTemp->tl) {
return false;
}
}
return true;
}
bool ai_agent::LOSCheck(nodeInfo* node1, nodeInfo* node2) {
//float ts = ptrWorld->Map->WorldInfo.tile_size;
float heading = getHeading(node1->x*ts+(ts/2),node1->y*ts+(ts/2),node2->x*ts+(ts/2),node2->y*ts+(ts/2));
float distance = getDistance(node1->x*ts+(ts/2),node1->y*ts+(ts/2),node2->x*ts+(ts/2),node2->y*ts+(ts/2));
//heading += 0.00000001f;
if (distance <= ts) {
return true;
}
for (float tempDist = 0; tempDist < distance; tempDist += ts/4) {
//long tile = getRelX(physPtr1,heading,tempDist);
long x2 = (node1->x*ts)+(ts/2);
long y2 = (node1->y*ts)+(ts/2);
int x1 = (int)(this->ptrWorld->ptrPhysics->getRelX(x2,heading,tempDist)/ts);
int y1 = (int)(this->ptrWorld->ptrPhysics->getRelY(y2,heading,tempDist)/ts);
BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(x1,y1);
if (blockTemp->tl) {
return false;
}
}
return true;
}
//------------------------------------------------------------------------------
// atReleaseInit() -- Init weapon data at release
//------------------------------------------------------------------------------
void Missile::atReleaseInit()
{
// First the base class will setup the initial conditions
BaseClass::atReleaseInit();
if (getDynamicsModel() == 0) {
// set initial commands
cmdPitch = (LCreal) getPitch();
cmdHeading = (LCreal) getHeading();
cmdVelocity = vpMax;
if (getTargetTrack() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetTrack()->getPosition();
trng = los.length();
trngT = trng;
}
else if (getTargetPlayer() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetPosition();
trng = los.length();
trngT = trng;
}
else {
trng = 0;
}
// Range dot
trdot = 0;
trdotT = 0;
}
}
//returns a vector with 9 doubles representing the sensory
//input to the network..
boost::dynamic_bitset<> * GTPWrapper::sqlook(int * pos, bool first)
{
//int * pos = getPos(first);
// cout << "in eye: pos0(height): " << pos[0] << " pos1: " << pos[1] << endl;
boost::dynamic_bitset<> * ret = new boost::dynamic_bitset<>((int)pow(eyesize,2)*2);
// cout << "creating addr: " << ret << endl;
// ret.insert(ret.begin(),);
int ah = 0, aw = 0;
int d = (int)floor(eyesize/2);
// cout << "======= sqlook start ========= " << endl;
for(int i=0;i<eyesize;i++){
for(int i2=0;i2<eyesize;i2++){
ah = pos[0]+i-d-1;
aw = pos[1]+i2-d-1;
if( ah<0 || aw<0 || ah==bsize || aw==bsize)//(((bsize-(ah+1))*bsize)+(2*aw))<0 || (unsigned int)(((bsize-(ah+1))*bsize)+(aw*2)+1)>=board.size() || ((2*aw)+1)>=(bsize*2) )
{
// cout << "adding ovalue: i:"<<i<<" i2:"<<i2<<" pos0: "
// << pos[0] << " pos1: " << pos[1]
// << " ah: " << ah << " aw: " << aw
// << " bsize: " << bsize << " the calc to coord " << (i*eyesize*2)+(i2*2) << "," << (i*eyesize*2)+(i2*2)+1 << endl;
(*ret)[(i*eyesize*2)+(i2*2)] = 1;
(*ret)[(i*eyesize*2)+(i2*2)+1] = 1;
// cout << "ret now: " << *ret<<endl;
}
else
{
// cout << "adding goodvalue: i:"<<i<<" i2:"<<i2<<" pos0: "
// << pos[0] << " pos1: " << pos[1]
// << " ah: " << ah << " aw: " << aw << " the calc: " << ((bsize-(ah+1))*bsize)+(aw*2) << "," << ((bsize-(ah+1))*bsize)+(aw*2)+1
// << " value: " << board[((bsize-(ah+1))*bsize)+(aw*2)] << "," << board[((bsize-(ah+1))*bsize)+(aw*2)+1]
// << " at coords: " << (i*2*eyesize)+(i2*2) << "," << (i*2*eyesize)+(i2*2)+1 << endl;
// cout << "adding value ("<<ah<<","
// <<aw<<"): " << board.at(((bsize-(ah+1))*bsize)+aw) << endl;
(*ret)[(i*eyesize*2)+(i2*2)] = board[((bsize-(ah+1))*bsize*2)+(aw*2)];
(*ret)[(i*eyesize*2)+(i2*2)+1] = board[((bsize-(ah+1))*bsize*2)+(aw*2)+1];
// if(board[((bsize-(ah+1))*bsize)+(aw*2)]||board[((bsize-(ah+1))*bsize)+(aw*2)+1])
// cout << "one of them over 0 printing: " << ret << endl;
}
}
}
int * heading = getHeading(first);
// cout << "before:" <<endl << printvector(ret,eyesize);
// if(headingsupport){
if(*heading!=0){
boost::dynamic_bitset<> * tmp = ret;
if(*heading==1)
ret = twistit(ret,*heading,eyesize);
else if(*heading==2)
ret = twistit(ret,*heading,eyesize);
else if(*heading==3)
ret = twistit(ret,*heading,eyesize);
delete tmp;
}
// else
// cout << "not twisting.. " << endl;
// cout << "returning addr: " << ret << " print: " << *ret << endl;
return ret;
}
void ai_agent::UpdateTarget() {
//this->ptrWorld->los_checks++;
if (EntMemory->targetList.size() > 0) {
Ent_Target = EntMemory->targetList.front();
if (Ent_Target->ID > -2) {
if ((Ent_Target->phys_state == -1) || (Ent->ID == Ent_Target->ID)) {
AttackStop();
EntMemory->targetList.pop_front();
} else {
//Ent->target.x = EntMemory->ptrTarget->position.x;
//Ent->target.y = EntMemory->ptrTarget->position.y;
Ent->target.x = Ent_Target->position.x;
Ent->target.y = Ent_Target->position.y;
Ent->target.distance = getDistance(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
Ent->target.angle = getHeading(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
}
} else {
EntMemory->targetList.pop_front();
}
} else {
if (EntMemory->hasTargetToGuard) {
Ent_Target = EntMemory->targetGuard;
if (Ent_Target->ID > -2) {
if ((Ent_Target->phys_state == -1) || (Ent->ID == Ent_Target->ID)) {
AttackStop();
//EntMemory->targetList.pop_front();
} else {
//Ent->target.x = EntMemory->ptrTarget->position.x;
//Ent->target.y = EntMemory->ptrTarget->position.y;
Ent->target.x = Ent_Target->position.x;
Ent->target.y = Ent_Target->position.y;
Ent->target.distance = getDistance(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
Ent->target.angle = getHeading(Ent->position.x,Ent->position.y,Ent->target.x,Ent->target.y);
}
}
}
}
}
int newHeading(int a){ //(+) rep to the right, (-) rep to the left
int new_heading = getHeading()+a;
if(new_heading<0)
new_heading+=360;
else if(new_heading>360)
new_heading-=360;
return new_heading;
}
bool CFootballPlayer::isBallKickable() const
{
btVector3 ballPos = m_simulationManager->getBallPosition();
btVector3 toBall = ballPos - getPosition();
btScalar dot = getHeading().dot(toBall.normalized());
bool canKick = false;
if(dot > 0 && (getPosition().distance(ballPos) < 2)) {
canKick = true;
}
return canKick;
}
int main(int argc, char** argv) {
initTruck();
int start_time = timeInSecs();
long double start_lat = getLatitude();
long double start_lon = getLongitude();
int start_heading = getHeading();
int phase = 0;
setSteering(0);
setThrottle(50);
while(TRUE){
int target_heading;
if(phase == 0){
if(distance(start_lat, getLatitude(), start_lon, getLongitude())>5){
setThrottle(30);
setSteering(60);
phase = 1;
target_heading = newHeading(90);
}
}
else if (phase == 1){
if(getHeading()==target_heading){
setSteering(-60);
target_heading=start_heading;
phase = 2;
}
}
else if (phase == 2){
if(getHeading()==target_heading){
setSteering(0);
setThrottle(0);
while(TRUE);
}
}
background();
}
return (EXIT_SUCCESS);
}
const NAString
ElemDDLColViewDef::displayLabel2() const
{
if (isHeadingSpecified())
{
return NAString("Heading: ") + getHeading();
}
else
{
return NAString("Heading not spec.");
}
}
/**
this method is used to return the current heading of the character, specified as an angle measured in radians
*/
double Character::getHeadingAngle(){
//first we need to get the current heading of the character. Also, note that q and -q represent the same rotation
Quaternion q = getHeading();
if (q.s<0){
q.s = -q.s;
q.v = -q.v;
}
double currentHeading = 2 * safeACOS(q.s);
if (q.v.dotProductWith(PhysicsGlobals::up) < 0)
currentHeading = -currentHeading;
return currentHeading;
}
void testCompass()
{
start = getuSecs();
float heading = getHeading();
end = getuSecs();
if(heading > maxF) maxF = heading;
if(heading < minF || minF == 0) minF = heading;
dif = (int)(end-start);
if(dif > maxt) maxt = dif;
if(dif < mint || mint == 0) mint = dif;
printf("heading: %3.2f,\tmin:%3.2f,\tmax:%3.2f\n", heading, minF, maxF);
printf("elapsed: %4dus, \tmin:%dus,\tmax:%dus\n", dif, mint, maxt);
}
AREXPORT void ArActionDesired::log(void) const
{
// all those maxes and movement parameters
if (getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransVel %.0f", getMaxVel());
if (getMaxNegVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tMaxTransNegVel %.0f",
getMaxNegVel());
if (getTransAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransAccel %.0f", getTransAccel());
if (getTransDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tTransDecel %.0f", getTransDecel());
if (getMaxRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tMaxRotVel %.0f", "",
getMaxRotVel());
if (getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotAccel %.0f", "",
getRotAccel());
if (getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotDecel %.0f", "",
getRotDecel());
if (getMaxLeftLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxLeftLatVel %.0f", "",
getMaxLeftLatVel());
if (getMaxRightLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tMaxRightLatVel %.0f", "",
getMaxRightLatVel());
if (getLatAccelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatAccel %.0f", "",
getLatAccel());
if (getLatDecelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatDecel %.0f", "",
getLatDecel());
// the actual movement part
if (getVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "\tVel %.0f", getVel());
if (getHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tHeading %.0f", "",
getHeading());
if (getDeltaHeadingStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tDeltaHeading %.0f", "",
getDeltaHeading());
if (getRotVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%25s\tRotVel %.0f", "",
getRotVel());
if (getLatVelStrength() >= ArActionDesired::MIN_STRENGTH)
ArLog::log(ArLog::Normal, "%12s\tLatVel %.0f", "",
getLatVel());
}
bool ai_agent::LOSCheck(phys_obj* physPtr1, phys_obj* physPtr2) {
//this->ptrWorld->los_checks++;
float heading = getHeading(physPtr1->position.x,physPtr1->position.y,physPtr2->position.x,physPtr2->position.y);
float distance = getDistance(physPtr1->position.x,physPtr1->position.y,physPtr2->position.x,physPtr2->position.y);
for (float tempDist = 0; tempDist < distance; tempDist += ptrWorld->Map->WorldInfo.tile_size/2) {
//long tile = getRelX(physPtr1,heading,tempDist);
BLKSTR* blockTemp = this->ptrWorld->Map->MapSource->MapGetBlock(this->ptrWorld->ptrPhysics->getRelX(physPtr1->position.x,heading,tempDist)/ptrWorld->Map->WorldInfo.tile_size,this->ptrWorld->ptrPhysics->getRelY(physPtr1->position.y,heading,tempDist)/ptrWorld->Map->WorldInfo.tile_size);
if (blockTemp->tl) {
return false;
}
}
return true;
}
/*
* send calculated heading to uart3
*/
void sendHeading(void) {
short b;
char accl[20];
accl[19] = '\0';
b = usprintf(accl, "$HEAD,%i*\n", (int) getHeading()); //$HEAD,value*
#ifndef wlan
nrf24l01p_send(accl);
#else
for (int i = 0; i < b && accl[i] != '\0'; i++) {
ROM_UARTCharPut(UART3_BASE, accl[i]);
}
#endif
}
float HM55B::getStableHeading(int sample_size){
float sum = 0;
int neg_count = 0;
float heading = 0;
for (int i = 0; i < sample_size; i++){
heading = getHeading();
if (heading < 0){
neg_count++;
}
sum = sum + fabs(heading);
}
if (neg_count < (sample_size / 2)){
return sum / sample_size;
} else {
return -1 * sum / sample_size;
}
}
/**
* \par Function
* getAngle
* \par Description
* Calculate the yaw angle by the calibrated sensor value.
* \param[in]
* None
* \par Output
* None
* \return
* The angle value from 0 to 360 degrees. If not success, return an error code.
* \par Others
* Will return a correct angle when you keep the MeCompass working in the plane which have calibrated.
*/
double MeCompass::getAngle(void)
{
int16_t cx,cy,cz;
double compass_angle;
int8_t return_value = 0;
deviceCalibration();
return_value = getHeading(&cx, &cy, &cz);
if(return_value != 0)
{
return (double)return_value;
}
if(Calibration_Flag == true)
{
cx = (cx + Cal_parameter.X_excursion) * Cal_parameter.X_gain;
cy = (cy + Cal_parameter.Y_excursion) * Cal_parameter.Y_gain;
cz = (cz + Cal_parameter.Z_excursion) * Cal_parameter.Z_gain;
if(Cal_parameter.Rotation_Axis == 1) //X_Axis
{
compass_angle = atan2( (double)cy, (double)cz );
}
else if(Cal_parameter.Rotation_Axis == 2) //Y_Axis
{
compass_angle = atan2( (double)cx, (double)cz );
}
else if(Cal_parameter.Rotation_Axis == 3) //Z_Axis
{
compass_angle = atan2( (double)cy, (double)cx );
}
}
else
{
compass_angle = atan2( (double)cy, (double)cx );
}
if(compass_angle < 0)
{
compass_angle = (compass_angle + 2 * COMPASS_PI) * 180 / COMPASS_PI;
}
else
{
compass_angle = compass_angle * 180 / COMPASS_PI;
}
return compass_angle;
}
int writeDatalink(float frequency){
if (time - lastTime > frequency) {
lastTime = time;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
statusData->debugChar = debugChar;
statusData->debugInt = debugInt;
statusData->debugFloat = debugFloat;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (time < lastTime){
lastTime = time;
return 0;
}
return 0;
}
void HMC5883L::Calibration(int count)
{
//평균 계산을 위한 변수
float average_offset = 0;
int16_t tmp_x = 0, tmp_y = 0, tmp_z = 0;
//현재 offset 초기화
Heading_offset = 0;
for(int i = 0; i < count; i++)
{
float tmp_yaw;
getHeading(&tmp_x, &tmp_y, &tmp_z);
getYaw(tmp_x, tmp_y, &tmp_yaw);
if(tmp_yaw < 0) tmp_yaw *= -1;
average_offset += tmp_yaw;
}
//새로운 offset 적용
Heading_offset = average_offset / count;
}
task CalcHeading()
{
prevTime =currTime = nPgmTime;
while(true)
{
eraseDisplay();
getHeading() ;
nxtDisplayTextLine(6, "heading %1.4f", heading);
if(heading > -5)
gPwr = 20;
else if(heading < 5)
gPwr = -20;
else
{
gPwr = 0;
}
wait1Msec(10);
}
}
//
// Vector getRelationship(target)
// Last modified: 03Sep2006
//
// Returns the relationship from this robot
// to the parameterized target vector.
//
// Returns: the relationship from this robot to the target vector
// Parameters:
// target in/out the target vector being related to
//
Vector Robot::getRelationship(Vector &target) const
{
Vector temp = target - *this;
temp.rotateRelative(-getHeading());
return temp;
} // getRelationship(Vector &) const
//
// void translateRelative(v)
// Last modified: 03Sep2006
//
// Translates the robot relative to itself based
// on the parameterized translation vector.
//
// Returns: <none>
// Parameters:
// v in the translation vector
//
void Robot::translateRelative(Vector v)
{
v.rotateRelative(getHeading());
x += v.x;
y += v.y;
} // rotateRelative(const Vector)
float getCompassHeading(void){
return getHeading(fd);
}
void Spherical::setLocation(double lat, double lon) {
set(lat, lon, getHeading());
}
// Function called when timer expires
void SysTick_Handler(void)
{
int currentHeading = 0;
currentHeading = getHeading();
printf("Heading: %d\n", currentHeading);
}