#include #include #include "ZRGame.h" #include "ZR_API.h" #include "spheres_types.h" #include "spheres_constants.h" #include "ctrl_attitude.h" #include "ctrl_position.h" #include "find_state_error.h" #include "math_matrix.h" #ifdef ZRSIMULATION extern void _init(void); void *_start = &_init; #endif #undef ZRSIMULATION static float trgPos[3]; //DECL::VAR::trgPos static float trgVel[3]; //DECL::VAR::trgVel static float sumPos[3]; //DECL::VAR::sumPos static int sumPosCnt; //DECL::VAR::sumPosCnt static float trgAtt[4]; //DECL::VAR::trgAtt static float trgVFin[3]; //DECL::VAR::trgVFin static float refState[13]; //DECL::VAR::refState static float objState[13]; //DECL::VAR::objState static float trgRot[3]; //DECL::VAR::trgRot static float AvObjectState[13]; //DECL::VAR::AvObjectState static float table[2940]; //DECL::VAR::table static int impactTime; //DECL::VAR::impactTime static int unObTime; //DECL::VAR::unObTime static int deflecting; //DECL::VAR::deflecting static int deflected; //DECL::VAR::deflected static int travelMode; //DECL::VAR::travelMode static float capState[13]; //DECL::VAR::capState static float predState[13]; //DECL::VAR::predState static float highCapState[13]; //DECL::VAR::highCapState static float highPredCapState[13]; //DECL::VAR::highPredCapState static float predCapState[13]; //DECL::VAR::predCapState static float margin; //DECL::VAR::margin static float currHighCapFromUs[3]; //DECL::VAR::currHighCapFromUs static int inCone; //DECL::VAR::inCone static int inCapture; //DECL::VAR::inCapture static float currDistToHighCap; //DECL::VAR::currDistToHighCap static float currDistToCap; //DECL::VAR::currDistToCap static float currHighCapFromObj[3]; //DECL::VAR::currHighCapFromObj static float savState[13]; //DECL::VAR::savState static int routeBlocked; //DECL::VAR::routeBlocked static int tanTime; //DECL::VAR::tanTime static float tanObjState[13]; //DECL::VAR::tanObjState static float tanCapState[13]; //DECL::VAR::tanCapState static float tanHighCapState[13]; //DECL::VAR::tanHighCapState static float impactObjState[13]; //DECL::VAR::impactObjState static float impactCapState[13]; //DECL::VAR::impactCapState static float impactHighCapState[13]; //DECL::VAR::impactHighCapState static int docking; //DECL::VAR::docking static float projectedStates[2743]; //DECL::VAR::projectedStates static float projectedCapStates[2743]; //DECL::VAR::projectedCapStates static float projectedHighCapStates[2743]; //DECL::VAR::projectedHighCapStates static float zero[13]; //DECL::VAR::zero static int time; //DECL::VAR::time static int coneCount; //DECL::VAR::coneCount static float baseState[13]; //DECL::VAR::baseState static int baseCnt; //DECL::VAR::baseCnt static float incRot[4]; //DECL::VAR::incRot static float omegaMag; //DECL::VAR::omegaMag static int highSpin; //DECL::VAR::highSpin static float orientationDot; //DECL::VAR::orientationDot static void mathVecScale (float *vout, float *vin, float scaler, int norm); //DECL::PROC::mathVecScale static void mathVecDiffScaleFrom (float *vOut, float *vHere, float *vThere, float scaler, int norm, float *vFrom); //DECL::PROC::mathVecDiffScaleFrom static void mathAxisAngle2Quat (float quat[4], float axis[3], float angle); //DECL::PROC::mathAxisAngle2Quat static void mathQuatDiff (float *qOut, float *qThere, float *qHere); //DECL::PROC::mathQuatDiff static void ChangeToObjFrame (float state[13]); //DECL::PROC::ChangeToObjFrame static void CalcVel2TargOverTime (float *vel, float *refState, float *trgState, float seconds); //DECL::PROC::CalcVel2TargOverTime static void DebugState (char *txt, float *state); //DECL::PROC::DebugState static void mathQuat2AxisAngle (float *quat, float *axis, float *angle); //DECL::PROC::mathQuat2AxisAngle static void calcBasicStateInfo (); //DECL::PROC::calcBasicStateInfo static void FireThrusters (float *control, float *target, int options); //DECL::PROC::FireThrusters static void FlyToStateInSeconds (float *targState, int seconds, int thrusterOptions, int calcOptions); //DECL::PROC::FlyToStateInSeconds static void JakesDock (int high); //DECL::PROC::JakesDock static void FlyOrbitToHighCap (); //DECL::PROC::FlyOrbitToHighCap void ZRUser01() { //BEGIN::PROC::ZRUser #define dv(a,b,c) DEBUG(("\n %s:\tx:%f\ty:%f\tz:%f\tm:%f",a,b[c+0],b[c+1],b[c+2],mathVecMagnitude(&b[c],3))) #define dq(a,b,c) DEBUG(("\n %s:\tx:%f\ty:%f\tz:%f\tw:%f\tm:%f",a,b[c+0],b[c+1],b[c+2],b[c+3],mathVecMagnitude(&b[c],4))) #define dt(a) DEBUG(("\n%s",a)) #define posOf(a) &a[0] #define velOf(a) &a[3] #define attOf(a) &a[6] #define quatOf(a) &a[6] #define rotOf(a) &a[10] // thruster/thruster calc Options #define FirePos 1 #define FireVel 2 #define FireAtt 4 #define FireQuat 8 #define FireRot 16 // flight modes #define FLY_STRAIGHT 0 #define FLY_SKATE 1 #define FLY_CONE 2 #define FLY_CAPTURE 3 //#define FLY_TANGENT 4 #define FLY_HIGHCAPTURE 5 // // Global Debug Control // // #define debugMode 1 int minInBounds; // = 35; int i; int tmpTime; int options; float origStart[3]; //={0.0f,-.6f,0.0f}; float tanPointOvererage; // = .00f; float deflectUnderage; // = .00f; int ch1; int ch2; int ch3; float absPos[3]; float tmpState[13]; //float towardsUs[3]; // = {-1.0f,0.0f,0.0f}; float oCheck1[3]; float oCheck2[3]; float oRef[3]; //float wallMargin=.2; minInBounds = 35; margin = .35f; //towardsUs[0] = 0.0f; //towardsUs[1] = -1.0f; //towardsUs[2] = 0.0f; deflectUnderage = .00f; tanPointOvererage = .00f; origStart[0] = 0.0f; origStart[1] = -.6f; origStart[2] = 0.0f; oRef[0]=-1.0f; oRef[1]= 0.0f; oRef[2]= 0.0f; #ifdef debugMode DEBUG(("\n\n\nTime: %d",time)); #endif #ifdef debugMode if(time<=0) { ZRGetMySphState(tmpState); baseCnt++; mathVecAdd(baseState,baseState,tmpState,13); ACGetObjectState(&projectedStates[0]); mathVecAdd(absPos,tmpState,&projectedStates[0],3); // #ifdef debugMode // dv("absPos",absPos,0); // #endif // DebugState("r2d2:",tmpState); } #endif if(time==0) { unsigned char c1; unsigned char c2; int inBoundsCount=0; float routeScore = 0.0f; float bestScore = -1.0f; float tvec[3]; float dot; float score; float tmpVel[3]; float tmpState[13]; #ifdef debugMode mathVecScale(baseState,baseState,1.0f/baseCnt,0); DebugState("baseSphState",baseState); ACGetObjectState(tmpState); DebugState("baseObjState",tmpState); #endif ACGetObjectState(&projectedStates[0]); // High Spin detection // This should be based on Y and Z only // X doesn't matter and it should be make a global // so other places just need to check the value (highSpin) // The following line is not a typo! We only want the magnitude of the // y & z components. omegaMag = mathVecMagnitude(&projectedStates[11],2); if(omegaMag>=.02) { highSpin = 1; minInBounds = 55; } for(i=0;i<210;i++) { float hiPos[3]; int ptr; score=0; ptr = i*13; ACGetCaptureState(&projectedStates[ptr],&projectedCapStates[ptr]); mathVecDiffScaleFrom(&projectedHighCapStates[ptr],&projectedStates[ptr],&projectedCapStates[ptr],margin,1,zero); mathVecDiffScaleFrom(&projectedHighCapStates[ptr+3],&projectedStates[ptr+3],&projectedCapStates[ptr+3],margin/.25,0,zero); mathVecAdd(&projectedHighCapStates[ptr+6],&projectedCapStates[ptr+6],zero,7); mathVecAdd(hiPos,&projectedHighCapStates[ptr],&projectedStates[ptr],3); //#ifdef debugMode //dv("hiPos",hiPos,0); //#endif c1= projectedStates[ptr+0]>-0.75f && projectedStates[ptr+0]<0.75f && projectedStates[ptr+1]>-0.3f && projectedStates[ptr+1]<1.5f && projectedStates[ptr+2]>-0.75f && projectedStates[ptr+2]<0.75f; //c2= projectedHighCapStates[ptr+0]>-0.75f && projectedHighCapStates[ptr+0]<0.75f // && projectedHighCapStates[ptr+1]>-0.9f && projectedHighCapStates[ptr+1]<.9f // && projectedHighCapStates[ptr+2]>-0.75f && projectedHighCapStates[ptr+2]<0.75f; c2= hiPos[0]>-0.65f && hiPos[0]<0.65f && hiPos[1]>-0.2f && hiPos[1]<1.4f && hiPos[2]>-0.65f && hiPos[2]<0.65f; //DEBUG(("\nc1: %d c2: %d",c1,c2)); if(c1==0 || c2==0) inBoundsCount=0; else inBoundsCount++; // for now just get the last time we've been inbounds long enough if(inBoundsCount>=minInBounds) { float oDot = 0.0f; // possible, now score it. // is it pointing at us? ZRQuat2AttVec(oRef, &projectedHighCapStates[ptr+6], oCheck1); ZRQuat2AttVec(oRef, &tmpState[6], oCheck2); oDot = mathVecInner(oCheck1,oCheck2,3); #ifdef debugMode dv("oRef",oRef,0); dv("oCheck1",oCheck1,0); dv("oCheck2",oCheck2,0); #endif //mathVecAdd(tvec,zero,&projectedHighCapStates[ptr],3); //mathVecNormalize(tvec,3); //if(mathVecInner(tvec,towardsUs,3)>0) if(oDot>0) score+=.25f; // make some distance / time connection score+=i/(mathVecMagnitude(&projectedHighCapStates[ptr],3)*100.0f); if(score>bestScore) { impactTime=i-minInBounds; bestScore=score; orientationDot=oDot; } } ACPredictState(1,&projectedStates[ptr],&projectedStates[ptr+13]); } // Protocolocon override mathVecSubtract(tvec,&projectedStates[0],toVec3(-.25f,1.2f,-.6f),3); #ifdef debugMode dv("Protocolocon Test",tvec,0); #endif if(mathVecMagnitude(tvec,3)<.1) { impactTime=40; } if(impactTime>0) { float tmpV1[3]; float tmpV2[3]; int j; float qDiff[4]; float qTarg[4]; float axis[3]; float angle; float qt[4]; mathVecAdd(&impactObjState[6],&projectedStates[impactTime*13+6],zero,7); mathVecAdd(impactHighCapState,&projectedHighCapStates[impactTime*13],zero,13); #ifdef debugMode //DebugState("ObjState 0",&projectedStates[0]); //DebugState("ObjState 53",&projectedStates[53*13]); //DebugState("ObjStateImp",&projectedStates[impactTime*13]); DEBUG(("\nImpactTime: %d",impactTime)); DEBUG(("\nBest Score: %f",bestScore)); DebugState("impactState",impactObjState); DebugState("impactHighCapState",impactHighCapState); DebugState("rawimpactState",&projectedStates[impactTime*13]); #endif // check for Object Collision if not we head straight on in // Estimate velocity used to move to target mathVecAdd(tmpV1, &projectedStates[impactTime*13], &projectedHighCapStates[impactTime*13],3); mathVecScale(tmpVel,tmpV1,1.0f/impactTime,0); // Set up object to move to us and add our estimated velocity to the object mathVecAdd(tmpState,&projectedStates[impactTime*13],zero,13); mathVecSubtract(&tmpState[3],&tmpState[3],tmpVel,3); #ifdef debugMode DebugState("projState",&projectedStates[impactTime*13]); DebugState("projHighCap",&projectedHighCapStates[impactTime*13]); dv("tmpV1",tmpV1,0); dv("tmpVel",tmpVel,0); DebugState("tmpState",tmpState); #endif // Until rotation is corrected... //impactTime-=6; tanTime=-1; for(j=0;j=0 && impactTime>0) { float tmpState[13]; calcBasicStateInfo(); travelMode=FLY_STRAIGHT; ch1 = mathVecMagnitude(refState,3)<(margin-deflectUnderage); #ifdef debugMode dv("ch1 refState",refState,0); #endif //ch2 = currDistToHighCap<.02f; //DEBUG(("\nch1: %d ch2: %d",ch1,ch2)); if(deflecting>=1 || ch1==1) //|| ch2==1) { deflecting=999; travelMode=FLY_SKATE; } if(currDistToHighCap<.25f && mathVecMagnitude(refState,3)1 || (xa>.9 && mathVecMagnitude(velOf(capState),3)<.01)) { travelMode=FLY_CAPTURE; } else { travelMode=FLY_HIGHCAPTURE; } } else { coneCount=0; } switch(travelMode) { case FLY_STRAIGHT: #ifdef debugMode dt("Straight"); #endif tmpTime=impactTime-time; if(tmpTime<1) tmpTime=1; #ifdef debugMode DEBUG(("\norientationDot %f",orientationDot)); #endif if(highSpin!=0) // || orientationDot<-.7) options=FireVel+FireQuat; else options=FireVel; if(time<10 || tmpTime<10 || (time%10)<3 ) FlyToStateInSeconds(impactHighCapState,tmpTime,options,FireVel); //+FireQuat); break; case FLY_SKATE: #ifdef debugMode dt("Deflecting"); #endif //if((time%4)==0) FlyOrbitToHighCap(); break; case FLY_CAPTURE: #ifdef debugMode dt("Docking"); #endif JakesDock(0); break; case FLY_HIGHCAPTURE: #ifdef debugMode dt("High Docking"); #endif JakesDock(1); break; } } else { // Too Dangerous save your fuel and grab two others instead. } time++; //END::PROC::ZRUser } void ZRInit01() { //BEGIN::PROC::ZRInit memset(trgPos,0,sizeof(float)*3); memset(trgVel,0,sizeof(float)*3); memset(sumPos,0,sizeof(float)*3); sumPosCnt = 0; memset(trgAtt,0,sizeof(float)*4); memset(trgVFin,0,sizeof(float)*3); memset(refState,0,sizeof(float)*13); memset(objState,0,sizeof(float)*13); memset(trgRot,0,sizeof(float)*3); memset(AvObjectState,0,sizeof(float)*13); memset(table,0,sizeof(float)*2940); impactTime = 0; unObTime = 0; deflecting = 0; deflected = 0; travelMode = 0; memset(capState,0,sizeof(float)*13); memset(predState,0,sizeof(float)*13); memset(highCapState,0,sizeof(float)*13); memset(highPredCapState,0,sizeof(float)*13); memset(predCapState,0,sizeof(float)*13); margin = 0.0f; memset(currHighCapFromUs,0,sizeof(float)*3); inCone = 0; inCapture = 0; currDistToHighCap = 0.0f; currDistToCap = 0.0f; memset(currHighCapFromObj,0,sizeof(float)*3); memset(savState,0,sizeof(float)*13); routeBlocked = 0; tanTime = 0; memset(tanObjState,0,sizeof(float)*13); memset(tanCapState,0,sizeof(float)*13); memset(tanHighCapState,0,sizeof(float)*13); memset(impactObjState,0,sizeof(float)*13); memset(impactCapState,0,sizeof(float)*13); memset(impactHighCapState,0,sizeof(float)*13); docking = 0; memset(projectedStates,0,sizeof(float)*2743); memset(projectedCapStates,0,sizeof(float)*2743); memset(projectedHighCapStates,0,sizeof(float)*2743); memset(zero,0,sizeof(float)*13); time = -3; coneCount = 0; memset(baseState,0,sizeof(float)*13); baseCnt = 0; memset(incRot,0,sizeof(float)*4); omegaMag = 0.0f; highSpin = 0; orientationDot = 0.0f; //END::PROC::ZRInit { //BEGIN::PROC::ZRInit // SAMPLE_OBJECT code block taken from the ZRASCC Sample Code // PROTOCOLOCON_TARGET code block taken from published Protocolocon Week 1 code. // // These defines select the target package // (uncomment exactly one) //#define SAMPLE_OBJECT 0 #define STRAIGHT_ON_STATIONARY 1 //#define UPPER_RIGHT_CORNER 2 //#define JOHNS_WABBLE 3 //#define FAR_SIDE_DRIFTING_IN 5 //#define ROCKET 6 //#define UPPER_LEFT 7 //#define CATCHER_WAY_BACK 8 //#define SAMPLE_OBJECT_WITH_PEP 9 //#define PROTOCOLOCON_TARGET 10 //#define PROTOCOLOCON_TARGET 11 float attitude[3]; float quaternian[4]; float reference[3]; float baseQuat[4]; ObjectParams p; reference[0]=1.0f; reference[1]=0.0f; reference[2]=0.0f; baseQuat[0]=0.0f; baseQuat[1]=0.0f; baseQuat[2]=1.0f; baseQuat[3]=0.0f; // sample object #ifdef SAMPLE_OBJECT p.inertia[0] = 0.02f; p.inertia[1] = 0.02f; p.inertia[2] = 0.02f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; //Initial orientation p.q0[0] = 0.7071f; p.q0[1] = 0.7071f; p.q0[2] = 0.0f; p.q0[3] = 0.0f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.0f; p.omega0[1] = 0.00f; //Set up a small Z rotation rate p.omega0[2] = 0.03f; #endif #ifdef SAMPLE_OBJECT_WITH_PEP p.inertia[0] = 0.02f; p.inertia[1] = 0.02f; p.inertia[2] = 0.02f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; //Initial orientation p.q0[0] = 0.7071f; p.q0[1] = 0.7071f; p.q0[2] = 0.0f; p.q0[3] = 0.0f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.0f; p.omega0[1] = 0.00f; //Set up a small Z rotation rate p.omega0[2] = 0.033f; #endif #ifdef STRAIGHT_ON_STATIONARY //Principal moments of inertia p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; p.q0[0]= 0.0f; p.q0[1]= 0.0f; p.q0[2]= 0.707107f; p.q0[3]= 0.707107f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.0f; p.omega0[1] = 0.0f; p.omega0[2] = 0.0f; #endif #ifdef UPPER_RIGHT_CORNER //Principal moments of inertia p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial velocity p.v0[0] = 0.004238f; p.v0[1] = -0.000178f; p.v0[2] = 0.001586f; //Initial position p.xzpos[0] = -0.460393f; p.xzpos[1] = -0.315412f; //Initial orientation p.q0[0] = 0.0f; p.q0[1] = 0.434845f; p.q0[2] = 0.869720f; p.q0[3] = -0.233445f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.020966f; p.omega0[1] = 0.000000f; p.omega0[2] = 0.007965f; #endif #ifdef JOHNS_WABBLE //Principal moments of inertia p.inertia[0] = 0.25f; p.inertia[1] = 0.5f; p.inertia[2] = 0.35f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = -0.08f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; attitude[0]=0; attitude[1]=1; attitude[2]=0; p.q0[0]=quaternian[0]; p.q0[1]=quaternian[1]; p.q0[2]=quaternian[2]; p.q0[3]=quaternian[3]; ZRAttVec2Quat(reference, attitude, baseQuat, quaternian); //Initial orientation //p.q0[0] = -0.7071f; //p.q0[1] = -0.7071f; //p.q0[2] = -0.0f; //p.q0[3] = 0.0f; //Initial orientation p.q0[0] = 0.09f; p.q0[1] = 0.1f; p.q0[2] = -0.6171f; p.q0[3] = 0.7471f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.16f; p.omega0[1] = 0.0f; //Set up a small Z rotation rate p.omega0[2] = 0.0f; #endif #ifdef case4_Idunno //Principal moments of inertia //So this one seems to be pretty challenging p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = -0.33f; p.xzpos[1] = -0.40f; //Initial orientation p.q0[0] = 0.7071f; p.q0[1] = 0.7071f; p.q0[2] = 0.0f; p.q0[3] = 0.0f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.0f; p.omega0[1] = 0.00f; //Set up a small Z rotation rate p.omega0[2] = 0.03f; #endif #ifdef FAR_SIDE_DRIFTING_IN //Principal moments of inertia p.inertia[0] = 0.25f; p.inertia[1] = 0.5f; p.inertia[2] = 0.35f; //Initial velocity p.v0[0] = 0.0f; p.v0[1] = -0.0025f; p.v0[2] = 0.0f; //Initial position p.xzpos[0] = 0.0f; p.xzpos[1] = 0.0f; attitude[0]=0; attitude[1]=1; attitude[2]=0; p.q0[0]=quaternian[0]; p.q0[1]=quaternian[1]; p.q0[2]=quaternian[2]; p.q0[3]=quaternian[3]; ZRAttVec2Quat(reference, attitude, baseQuat, quaternian); /* //Initial orientation p.q0[0] = -0.7071f; p.q0[1] = -0.7071f; p.q0[2] = -0.0f; p.q0[3] = 0.0f;*/ //Initial orientation p.q0[0] = 0.09f; p.q0[1] = 0.1f; p.q0[2] = -0.6171f; p.q0[3] = 0.7471f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.16f; p.omega0[1] = 0.0f; //Set up a small Z rotation rate p.omega0[2] = 0.0f; #endif #ifdef ROCKET //Maybe a bit too fast... workiing on t3 //Principal moments of inertia p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial velocity p.v0[0] = 0.01f; p.v0[1] = -0.01f; p.v0[2] = 0.01f; //Initial position p.xzpos[0] = -0.33f; p.xzpos[1] = -0.40f; //Initial orientation //p.q0[0] = 0.09f; //p.q0[1] = 0.7071f; //p.q0[2] = 0.0f; //p.q0[3] = 0.0f; p.q0[0] = 0.0f; p.q0[1] = 0.0f; p.q0[2] = -0.7071f; p.q0[3] = 0.7071f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.06f; p.omega0[1] = 0.06f; //Set up a small Z rotation rate p.omega0[2] = 0.09f; #endif #ifdef UPPER_LEFT //nice and steady but tricky too //Principal moments of inertia p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial velocity p.v0[0] = 0.005f; p.v0[1] = -0.005f; p.v0[2] = 0.005f; //Initial position p.xzpos[0] = -0.33f; p.xzpos[1] = -0.40f; //Initial orientation //p.q0[0] = 0.09f; ///p.q0[1] = 0.7071f; //p.q0[2] = 0.0f; //p.q0[3] = 0.0f; p.q0[0] = -0.8f; p.q0[1] = -0.3f; p.q0[2] = -0.7071f; p.q0[3] = -0.7071f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.015f; p.omega0[1] = 0.015f; //Set up a small Z rotation rate p.omega0[2] = 0.03f; #endif #ifdef CATCHER_WAY_BACK //Maybe a bit too fast... workiing on t3 //Principal moments of inertia p.inertia[0] = 0.1f; p.inertia[1] = 0.1f; p.inertia[2] = 0.1f; //Initial position p.xzpos[0] = +0.3f; p.xzpos[1] = -0.00f; //Initial velocity p.v0[0] = -0.003f; p.v0[1] = -0.0015f; p.v0[2] = -0.001f; //Initial orientation //p.q0[0] = 0.09f; //p.q0[1] = 0.7071f; //p.q0[2] = 0.0f; //p.q0[3] = 0.0f; p.q0[0] = 0.741334f; p.q0[1] = -0.665010f; p.q0[2] = -0.036010f; p.q0[3] = 0.082994f; mathVecNormalize(p.q0,4); //Initial rotation rates p.omega0[0] = 0.000f; p.omega0[1] = -0.002f; p.omega0[2] = -0.0025f; #endif #ifdef PROTOCOLOCON_TARGET // Principal moments of inertia p.inertia[0] = 0.02f; p.inertia[1] = 0.02f; p.inertia[2] = 0.02f; // Initial velocity p.v0[0] = 0.0f; p.v0[1] = 0.0f; p.v0[2] = 0.0f; // Initial position p.xzpos[0] = -0.25f; p.xzpos[1] = -0.6f; // Initial orientation p.q0[0] = -0.2f; p.q0[1] = -0.05f; p.q0[2] = sin(-40*PI2/180.0); p.q0[3] = cos(-40*PI2/180.0); mathVecNormalize(p.q0, 4); // Initial rotation rates p.omega0[0] = 0.002f; p.omega0[1] = 0.00f; p.omega0[2] = -0.005f; #endif ACSetObjectParams(&p); ACShowNoisyState(0); //END::PROC::ZRInit } } //User-defined procedures static void mathVecScale (float *vout, float *vin, float scaler, int norm) { //BEGIN::PROC::mathVecScale float vtmp[3]; mathVecAdd(vtmp,vin,zero,3); if(norm==1) mathVecNormalize(vtmp,3); vtmp[0]*=scaler; vtmp[1]*=scaler; vtmp[2]*=scaler; mathVecAdd(vout,vtmp,zero,3); //END::PROC::mathVecScale } static void mathVecDiffScaleFrom (float *vOut, float *vHere, float *vThere, float scaler, int norm, float *vFrom) { //BEGIN::PROC::mathVecDiffScaleFrom float vtmp[3]; mathVecSubtract(vtmp, vThere, vHere,3); mathVecScale(vtmp, vtmp, scaler,norm); mathVecAdd(vOut, vtmp, vFrom, 3); //END::PROC::mathVecDiffScaleFrom } static void mathAxisAngle2Quat (float quat[4], float axis[3], float angle) { //BEGIN::PROC::mathAxisAngle2Quat mathVecScale(quat,axis,sinf(angle/2.0f),1); quat[3] = cosf(angle/2.0f); //END::PROC::mathAxisAngle2Quat } static void mathQuatDiff (float *qOut, float *qThere, float *qHere) { //BEGIN::PROC::mathQuatDiff float qTmp[4]; mathVecScale(qTmp,qHere,-1.0f,0); qTmp[3]=qHere[3]; quatMult(qOut,qThere,qTmp); //END::PROC::mathQuatDiff } static void ChangeToObjFrame (float state[13]) { //BEGIN::PROC::ChangeToObjFrame mathVecSubtract(state,state,posOf(savState),6); //END::PROC::ChangeToObjFrame } static void CalcVel2TargOverTime (float *vel, float *refState, float *trgState, float seconds) { //BEGIN::PROC::CalcVel2TargOverTime float tmpPos[3]; float tmpDist; mathVecSubtract(tmpPos,posOf(trgState),posOf(refState),3); tmpDist = mathVecMagnitude(tmpPos,3); mathVecScale(vel,tmpPos,tmpDist/seconds,1); //mathVecSubtract(velOf(tmpState),velOf(tmpState),tmpVel,3); //END::PROC::CalcVel2TargOverTime } static void DebugState (char *txt, float *state) { //BEGIN::PROC::DebugState DEBUG(("\n%s",txt)); dv("Position",state, 0); dv("Velocity",state, 3); dq("Attitude",state, 6); dv("Rotation",state,10); //END::PROC::DebugState } static void mathQuat2AxisAngle (float *quat, float *axis, float *angle) { //BEGIN::PROC::mathQuat2AxisAngle float vMag; float neutral[3]={1,0,0}; vMag=mathVecMagnitude(quat,3); if(vMag==0) { mathVecAdd(axis,neutral,zero,3); *angle=PI4-(PI4*quat[3]); } else { mathVecScale(axis,quat,1.0f/vMag,0); *angle = acosf(quat[3])*2.0f; } //END::PROC::mathQuat2AxisAngle } static void calcBasicStateInfo () { //BEGIN::PROC::calcBasicStateInfo //float absPos[3]; float wrkv1[3]; // // Get Basic current state info // ZRGetMySphState(refState); ACGetObjectState(objState); ACGetCaptureState(objState, capState); /* #ifdef debugMode DebugState("extra raw us(refState):",refState); DebugState("extra raw objState:",objState); DebugState("extra raw capState:",capState); #endif */ // Flip states to Object Perspective mathVecAdd(savState, objState, zero, 13); mathVecAdd(refState,zero,zero,6); ChangeToObjFrame(refState); ChangeToObjFrame(objState); ChangeToObjFrame(capState); inCone = ACInCone(objState,refState); inCapture = ACInCapture(objState,refState); // // current distance to Capture Zone // mathVecSubtract(wrkv1,capState,refState,3); currDistToCap=mathVecMagnitude(wrkv1,3); // // current distance to High Capture Zone mathVecDiffScaleFrom(currHighCapFromObj, objState, capState, margin, 1, objState); // current vector from Us to High Capture Zone mathVecSubtract(currHighCapFromUs,currHighCapFromObj,refState,3); currDistToHighCap=mathVecMagnitude(currHighCapFromUs,3); #ifdef debugMode DebugState("us(refState):",refState); DebugState("objState:",objState); DebugState("capState:",capState); dv("currHighCapFromUs",currHighCapFromUs,0); //dv("absPos",absPos,0); DEBUG(("\nIn Cone: %d In Capture: %d",inCone,inCapture)); #endif #ifndef debugMode //if(time<10 && 1==2) //{ // DEBUG(("\nkey: %d",time)); // DebugState("Spid nodes:5 (7.11):",objState); // DebugState("Epicycle: 9",capState); //} #endif //END::PROC::calcBasicStateInfo } static void FireThrusters (float *control, float *target, int options) { //BEGIN::PROC::FireThrusters float c[13]; float t[13]; float attV[3]; mathVecAdd(c,zero,zero,13); mathVecAdd(t,zero,zero,13); if((options & FirePos)!=0) { mathVecAdd(&c[0],&control[0],zero,3); mathVecAdd(&t[0],&target[0],zero,3); } if((options & FireVel)!=0) { mathVecAdd(&c[3],&control[3],zero,3); mathVecAdd(&t[3],&target[3],zero,3); } if((options & FireAtt)!=0) { float attR[3]={-1,0,0}; mathVecAdd(&c[6],&control[6],zero,7); mathVecAdd(&t[6],&target[6],zero,7); ZRQuat2AttVec(attR,&t[6],attV); #ifdef debugMode dv("attR",attR,0); dq("quat",t,6); dv("attV",attV,0); #endif } if((options & FireQuat)!=0) { mathVecAdd(&c[6],&control[6],zero,7); mathVecAdd(&t[6],&target[6],zero,7); #ifdef debugMode dt("set quats"); #endif } if((options & FireRot)!=0) { mathVecAdd(&c[10],&control[10],zero,3); mathVecAdd(&t[10],&target[10],zero,3); } #ifdef debugMode DebugState("Control Measurements:",c); DebugState("Target Requests:",t); #endif ZRSetCtrlMeasurement(c); if((options & FirePos)!=0) { #ifdef debugMode dt("ZRSetPositionTarget"); #endif ZRSetPositionTarget(&t[0]); } if((options & FireVel)!=0) { #ifdef debugMode dt("ZRSetVelocityTarget"); #endif ZRSetVelocityTarget(&t[3]); } if((options & FireAtt)!=0) { #ifdef debugMode dv("ZRSetAttitudeTarget",attV,0); #endif ZRSetAttitudeTarget(attV); } if((options & FireQuat)!=0) { #ifdef debugMode dt("ZRSetQuatTarget"); #endif ZRSetQuatTarget(&t[6]); } if((options & FireRot)!=0) { #ifdef debugMode dt("ZRSetAttRateTarget"); #endif ZRSetAttRateTarget(&t[10]); } //END::PROC::FireThrusters } static void FlyToStateInSeconds (float *targState, int seconds, int thrusterOptions, int calcOptions) { //BEGIN::PROC::FlyToStateInSeconds state_vector cntlState; float distToTarget; float tmpPos[3]; float qDiff[4]; float qTarg[4]; float axis[3]; float angle; float diffState[13]; if (seconds<1) seconds=1; mathVecAdd(posOf(cntlState),posOf(refState),zero,13); if((calcOptions & FireVel)!=0 && seconds>0) { CalcVel2TargOverTime(velOf(targState),refState,targState,seconds); #ifdef debugMode DEBUG(("\nTime To Target: %d",seconds)); dv("Fly to state in time - target velocity\n",targState,3); #endif } if(seconds>3) { #ifdef debugMode dv("FlyToStateInSeconds - cntlPosBefore Position Extension\n",cntlState,0); #endif mathVecScale(tmpPos,velOf(refState),seconds,0); mathVecAdd(posOf(cntlState),posOf(cntlState),tmpPos,3); #ifdef debugMode dv("FlyToStateInSeconds - cntlPosAfter Position Extension\n",cntlState,0); #endif } else { mathVecAdd(posOf(cntlState),posOf(refState),zero,3); } FireThrusters(cntlState,targState,thrusterOptions); //END::PROC::FlyToStateInSeconds } static void JakesDock (int high) { //BEGIN::PROC::JakesDock // Taken from ZRASCC sample code state_vector objState; state_vector refState; state_vector capState; state_vector targState; state_vector hcState; float attV[3]; float attR[3]={-1,0,0}; float dbg[7]; //=== Create State "Measurement" for Controller ===// //Get SPHERES state to set the attitude measurement //(position will be overwritten below) ZRGetMySphState(refState); //Relative state of object in tender frame (with global attitude) ACGetObjectState(objState); //Find state of the capture zone given this information ACGetCaptureState(objState, capState); if(high==1) { // try it with High capture State. mathVecDiffScaleFrom(hcState,objState,capState,margin,1,objState); mathVecScale(&hcState[3],&capState[3],margin/.25,0); mathVecAdd(&hcState[6],&capState[6],zero,7); mathVecAdd(capState,hcState,zero,13); } //Negate the position and velocity to center the frame on the capture //zone. memset(refState,0,sizeof(float)*6); mathVecSubtract(refState, refState, capState, 6); //'refState' now holds the position of the tender with respect to //the capture zone as well as the global attitude of the tender. This //value is used as the measurement for the controller. ZRSetCtrlMeasurement(refState); //=== Create Target State for Controller ===// //Position and velocity are just controlled to 0 since the state //is referenced with respect to the capture zone memset(targState,0,sizeof(float)*6); //Attitude and rotation rates are set to the same values as the capture zone //7 values are copied here (4 quats, 3 rates) memcpy(&targState[QUAT_1],&capState[QUAT_1],sizeof(float)*7); //Assign targets ZRSetPositionTarget(&targState[POS_X]); ZRSetVelocityTarget(&targState[VEL_X]); if(mathVecMagnitude(&objState[10],3)<.02) { ZRQuat2AttVec(attR,&targState[QUAT_1],attV); ZRSetAttitudeTarget(attV); } else { ZRSetQuatTarget(&targState[QUAT_1]); //DEBUG(("quat")); } //ZRSetAttRateTarget(&targState[10]); //END::PROC::JakesDock } static void FlyOrbitToHighCap () { //BEGIN::PROC::FlyOrbitToHighCap float vx[3]={1,0,0}; float v1[3]; float v2[3]; float v3[3]; float v4[3]; float v5[3]; float v6[3]; float v7[3]; float v8[3]; float f; float ov; float cntlState[13]; float targState[13]; float quat[4]; float nc; float axis[3]; float angle; float angleInc=3; #ifdef debugMode dt("deflect"); #endif mathVecAdd(cntlState,refState,zero,13); mathVecAdd(targState,zero,zero,13); mathVecScale(v8,posOf(refState),1,1); // This whole "nan" check should not be needed, but we were getting // errors (and nan's) and this fixed it and so it stays until we have // spare time. //dv("nancheck",v1,0); nc=mathVecInner(posOf(refState),posOf(capState),3); #ifdef debugMode DEBUG(("\nnc: %f",nc)); #endif if(nc<-.9 || nc>.9) { //dt("nan detected"); mathVecAdd(v1,vx,zero,3); } else { //dt("v1 okay?"); mathVecCross(v1,posOf(refState),posOf(capState)); mathVecNormalize(v1,3); } mathAxisAngle2Quat(quat,v1,angleInc*PI/180.0); // Test reverse function //mathQuat2AxisAngle(quat,axis,&angle); //#ifdef debugMode //dv("axis",axis,0); //DEBUG(("\nangle: %f",angle*180/PI)); //#endif mathVecScale(v2,posOf(capState),1,1); ZRQuat2AttVec(v8,quat,v3); mathVecScale(v4,v3,margin,1); mathVecScale(v7,posOf(refState),margin,1); mathVecSubtract(v5,v4,v7,3); mathVecSubtract(v6,v5,velOf(refState),3); mathVecScale(velOf(targState),v6,1,0); //if(currDistToHighCap<.15) //{ // mathVecScale(velOf(targState),v6,currDistToHighCap/2,0); //} #ifdef debugMode dv("raw ref(refState)",refState,0); dq("quat",quat,0); dv("crossProduct(v1)",v1,0); dv("normalized Ref(v8)",v8,0); dv("normalized Cap(v2)",v2,0); dv("rotated Ref(v3)",v3,0); dv("Scaled Rotated Ref(v4)",v4,0); dv("Scaled Ref(v7)",v7,0); dv("There-Here(v5)",v5,0); dv("Shifted for Object velocity(v6)",v6,0); //dv("tangent(v7)",v7,0); DEBUG(("\nDist to High Cap: %f ",currDistToHighCap)); dv("Final velocity(velOf(targState))",targState,3); #endif FireThrusters(cntlState, targState, FireVel); //END::PROC::FlyOrbitToHighCap }