File content as of revision 2:64f85c9eb556:

#include "mbed.h"
#include "MPU6050.h"
#include "BLEDevice.h"
#include "wallbotble.h"
#include "RCBController.h"
#include "Adafruit_LEDBackpack.h"
#include "Adafruit_GFX.h"
#include "pictLIB.h"


#define DBG 0

enum _mode {
    Nomal = 0,      // ????[?h
    LineFollow,     // ???C???g???[?X???[?h
    Challenge,      // ?`???????W???[?h        
};

Serial pc(USBTX, USBRX);
BLEDevice  ble;
Ticker ticker;
MPU6050 mpu;
wallbotble wb;

myI2C i2c(P0_22,P0_21);

Adafruit_8x8matrix matrix1 = Adafruit_8x8matrix(&i2c);
Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c);

PICTLIB pl(16,16);

int mode = Nomal;

char bValue = 0;

#if 1
char data[16][16] =
{
{0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
{0,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0},
{0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0},
{0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
};
char data2[16][16] =
{
{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
{0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0},
{0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0},
{0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0},
{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
{1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
{0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0},
{0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0},
{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
{0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
};
#else
char data[16][16] =
{
{0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
{0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0},
{0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0},
{0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0},
{0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0},
{0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0},
{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
{0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
{0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0},
{0,1,0,0,0,0,1,0,0,1,0,0,0,0,1,0},
{0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0},
{0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0},
{0,1,0,0,0,1,1,0,0,1,1,0,0,0,1,0},
{0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0},
{0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0},
};
#endif

char	dsp_data[16][16];
char	dsp_data2[16][16];

/* RCBController Service */
static const uint16_t RCBController_service_uuid = 0xFFF0;
static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
uint8_t RCBControllerPayload[10] = {0,};

GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
//static uint8_t _bValue = 0x00;
static uint8_t _mValue[10] = {0,};
GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
                       GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));

RCBController controller;


void SendMessage(char *msg)
{
    if (!ble.getGapState().connected) {
        return;
    }
    int len = strlen(msg);
    
    if(len < 10)
    {
        strcpy( (char*)_mValue ,msg );
        ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
    }
}

void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params)
{
    wb.set_led2(1);
#if DBG
    pc.printf("Connected\n\r");
#endif
}

void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    wb.stop();

    ble.startAdvertising();
    wb.set_led2(0);
#if DBG
    pc.printf("Disconnected\n\r");
#endif
}

void periodicCallback(void)
{
    if (!ble.getGapState().connected) {
        return;
    }
    if( (bValue == 0)&&(wb.GetLinePosition() != 0) )
    {
        // Game over
        wb.stop();
        bValue = 10;  
    }
    if( bValue > 0 )
    {
        wb.stop();
        SendMessage("GAME OVER");
        bValue--;
        if( bValue == 0 )
        {
            wb.set_led1(0);
            SendMessage(" NOMAL  ");
            mode = Nomal;
            ticker.detach();
        }
    }
}


// GattEvent
void onDataWritten(const GattCharacteristicWriteCBParams *params)
{
    if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow ))
    {
        float right_factor;
        float left_factor;

        memcpy( &controller.data[0], params->data , params->len );
#if DBG
        pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
                                                               controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
#endif        
        if(mode == Challenge)
        {
            float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 

            float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
            float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));

            if( /*controller.status.B ==*/ 1 )
            {
                wb.left_motor(left_factor);
                wb.right_motor(right_factor);
            }
            else if( controller.status.A == 1 )
            {
                wb.left_motor(-right_factor);
                wb.right_motor(-left_factor);
            }
            else
            {
        		wb.forward(1.0);
            }
        }
        else
        {
            if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) )
            {
                left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
                right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);

                wb.left_motor(left_factor);
                wb.right_motor(right_factor);
            }
            else
            {
                float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
    
                right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
                left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
    
                if( controller.status.B == 1 )
                {
                    wb.left_motor(left_factor);
                    wb.right_motor(right_factor);
                }
                else if( controller.status.A == 1 )
                {
                    wb.left_motor(-right_factor);
                    wb.right_motor(-left_factor);
                }
                else if( controller.status.UP == 1 )
                {
                    wb.forward(1.0);
                }
                else if( controller.status.DOWN == 1 )
                {
                    wb.backward(1.0);
                }
                else if( controller.status.RIGHT == 1 )
                {
                    wb.right_turn(1.0);
                }
                else if( controller.status.LEFT == 1 )
                {
                    wb.left_turn(1.0);
                }
                else
                {
                    wb.stop();   
                }
                if((controller.status.UP == 1)&&(controller.status.DOWN == 1))  // START BUTTON
                {
                    SendMessage("Challenge");
                    wb.set_led1(1);
                    mode = Challenge;
                    wb.stop();
                    ticker.attach(periodicCallback, 0.1);
                }
            }
        }
    }
}

void line_follow_mode(void)
{
    switch(wb.GetLinePosition())
    {
        case 1:                 // ????????
            wb.left_motor(1.0);
            wb.right_motor(-1.0);
            break;
        case 3:                 // ????????
            wb.left_motor(1.0);
            wb.right_motor(-0.5);
            break;
        case 2:                 // ????????
            wb.left_motor(1.0);
            wb.right_motor(0.5);
            break;
        case 6:                 // ????????
            wb.left_motor(1.0);
            wb.right_motor(1.0);
            break;
        case 4:                 // ????????
            wb.left_motor(0.5);
            wb.right_motor(1.0);
            break;
        case 12:                // ????????
            wb.left_motor(-0.5);
            wb.right_motor(1.0);
            break;
        case 8:                 // ????????
            wb.left_motor(-1.0);
            wb.right_motor(1.0);
            break;
        default:
            wb.left_motor(1.0);
            wb.right_motor(1.0);
            break;
    }
}

float read_rad()
{
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
    static float old_rad = 1000.0;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //pc.printf("%d;%d;%d;%d;%d;%d\n\r",ax,ay,az,gx,gy,gz);
    float rad;
    if( ax > 0.0 )
    {
        rad = ((float)ay / 16000.0) * 90.0;    
    }
    else
    {
        rad = ((float)ay / 16000.0) * 90.0;
        if( ay > 0 )
        {
            rad = 180 - rad;
        }
        else
        {
            rad = -180 - rad;
        }
    }
    int d;
    if( rad > 0 )
    {
        d = (rad + 0.5) / 10;
    }
    else
    {
        d = (rad - 0.5) / 10;
    }
    d *= 10;
    rad = -d;
    //pc.printf("%f;%d;%d\n\r",rad,ax,ay);

    if( old_rad != 1000.0 )
    {
        if( (old_rad + 20.0) >= rad)
        {
            old_rad = rad;    
        }
        else if( (old_rad - 20.0) <= rad)
        {
            
        }
        else
        {
            rad = old_rad;
        }
    }
    return(rad);
}


void drowDisplay()
{
    for (int y = 0; y < 8; y++) {
      for (int x = 0; x < 16; x++) {
        if( dsp_data[x][y+8] == 1 )
        {
            matrix1.drawPixel(x, y, LED_ON);
        }
        else
        {
            matrix1.drawPixel(x, y, LED_OFF);
        }
        if( dsp_data[x][y] == 1 )
        {
            matrix2.drawPixel(x, y, LED_ON);
        }
        else
        {
            matrix2.drawPixel(x, y, LED_OFF);
        }
      }
    }
    matrix1.writeDisplay();  // write the changes we just made to the display
    matrix2.writeDisplay();  // write the changes we just made to the display

}

/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{
    char   in[ 16 ][ 16 ];  //  
    char   in2[ 16 ][ 16 ];  //  
    float rad_set = 0.0;
    float rad = 0.0;
    int count = 0;

    pc.printf("START\n\r");
    
    mpu.initialize();
#if 1
    matrix1.begin(0x71);
    matrix2.begin(0x70);
#else
    matrix1.begin(0x70);
    matrix2.begin(0x74);
#endif
    matrix1.setBrightness(15);
    matrix2.setBrightness(15);
    matrix1.clear();
    matrix2.clear();

    for ( int i = 0; i < 16; i++ )
        for ( int j = 0; j < 16; j++ )
            in[15-i][j] = data[i][j];

    for ( int i = 0; i < 16; i++ )
        for ( int j = 0; j < 16; j++ )
            dsp_data[i][j] = in[i][j];
            
    for ( int i = 0; i < 16; i++ )
        for ( int j = 0; j < 16; j++ )
            in2[15-i][j] = data2[i][j];

    for ( int i = 0; i < 16; i++ )
        for ( int j = 0; j < 16; j++ )
            dsp_data[i][j] = in2[i][j];
            
	drowDisplay();

    wb.f_sensor_calibrate();

#if DBG
    pc.baud(9600);
    DBG(pc.printf("Start\n\r");)
#endif
    
    ble.init(); 
    ble.onConnection(onConnected);
    ble.onDisconnection(onDisconnected);
    ble.onDataWritten(onDataWritten);
    
    /* setup advertising */
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                    (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                    (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));

    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
    ble.startAdvertising();

    ble.addService(RCBControllerService);
    

    while (true) {
        
        if(wb.GetSw())
        {
            if( mode == LineFollow)
            {
			    wb.stop();
			    
			    mode = Nomal;
			    wb.set_led1(0);
	        }
	        else
	        {
	            mode = LineFollow;
			    wb.set_led1(1);
	        }
	        wait(1.0);
        }
        if(mode == LineFollow)
        {
            line_follow_mode();
        }
        else
        {
            ble.waitForEvent();
        }
        if(mode == Challenge)
        {
			count++;
			if(count > 50)
			{
				count = 0;
		        pl.scroll(&in[0][0],&dsp_data[0][0]);
				drowDisplay();
			    for ( int i = 0; i < 16; i++ )
			        for ( int j = 0; j < 16; j++ )
			            in[i][j] = dsp_data[i][j];
			}
		}
		else
		{
        rad = read_rad();
		
		if( (rad > -45.0)&&(rad < 45.0 ) )
		{
			rad = 0.0;
	    }
		else if( (rad > 45.0)&&(rad < 135.0 ) )
		{
			rad = 90.0;
	    }
		else if( ((rad > 135.0)&&(rad < 180.0 ))||((rad > -180.0)&&(rad < -135.0 )) )
		{
			rad = 180.0;
	    }
		else if( (rad > -135.0)&&(rad < -45.0 ) )
		{
			rad = -90.0;
	    }
		else
		{
			
		}
		
		if( rad_set > rad )
		{
			if( (rad_set == 180)&&(rad <0) )
			{
				rad_set = -175;
			}
			else
			{
				rad_set -= 5.0;
			}
		}
		else if( rad_set < rad )
		{
			if( (rad == 180.0)&&(rad_set <0))
			{
				rad_set -= 5.0;
			}
			else
			{
				rad_set += 5.0;
			}
		}
		else
		{
			;
		}
		if(rad_set == -180.0)
		{
			rad_set = 180.0;
		}
        pl.rotation(rad_set,&in2[0][0],&dsp_data[0][0]);
		drowDisplay();
		}
    }
}