wbcdemo/src/balance.c

/*---------------------------------------------------------------------------*
  Project:    WPAD health demo program
  File:       wbc_simple.c

  Copyright (C) 2007 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: $
 *---------------------------------------------------------------------------*/
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stddef.h>
#include <stdarg.h>
#include <stdio.h>
#include <ctype.h>

#include <revolution.h>
#include <revolution/wpad.h>
#include <revolution/wpadBalance.h>
#include <revolution/sc.h>
#include <revolution/wbc.h>

#define DEMO_USE_MEMLIB=1 // This turns on the DEMO library's MEM heaps.
#include <demo.h>


/*---------------------------------------------------------------------------*
 * Local Definitions
 *---------------------------------------------------------------------------*/
static u8  workarea[WPAD_BLCINT_WORK_LEN] ATTRIBUTE_ALIGN(32);
static WPADBLStatus status;
static double zero[WPAD_PRESS_UNITS];

/*---------------------------------------------------------------------------*
 * Function prototypes
 *---------------------------------------------------------------------------*/
// MEM2 memory allocation routines. The application must provide these to
// WPAD, so it can setup the data transfer buffer. This buffer must reside
// in MEM2.
static void *myAlloc                 ( u32 size );
static u8    myFree                  ( void *ptr );


// callbacks for CONNECT and EXTENSION events
static void connectCallback          ( s32 chan, s32 reason );
static void extensionCallback        ( s32 chan, s32 result );

/*===========================================================================*
 *                   F U N C T I O N    D E F I N I T I O N S
 *===========================================================================*/
static void WaitMilliTime(s32 msec)
{
    OSTime t=OSGetTime();
    while(OSTicksToMilliseconds(OSGetTime()-t) < msec)
        ;
}


static void ZeroSetStart2( s32 chan, s32 result )
{
    #pragma unused(chan, result)

    // Wait about 200[ms] so that Board's press value rarely becomes unstable
    // after updating Balance Wii Board's temperature.
    WaitMilliTime(200);

    // Set Zero point.
    zero[0] = status.press[0]; //Normally, set the average for 2 seconds.
    zero[1] = status.press[1]; //Normally, set the average for 2 seconds.
    zero[2] = status.press[2]; //Normally, set the average for 2 seconds.
    zero[3] = status.press[3]; //Normally, set the average for 2 seconds.
    WBCSetZEROPoint( zero,(u32)(sizeof(zero) / sizeof(zero[0])));
}


static void ZeroSetStart( s32 chan, s32 result )
{
    #pragma unused(chan, result)

    // Check Board's temperature.
    WPADRead(WPAD_CHAN3, &status);


    if((status.temp == 127) || (status.temp == -128))
    {
        // Update Board's temperature again if you can not get correct Board's temperature.
        WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_UPDATE_TEMP, ZeroSetStart2);
    }
    else
    {
        // Wait about 200[ms] so that Board's press value rarely becomes unstable
        // after updating Balance Wii Board's temperature.
        WaitMilliTime(200);

        // Set Zero point.
        zero[0] = status.press[0]; //Normally, set the average for 2 seconds.
        zero[1] = status.press[1]; //Normally, set the average for 2 seconds.
        zero[2] = status.press[2]; //Normally, set the average for 2 seconds.
        zero[3] = status.press[3]; //Normally, set the average for 2 seconds.
        WBCSetZEROPoint( zero,(u32)(sizeof(zero) / sizeof(zero[0])));
    }
}


/*---------------------------------------------------------------------------*
 * Name        : main()
 * Description :
 * Arguments   : None.
 * Returns     : None.
 *---------------------------------------------------------------------------*/
int main( void )
{
    double weight[WPAD_PRESS_UNITS];
    u32 type;

    DEMOInit( NULL );
    DEMOPadInit();

    // If Japan, need to call WPADRegisterBLCWorkarea to set a work buffer.
    // If not Japan, WPADRegisterBLCWorkarea does nothing.
    WPADRegisterBLCWorkarea( workarea );

    WPADRegisterAllocator(myAlloc, myFree);
    WPADInit();
    WPADSetConnectCallback(WPAD_CHAN3, connectCallback);

    while (WPAD_STATE_SETUP != WPADGetStatus())
    {
        ;
    }

    // You can release the work buffer so initialize of WPAD library has finished.
    // This sample program does nothing it uses static buffer.


    if (DEMOInitROMFont() == 0)
    {
        OSHalt("FONT ERROR\n");
        // NOT REACHED HERE
    }

    while(1)
    {
        GXRenderModeObj *rmp = DEMOGetRenderModeObj();
        s16 x,y;
        double w1,w2,w3,w4;
        double left,right,front,back;

        w1 = weight[0]; // front right
        w2 = weight[1]; // back  right
        w3 = weight[2]; // front left
        w4 = weight[3]; // back  left

        if(fabs(w1) < 1) w1 = 1;
        if(fabs(w2) < 1) w2 = 1;
        if(fabs(w3) < 1) w3 = 1;
        if(fabs(w4) < 1) w4 = 1;

        left  = (double)(w3 + w4);
        right = (double)(w1 + w2);
        front = (double)(w1 + w3);
        back  = (double)(w4 + w2);

        // Calculate a balance point.
        x = (s16)((right / (left + right)) * rmp->fbWidth);
        y = (s16)((back  / (front + back)) * rmp->viHeight);

        DEMOBeforeRender();
        DEMOInitCaption(DM_FT_XLU, (s16) rmp->fbWidth, (s16) rmp->efbHeight);
        DEMOSetROMFontSize(16, -1);
        DEMORFPrintf(x, y, 0, "X"); //Draw the center of gravity.
        DEMODoneRender();


        DEMOPadRead();

        if (DEMOPadGetButtonDown(0) & PAD_BUTTON_A)
        {
            // Update Board's temperature the eve of zero point correction.
            WPADControlBLC(WPAD_CHAN3, WPAD_BLCMD_UPDATE_TEMP, ZeroSetStart);
        }

        if (WPADProbe(WPAD_CHAN3, &type) == WPAD_ERR_NO_CONTROLLER)
        {
            if (WPADIsRegisteredBLC())
            {
                OSReport("Balance Wii board is not connected.\n");
            }
            else
            {
                OSReport("Balance Wii board is not registered. Please regist Balance Wii board with SYNC button.\n");
            }
        }
        else
        {
            if (type == WPAD_DEV_BALANCE_CHECKER)
            {
                WPADRead(WPAD_CHAN3, &status);

                if(WBCGetCalibrationStatus() == TRUE)
                {
                    double total;

                    WBCRead(&status, weight, (u32)(sizeof(weight) / sizeof(weight[0])));
                    total = (double)(weight[0]+weight[1]+weight[2]+weight[3]);

                    // Show the total weight. Normally, "total" value is the average for 2 seconds.
                    OSReport("%3.1f[kg]\n", WBCGetTGCWeight(total, &status));
                }
                else
                {
                    OSReport("Calibration data read error. Please reconnect Balance Wii board.\n");
                }
            }
            else
            {
                OSReport("No Balance Wii board is attached.\n");
            }
        }
    }
} // end main()


/*---------------------------------------------------------------------------*
 * Name        : connectCallback()
 *
 * Description : This callback is invoked when a controller is connected or
 *               disconnected.
 *
 * Arguments   : The channel (chan) for which the event has occurred.
 *               The channel status (reason):
 *                 WPAD_ERR_NONE means a controller has been connected.
 *                 WPAD_ERR_NO_CONTROLLER means a controller disconnected.
 *
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static void connectCallback(s32 chan, s32 reason)
{
    u32 type;

    if (reason == WPAD_ERR_NONE)
    {
        // Disconnect 4P if 4P is not Balance Wii board.
        // Because 4P is reserved for Balance Wii board.
        WPADProbe(chan, &type);
        if (chan == WPAD_CHAN0 && type != WPAD_DEV_BALANCE_CHECKER)
        {
            OSReport("Channel%d is reserved for Balance Wii board.\n", chan);
            WPADDisconnect(chan);
        }
        else
        {
            // Read the calibration value for calculating a weight.
            if(WPAD_ERR_NONE != WBCSetupCalibration())
            {
                OSHalt("Balance Wii board FATAL ERROR!!\n");
            }
            OSReport("Channel%d is connected.\n", chan);
            WPADSetExtensionCallback(chan, extensionCallback);
        }
    }
    else
    {
        OSReport("Channel%d is disconnected.\n", chan);
    }
} // end connectCallback()

/*---------------------------------------------------------------------------*
 * Name        : extensionCallback()
 *
 * Description : This callback is invoked when an Extension has been attached.
 *
 * Arguments   : The channel (chan) for which the extension event occurred.
 *               The device type (result):
 *
 *                 WPAD_DEV_UNKNOWN means that something has been attached, but
 *                 it's being initialized, and we won't know what it is until
 *                 initialization is complete.
 *
 *                 WPAD_DEV_CORE means that an extension has been removed and
 *                 we're back to just the core device.
 *
 *                 WPAD_DEV_FREESTYLE means that the "NUNCHAK" extension has
 *                 been attached and initialized.
 *
 *                 WPAD_DEV_CLASSIC means that the "CLASSIC" extension has been
 *                 attached and initialized.
 *
 * Returns     : None.
 *---------------------------------------------------------------------------*/
static void extensionCallback(s32 chan, s32 result)
{
    switch(result)
    {
        case WPAD_DEV_UNKNOWN:
            OSReport("Initializing extension on channel%d...\n", chan);
            break;

        case WPAD_DEV_CORE:
            WPADControlDpd(chan, WPAD_DPD_EXP, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_CORE_ACC_DPD);

            OSReport("Extension removed on channel%d.\n", chan);
            break;

        case WPAD_DEV_NOT_SUPPORTED:
        case WPAD_DEV_FUTURE:
            WPADControlDpd(chan, WPAD_DPD_EXP, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_CORE_ACC_DPD);

            OSReport("Extension is not useful on channel%d.\n", chan);
            break;

        case WPAD_DEV_FREESTYLE:
            WPADControlDpd(chan, WPAD_DPD_STD, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_FREESTYLE_ACC_DPD);

            OSReport("Freestyle initialized on channel%d.\n", chan);
            break;

        case WPAD_DEV_CLASSIC:
            WPADControlDpd(chan, WPAD_DPD_STD, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_CLASSIC_ACC_DPD);

            OSReport("Classicstyle initialized on channel%d.\n", chan);
            break;

        case WPAD_DEV_BALANCE_CHECKER:
            WPADControlDpd(chan, WPAD_DPD_OFF, NULL);
            WPADSetDataFormat(chan, WPAD_FMT_BALANCE_CHECKER);
            WPADControlBLC(chan, WPAD_BLCMD_ON, NULL);

            OSReport("Balance Wii board initialized on channel%d.\n", chan);
            break;

        default:
            // Here is WPAD_DEV_NOT_FOUND.
            // If the controller is disconnected while the extension is initializing
            // it reaches here. There is nothing to do.
            break;

    } // end
} // end extensionCallback()

/*---------------------------------------------------------------------------*
 * Name        : myAlloc()
 * Description : Callback needed by WPAD to allocate mem from MEM2 heap
 * Arguments   : size of block, in bytes.
 * Returns     : pointer to allocated block.
 *---------------------------------------------------------------------------*/
static void *myAlloc(u32 size)
{
    void *ptr;

    ptr = MEMAllocFromAllocator(&DemoAllocator2, size);
    ASSERTMSG(ptr, "Memory allocation failed\n");

    return(ptr);

} // myAlloc()

/*---------------------------------------------------------------------------*
 * Name        : myFree()
 * Description : Callback needed by WPAD to free mem from MEM2 heap
 * Arguments   : None.
 * Returns     : Always 1.
 *---------------------------------------------------------------------------*/
static u8 myFree(void *ptr)
{

    MEMFreeToAllocator(&DemoAllocator2, ptr);

    // we should ensure that memory is free'd properly, but oh well
    return(1);

} // myFree()