/*-------------------------------------------------------------------------------------------------------------
UTLGetInitialPairs
-------------------------------------------------------------------------------------------------------------*/
int UTLGetInitialPairs(const Minutia pminR[], const int nRCnt, const Minutia pminT[], const int nTCnt, Pair pPairs[])
{
int i = 0;
int j = 0;
RMinutia rminR[MAX_MINUTIAE];
RMinutia rminT[MAX_MINUTIAE];
/*sanity check*/
if(pminR == NULL || nRCnt < 1 || pminT == NULL || nTCnt < 1 || pPairs == NULL)
return UTL_ERR_INVALID_PARAMETER;
/* test matching hypotheses */
for(i=0;i<nRCnt;i++)
{
UTLConvertToRadial(pminR,nRCnt,pminR[i].m_nX,pminR[i].m_nY,pminR[i].m_nTheta,rminR);
for(j=0;j<nTCnt;j++)
{
UTLConvertToRadial(pminT,nTCnt,pminT[j].m_nX,pminT[j].m_nY,pminT[j].m_nTheta,rminT);
DBGPrintf("----------------------------------------------------\n");
DBGPrintf("MatchMinutiaSets: Hypothesis (%d,%d)->(%d,%d)\n",pminR[i].m_nX,pminR[i].m_nY,pminT[j].m_nX,pminT[j].m_nY);
DBGPrintf("----------------------------------------------------\n");
UTLMatchRadial(rminR,nRCnt,rminT,nRCnt,pPairs);
}
}
UTLSortPairs(pPairs,nRCnt*nTCnt);
DBGPrintPairs(pminR,nRCnt,pminT,nRCnt,pPairs);
return UTL_ERR_NO_ERROR;
}
/*-------------------------------------------------------------------------------------------------------------
UTLConvertToRadial
-------------------------------------------------------------------------------------------------------------*/
int UTLMatchRadial(const RMinutia pRMin[], const int nRCnt, const RMinutia pTMin[], const int nTCnt,Pair pPair[])
{
int i = 0;
int j = 0;
double dR = 0;
double dTheta = 0;
double dDelta = 0;
int nAbs = 0;
int nTFlags[MAX_MINUTIAE] ={0};
int nRFlags[MAX_MINUTIAE] ={0};
for(i=0;i<nRCnt;i++)
{
/*skip far away points*/
if(pRMin[i].m_dR > MAX_R)
continue;
for(j=0;j<nTCnt;j++)
{
if(nTFlags[j]) /*already matched*/
continue;
/*skip far away points*/
if(pTMin[j].m_dR > MAX_R)
continue;
/*check for match*/
dR = fabs(log10((pRMin[i].m_dR+1e-5)/(pTMin[j].m_dR+1e-5)));
if(dR > DR) /*no match*/
continue;
nAbs = abs(pRMin[i].m_nTheta - pTMin[j].m_nTheta);
if((360-nAbs) < nAbs)
dTheta = (360-nAbs);
else
dTheta = nAbs;
if(dTheta > DTHETA) /*no match*/
continue;
nAbs = abs(pRMin[i].m_nDelta - pTMin[j].m_nDelta);
if((360-nAbs) < nAbs)
dDelta = (360-nAbs);
else
dDelta = nAbs;
if(dDelta > DDELTA) /*no match*/
continue;
if(!nRFlags[i] && !nTFlags[j]) /*match found*/
{
nRFlags[i] = 1;
nTFlags[j] = 1;
//add vote to pair matrix
pPair[i*nTCnt+j].m_dScore++;
pPair[i*nTCnt+j].m_nRIdx = i;
pPair[i*nTCnt+j].m_nTIdx = j;
DBGPrintf("Radial Match: [%d,%d]->(%lf,%d,%d),(%lf,%d,%d)\n",i,j,pRMin[i].m_dR,pRMin[i].m_nTheta,pRMin[i].m_nDelta,
pTMin[j].m_dR,pTMin[j].m_nTheta,pTMin[j].m_nDelta);
}
}
}
return UTL_ERR_NO_ERROR;
}
//==============================================================================
// This is code the checks for and processes input from the DIY XBee receiver
// work
//==============================================================================
void DIYXBeeController::ControlInput(void)
{
byte iNumButton;
// Then try to receive a packet of information from the XBEE.
// It will return true if it has a valid packet
if (ReceiveXBeePacket(&g_diyp)) {
if (memcmp((void*)&g_diyp, (void*)&g_diypPrev, sizeof(g_diyp)) != 0) {
#ifdef XBEE_NEW_DATA_ONLY
if (g_diystate.fPacketForced)
SendXbeeNewDataOnlyPacket(1);
#endif
#ifdef DEBUG
// setup to output back to our USB port
if (g_fDebugOutput) {
DBGPrintf("%x - %d %d - %d %d - %d %d\n", g_diyp.s.wButtons, g_diyp.s.bRJoyLR, g_diyp.s.bRJoyUD,
g_diyp.s.bLJoyLR, g_diyp.s.bLJoyUD, g_diyp.s.bRSlider, g_diyp.s.bLSlider);
}
#endif
}
// OK lets try "0" button for Start.
if ((g_diyp.s.wButtons & (1<<0)) && ((g_diypPrev.s.wButtons & (1<<0)) == 0)) { //Start Button (0 on keypad) test
if(g_InControlState.fHexOn) {
//Turn off
g_InControlState.BodyPos.x = 0;
g_InControlState.BodyPos.y = 0;
g_InControlState.BodyPos.z = 0;
g_InControlState.BodyRot1.x = 0;
g_InControlState.BodyRot1.y = 0;
g_InControlState.BodyRot1.z = 0;
g_InControlState.TravelLength.x = 0;
g_InControlState.TravelLength.z = 0;
g_InControlState.TravelLength.y = 0;
g_BodyYOffset = 0;
g_BodyYSift = 0;
g_InControlState.SelectedLeg = 255;
g_InControlState.fHexOn = 0;
}
else {
//Turn on
g_InControlState.fHexOn = 1;
}
}
if (g_InControlState.fHexOn) {
if ((g_diyp.s.wButtons & (1<<0xa)) && ((g_diypPrev.s.wButtons & (1<<0xa)) == 0)) { // A button test
MSound(1, 50, 2000);
XBeePlaySounds(1, 50, 2000);
bXBeeControlMode = WALKMODE;
XBeeOutputStringF(F("Walking"));
}
if ((g_diyp.s.wButtons & (1<<0xb)) && ((g_diypPrev.s.wButtons & (1<<0xb)) == 0)) { // B button test
MSound(1, 50, 2000);
XBeePlaySounds(1, 50, 2000);
bXBeeControlMode = TRANSLATEMODE;
XBeeOutputStringF(F("Body Translate"));
}
if ((g_diyp.s.wButtons & (1<<0xc)) && ((g_diypPrev.s.wButtons & (1<<0xc)) == 0)) { // C button test
MSound(1, 50, 2000);
bXBeeControlMode = ROTATEMODE;
XBeeOutputStringF(F("Body Rotate"));
}
if ((g_diyp.s.wButtons & (1<<0xD)) && ((g_diypPrev.s.wButtons & (1<<0xd)) == 0)) { // D button test - Single Leg
MSound(1, 50, 2000);
if (g_InControlState.SelectedLeg==255) // none
g_InControlState.SelectedLeg=cRF;
else if (bXBeeControlMode==SINGLELEGMODE) //Double press to turn all legs down
g_InControlState.SelectedLeg=255; //none
bXBeeControlMode=SINGLELEGMODE;
XBeeOutputStringF (F("Single Leg"));
}
if ((g_diyp.s.wButtons & (1<<0xe)) && ((g_diypPrev.s.wButtons & (1<<0xe)) == 0)) { // E button test - Balance mode
if (!g_InControlState.BalanceMode) {
g_InControlState.BalanceMode = 1;
MSound( 2, 100, 2000, 50, 4000);
XBeePlaySounds(2, 100, 2000, 50, 4000);
XBeeOutputStringF(F("Balance On"));
}
else {
g_InControlState.BalanceMode = 0;
MSound( 1, 250, 1500);
XBeePlaySounds(1, 50, 1500);
XBeeOutputStringF(F("Balance Off"));
}
}
#ifdef OPT_GPPLAYER
if ((g_diyp.s.wButtons & (1<<0xf)) && ((g_diypPrev.s.wButtons & (1<<0xf)) == 0)) { // F button test - GP Player
if (g_ServoDriver.FIsGPEnabled()) { //F Button GP Player Mode Mode on/off -- SSC supports this mode
XBeeOutputStringF(F("Run Sequence"));
MSound(1, 50, 2000);
g_InControlState.BodyPos.x = 0;
g_InControlState.BodyPos.z = 0;
g_InControlState.BodyRot1.x = 0;
g_InControlState.BodyRot1.y = 0;
g_InControlState.BodyRot1.z = 0;
g_InControlState.TravelLength.x = 0;
g_InControlState.TravelLength.z = 0;
g_InControlState.TravelLength.y = 0;
g_InControlState.SelectedLeg=255; //none
g_InControlState.fSLHold=0;
bXBeeControlMode = GPPLAYERMODE;
}
else {
XBeeOutputStringF(F("Seq Disabled"));
MSound(1, 50, 2000);
}
}
#endif
//Hack there are several places that use the 1-N buttons to select a number as an index
// so lets convert our bitmap of which key may be pressed to a number...
// BUGBUG:: There is probably a cleaner way to convert... Will extract buttons 1-9
iNumButton = 0; // assume no button
if ((g_diyp.s.wButtons & 0x3fe) && ((g_diypPrev.s.wButtons & 0x3fe) == 0)) { // buttons 1-9
word w = g_diyp.s.wButtons & 0x3fe;
while ((w & 0x1) == 0) {
w >>= 1;
iNumButton++;
}
}
// BUGBUG:: we are using all keys now, may want to reserve some...
//Switch gait
// We will do slightly different here than the RC version as we have a bit per button
if ((bXBeeControlMode==WALKMODE) && iNumButton && (iNumButton <= NUM_GAITS)) { //1-8 Button Gait select
if ( abs(g_InControlState.TravelLength.x)<cTravelDeadZone && abs(g_InControlState.TravelLength.z)<cTravelDeadZone &&
abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone) {
//Switch Gait type
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.GaitType = iNumButton-1;
#ifdef DEBUG
DBGPrintf("New Gate: %d\n\r", g_InControlState.GaitType);
#endif
GaitSelect();
#ifdef DEBUG
DBGPrintf("Output Gate Named\n\r");
#endif
XBeeOutputStringF((const __FlashStringHelper *)pgm_read_word(&s_asGateNames[g_InControlState.GaitType]));
}
}
//Switch single leg
if (bXBeeControlMode==SINGLELEGMODE) {
if (iNumButton>=1 && iNumButton<=6) {
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.SelectedLeg = iNumButton-1;
g_InControlState.fSLHold=0;
}
if (iNumButton == 9) { //Switch Directcontrol
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.fSLHold ^= 1; //Toggle g_InControlState.fSLHold
}
}
else if (bXBeeControlMode==WALKMODE) {
g_InControlState.SelectedLeg=255; // none
g_InControlState.fSLHold=0;
}
//Body Height - Control depends on how big our packet was (ie which controller we are using)
if (g_diystate.cbPacketSize > PKT_MSLIDER)
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bMSlider / 2), g_InControlState.BodyPos.y, SmDiv);
else if (g_diystate.cbPacketSize > PKT_LPOT)
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bLPot*2/3), g_InControlState.BodyPos.y, SmDiv);//Zenta test
else
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bLJoyUD / 2), g_InControlState.BodyPos.y, SmDiv);
//Leg lift height - Right slider has value 0-255 translate to 30-93
g_InControlState.LegLiftHeight = 30 + g_diyp.s.bRSlider/3;//Zenta trying 3 instead of 4
//---------------------------------------------------------------------------------------------------
//Walk mode
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==WALKMODE) { // Kurt's Arduino version
if (g_diystate.cbPacketSize > PKT_MSLIDER) {
g_InControlState.TravelLength.x = -(g_diyp.s.bLJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bRJoyLR - 128)/3;
//g_InControlState.BodyRot1.x = SmoothControl(((bPacket(PKT_RJOYUD)-128)*2), g_InControlState.BodyRot1.x, 2);
//g_InControlState.BodyRot1.z = SmoothControl((-(bPacket(PKT_RPOT)-128)*2), g_InControlState.BodyRot1.z, 2);
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bLJoyLR-128), abs(g_diyp.s.bLJoyUD-128)), abs(g_diyp.s.bRJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
else if (g_diystate.cbPacketSize > PKT_LPOT) { // Case for original DIY XBee with extra pots
g_InControlState.TravelLength.x = -(g_diyp.s.bLJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bRJoyLR - 128)/3;
g_InControlState.BodyRot1.z = SmoothControl((-(g_diyp.s.bRPot-128)*2), g_InControlState.BodyRot1.z, SmDiv);
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bLJoyLR-128), abs(g_diyp.s.bLJoyUD-128)), abs(g_diyp.s.bRJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
else { // original DIY XBee
g_InControlState.TravelLength.x = -(g_diyp.s.bRJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bRJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bLJoyLR - 128)/3;
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bRJoyLR-128), abs(g_diyp.s.bRJoyUD-128)), abs(g_diyp.s.bLJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
}
//---------------------------------------------------------------------------------------------------
//Body move
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==TRANSLATEMODE) {
if (g_diystate.cbPacketSize > PKT_LPOT) {
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bRJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bRJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl(((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
else {
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bRJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bRJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl(((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
g_InControlState.InputTimeDelay = 128 - abs(g_diyp.s.bLJoyUD-128) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
//Body rotate
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==ROTATEMODE) {
if (iNumButton &&(iNumButton <=3)) {
LjoyUDFunction = iNumButton -1;
MSound( 1, 20, 2000);
XBeeOutputStringF((const __FlashStringHelper *)pgm_read_word(&s_asLJoyUDNames[LjoyUDFunction]));
}
if (iNumButton == 4) { // Toogle Left Joystick left/Right function
LeftJoyLRmode = !LeftJoyLRmode;
if (LeftJoyLRmode) {
XBeeOutputStringF(F("LJOYLR trans"));
MSound( 1, 20, 1000);
}
else {
XBeeOutputStringF(F("LJOYLR rotate"));
MSound( 1, 20, 2000);
}
}
#ifdef DISP_VOLTAGE
if (iNumButton == 8) { // Toogle g_fDisplayLiPo
g_fDisplayLiPo = !g_fDisplayLiPo;
MSound( 1, 20, 1500+500*g_fDisplayLiPo);
}
#endif
if (iNumButton == 9) { // Toogle LockFunction
LockFunction = !LockFunction;
if (LockFunction) {
XBeeOutputStringF(F("Lock ON"));
MSound( 1, 20, 1500);
}
else {
XBeeOutputStringF(F("Lock OFF"));
MSound( 1, 20, 2500);
}
}
// BranchLJoyUDFunction in basic
switch (LjoyUDFunction) {
case 0:
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD-128); // dito need to update
break;
case 1:
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bLJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
break;
default:
if (!LockFunction) {
g_InControlState.BodyRotOffset.z = (g_diyp.s.bLJoyUD - 128);
g_InControlState.BodyRotOffset.y = (g_diyp.s.bRPot - 128);
}
}
if (LeftJoyLRmode) {//;Do X translation:
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bLJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
}
else {
g_InControlState.BodyRot1.y = SmoothControl (((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
g_InControlState.BodyRot1.x = SmoothControl(((g_diyp.s.bRJoyUD-128)*2), g_InControlState.BodyRot1.x, SmDiv);
g_InControlState.BodyRot1.z = SmoothControl((-(g_diyp.s.bRJoyLR-128)*2), g_InControlState.BodyRot1.z , SmDiv);
g_InControlState.InputTimeDelay = 128 - abs(g_diyp.s.bLJoyUD-128) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
//Single Leg Mode
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode == SINGLELEGMODE) {
g_InControlState.SLLeg.x = SmoothControl(((g_diyp.s.bRJoyLR-128)), g_InControlState.SLLeg.x , SmDiv);//;
g_InControlState.SLLeg.z = SmoothControl((-(g_diyp.s.bRJoyUD-128)), g_InControlState.SLLeg.z , SmDiv);//;
g_InControlState.SLLeg.y = SmoothControl((-(g_diyp.s.bLJoyUD-128)), g_InControlState.SLLeg.y , SmDiv);//Need to check packetsize..
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bRJoyLR-128), abs(g_diyp.s.bRJoyUD-128)), abs(g_diyp.s.bLJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
// Sequence General Player Mode
//---------------------------------------------------------------------------------------------------
#ifdef OPT_GPPLAYER
if (bXBeeControlMode == GPPLAYERMODE) {
// Lets try some speed control... Map all values if we have mapped some before
// or start mapping if we exceed some minimum delta from center
// Have to keep reminding myself that commander library already subtracted 128...
if (g_ServoDriver.FIsGPSeqActive() ) {
if ((g_sGPSMController != 32767)
|| (g_diyp.s.bRJoyUD > (128+16)) || (g_diyp.s.bRJoyUD < (128-16)))
{
// We are in speed modify mode...
short sNewGPSM = map(g_diyp.s.bRJoyUD, 0, 255, -200, 200);
if (sNewGPSM != g_sGPSMController) {
g_sGPSMController = sNewGPSM;
g_ServoDriver.GPSetSpeedMultiplyer(g_sGPSMController);
}
}
// See what step we are on, if it changed then output the step to the user
byte bCurStep = g_ServoDriver.GPCurStep();
if (bCurStep != g_bGPCurStepPrev) {
g_bGPCurStepPrev = bCurStep;
// Lets build a quick and dirty string to output
char szTemp[20];
sprintf(szTemp, "cs: %d SM: %d", bCurStep, (g_sGPSMController == 32767) ? 100 : g_sGPSMController);
XBeeOutputString(szTemp);
}
}
if (iNumButton>=1 && iNumButton<=9) { //1-9 Button Play GP Seq
word wGPSeqPtr;
if (!g_ServoDriver.FIsGPSeqActive() ) {
uint8_t GPSeq = iNumButton-1;
if ( g_ServoDriver.FIsGPSeqDefined(GPSeq)) {
XBeeOutputStringF(F("Start Sequence")); // Tell user sequence started.
g_ServoDriver.GPStartSeq(GPSeq);
g_sGPSMController = 32767; // Say we are not in modifiy speed modifier mode yet...
g_bGPCurStepPrev = 0xff;
}
else {
XBeeOutputStringF(F("Seq Not defined")); // that sequence was not defined...
}
}
else {
// Cancel the current one
g_ServoDriver.GPStartSeq(0xff); // tell the GP system to abort if possible...
MSound (2, 50, 2000, 50, 2000);
}
}
}
#endif
//Calculate walking time delay
}
g_diypPrev = g_diyp; // remember the last packet
}