Diff: main.cpp

Revision:

0:fccb789424fc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 10 16:01:50 2013 +0000
@@ -0,0 +1,382 @@
+
+#include "mbed.h"
+#include <btstack/hci_cmds.h>
+#include <btstack/run_loop.h>
+#include <btstack/sdp_util.h>
+#include "hci.h"
+#include "l2cap.h"
+#include "btstack_memory.h"
+#include "remote_device_db.h"
+#include "rfcomm.h"
+extern "C" {
+#include "sdp.h"
+}
+#include "config.h"
+#include "debug.h"
+#include "bd_addr.h"  // class bd_addr
+
+#include "Wiimote.h"
+#include "TB6612.h"
+
+// ----- Wallbot I/O Setting ----- 
+// Motor
+TB6612 right(p21,p12,p11);
+TB6612 left(p22,p14,p13);
+
+#define WII_REMOTE_CONNECTION_SYNC    0        // 0:old Wii Remote 1:New Wii Remote
+#define REPORTMODE_TT_BIT           0x00
+#define WII_REMOTE_REPORT_MODE       0x31
+#define MAX_SPEED 100
+
+#define HEARTBEAT_PERIOD_MS     1000
+#define INQUIRY_INTERVAL          15
+
+Serial pc(USBTX, USBRX);
+DigitalOut ActLED(LED1);
+
+
+
+static bd_addr_t keyboard;
+static int haveKeyboard = 0;
+static char                     lineBuffer[80];
+static short wii_remocon_channel_id=0;
+
+int DirectMode( Wiimote* wii, int stat )
+{
+    
+    if( wii->left )
+    {
+        right = -MAX_SPEED;
+        left = MAX_SPEED;
+    }
+    else if( wii->right )
+    {
+        right = MAX_SPEED;
+        left = -MAX_SPEED;
+    }    
+    else if( wii->up )
+    {
+        right = MAX_SPEED;
+        left = MAX_SPEED;
+    }
+    else if( wii->down )
+    {
+        right = -MAX_SPEED;
+        left = -MAX_SPEED;
+    }
+    else
+    {
+        right = 0;
+        left = 0;
+    }
+    
+    float factor = wii->wheel * 1.5f;
+    
+    if(factor > 100.0f )    factor = 100.0f; 
+    if(factor < -100.0f )    factor = -100.0f;
+    
+    printf("%f\t%f\r\n",wii->wheel,factor);
+    
+    int left_factor = (int)((factor <= 0.0) ? 100.0 : 100.0 - factor);
+    int right_factor = (int)((factor >= 0.0) ? 100.0 : 100.0 - (-factor));
+    
+    if( wii->one )
+    {
+        right = right_factor;
+        left = left_factor;
+    }
+    if( wii->two )
+    {
+        right = -left_factor;
+        left = -right_factor;
+    }
+    
+    return(stat);
+}
+
+int wall_bot_remote(char *c)
+{
+    Wiimote wii;
+    int ret = 0;
+    
+    wii.decode(c);
+    
+    ret = DirectMode( &wii ,ret );
+    
+    return(ret);
+}
+
+void wii_remote_report_mode(uint16_t channel,uint8_t tt,uint8_t mm)
+{
+    lineBuffer[0]=0xa2;
+    lineBuffer[1]=0x12;
+    lineBuffer[2]=tt;
+    lineBuffer[3]=mm;
+    l2cap_send_internal(channel,(uint8_t*)lineBuffer,4);
+}
+
+void wii_remote_request_status_report(uint16_t channel)
+{
+    lineBuffer[0]=0xa2;
+    lineBuffer[1]=0x15;
+    lineBuffer[2]=0x00;
+    l2cap_send_internal(channel,(uint8_t*)lineBuffer,3);
+}
+
+void hid_process_packet(uint16_t channel,unsigned char *hid_report,uint16_t size)
+{
+    if(hid_report[0]!=0xa1){
+        return;
+    }
+
+    if(hid_report[1]==0x30){
+        wii_remote_report_mode(channel,REPORTMODE_TT_BIT,WII_REMOTE_REPORT_MODE);
+        wii_remote_request_status_report(channel);
+    }
+
+    switch(hid_report[1]){
+    case    0x30:                                    //0x30:Core Buttons
+    case    0x31:                                    //0x31:Core Buttons and Accelerometer
+    case    0x35:                                    //0x35:Core Buttons and Accelerometer with 16 Extension Bytes
+        wall_bot_remote( (char*)&hid_report[2] );
+        break;
+    default:
+        break;
+    }
+    return;
+}
+
+static void l2cap_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
+{
+    if (packet_type == HCI_EVENT_PACKET){
+        switch(packet[0]){
+        case L2CAP_EVENT_INCOMING_CONNECTION:
+            log_info("L2CAP_EVENT_INCOMING_CONNECTION\n");
+            l2cap_accept_connection_internal(channel);
+            break;
+        case L2CAP_EVENT_CHANNEL_OPENED:
+            if (packet[2]) {
+                log_info("Connection failed\n");
+                return;
+            }
+            log_info("interrupt channel connected\n");
+            wii_remocon_channel_id=channel;
+             break;
+        case L2CAP_EVENT_CHANNEL_CLOSED:
+            wii_remocon_channel_id=0;
+        //    use_extension=0;
+            haveKeyboard = 0;
+            hci_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, INQUIRY_INTERVAL, 0);
+            break;
+        default:
+            break;
+        }    
+    }
+    if (packet_type == L2CAP_DATA_PACKET){
+        hid_process_packet(channel,packet,size);
+    }
+}
+
+//static uint16_t control_channel=0;
+static void l2cap_control_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
+{
+    if (packet_type == HCI_EVENT_PACKET){
+        switch(packet[0]){
+        case L2CAP_EVENT_INCOMING_CONNECTION:
+            log_info("L2CAP_EVENT_INCOMING_CONNECTION\n");
+            l2cap_accept_connection_internal(channel);
+            break;
+        case L2CAP_EVENT_CHANNEL_OPENED:
+            if (packet[2]) {
+               // xputs("control Connection failed\n");
+                return;
+            }
+          //  xputs("control channel connected\n");
+            l2cap_create_channel_internal(NULL, l2cap_packet_handler, keyboard, PSM_HID_INTERRUPT, 150);
+            //control_channel=channel;
+            break;
+        case L2CAP_EVENT_CHANNEL_CLOSED:
+            log_info("L2CAP_CHANNEL_CLOSED\n");
+            //control_channel=0;
+            break;
+        case L2CAP_EVENT_CREDITS:
+            break;
+        default:
+            log_info("l2cap:unknown(%02x)\n",packet[0]);
+            break;
+        }    
+    }
+}
+
+// Bluetooth logic
+static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
+    bd_addr_t event_addr;
+    
+    switch (packet_type) {
+        case HCI_EVENT_PACKET:
+            switch (packet[0]) {
+                case BTSTACK_EVENT_STATE:
+                    // bt stack activated, get started - set local name
+                    if (packet[2] == HCI_STATE_WORKING) {
+                        hci_send_cmd(&hci_write_local_name, "mbed");
+                    }
+                    break;
+                
+                case HCI_EVENT_COMMAND_COMPLETE:
+                    if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)){
+                        bt_flip_addr(event_addr, &packet[6]);
+                        log_info("BD-ADDR: %s\n", bd_addr_to_str(event_addr));
+                        break;
+                    }
+                    if (COMMAND_COMPLETE_EVENT(packet, hci_write_local_name)){
+//                        hci_discoverable_control(1);
+//                        hci_send_cmd(&hci_write_authentication_enable, 1);
+                        hci_send_cmd(&hci_write_authentication_enable, 0);
+                        break;
+                    }
+                    if (COMMAND_COMPLETE_EVENT(packet, hci_write_authentication_enable)){
+                    inquiry:
+
+                        log_info("Inquiry\n");
+                        hci_send_cmd(&hci_inquiry, HCI_INQUIRY_LAP, INQUIRY_INTERVAL, 0);
+
+                        break;
+                    }
+                    if (COMMAND_COMPLETE_EVENT(packet, hci_inquiry_cancel) ) {
+                        //l2cap_create_channel_internal(NULL, l2cap_packet_handler, keyboard, PSM_HID_INTERRUPT, 150);
+                        l2cap_create_channel_internal(NULL, l2cap_control_packet_handler, keyboard, PSM_HID_CONTROL, 150);
+                        break;
+                    }
+                    break;
+                case HCI_EVENT_INQUIRY_RESULT:
+                    
+                    // ignore further results
+                    if (haveKeyboard) break;
+                    
+                    // ignore none wii remocon
+                    if(
+                        !(packet[12]==0x04 && packet[13]==0x25 && packet[14]==0x00)
+                    &&    !(packet[12]==0x08 && packet[13]==0x05 && packet[14]==0x00)    //RVL-CNT-01-TR
+                    ){
+                        break;
+                    }
+
+                    // flip addr
+                    bt_flip_addr(keyboard, &packet[3]);
+                    
+                    // show
+                    log_info("found wii remocon:%s\n",bd_addr_to_str(keyboard));
+
+                    haveKeyboard = 1;
+                    hci_send_cmd(&hci_inquiry_cancel);
+                    break;
+                    
+                case HCI_EVENT_INQUIRY_COMPLETE:
+                    log_info("No wii remocon\n");
+                    goto inquiry;
+                    //break;
+                case HCI_EVENT_LINK_KEY_REQUEST:
+                    // deny link key request
+                    log_info("Link key request\n");
+                    bt_flip_addr(event_addr, &packet[2]);
+                    hci_send_cmd(&hci_link_key_request_negative_reply, &event_addr);
+                    break;
+                    
+                case HCI_EVENT_PIN_CODE_REQUEST:
+                    // inform about pin code request
+                    bt_flip_addr(event_addr, &packet[2]);
+                    //wii remocon
+#if    WII_REMOTE_CONNECTION_SYNC
+                    bt_flip_addr(packet+2, addr_global);
+#endif
+                    log_info("Pin code request - using %02x%02x%02x%02x%02x%02x\n",packet[2],packet[3],packet[4],packet[5],packet[6],packet[7]);
+                    memset(packet+2+6,0x00,10);
+                    hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 6, &packet[2]);
+                    break;
+                case HCI_EVENT_CONNECTION_COMPLETE:
+                    //log_info("connection complete: status=%d\n",packet[2]);
+                    if(packet[2]){
+                        haveKeyboard=0;
+                        goto inquiry;
+                    }
+                    break;
+                default:
+                    break;
+            }
+            break;
+        default:
+            break;
+    }
+}
+
+static void  heartbeat_handler(struct timer *ts){
+    static int counter = 0;
+    counter++;
+
+    if(wii_remocon_channel_id){
+        lineBuffer[0]=0xa2;
+        lineBuffer[1]=0x11;
+        lineBuffer[2]=0x10<<(counter%4);        //LED Player1
+        if(l2cap_send_internal(wii_remocon_channel_id,(uint8_t*)lineBuffer,3)<0){
+            //error control init
+        }
+       ActLED = !ActLED;
+    }
+    run_loop_set_timer(ts, HEARTBEAT_PERIOD_MS);
+    run_loop_add_timer(ts);
+} 
+
+// main
+int main(void)
+{
+    pc.baud(921600);
+    log_info("%s\n", __FILE__);
+
+    // init LEDs
+    ActLED = 0;
+    
+    /// GET STARTED with BTstack ///
+    btstack_memory_init();
+    run_loop_init(RUN_LOOP_EMBEDDED);
+    
+    // init HCI
+    hci_transport_t* transport = hci_transport_usb_instance();
+    remote_device_db_t * remote_db = (remote_device_db_t *) &remote_device_db_memory;
+    hci_init(transport, NULL, NULL, remote_db);
+    
+    // init L2CAP
+    l2cap_init();
+    l2cap_register_packet_handler(packet_handler);
+    
+#if 0
+    // init RFCOMM
+    rfcomm_init();
+    rfcomm_register_packet_handler(packet_handler);
+    rfcomm_register_service_internal(NULL, rfcomm_channel_nr, 100);  // reserved channel, mtu=100
+
+
+    // init SDP, create record for SPP and register with SDP
+    sdp_init();
+    memset(spp_service_buffer, 0, sizeof(spp_service_buffer));
+    service_record_item_t * service_record_item = (service_record_item_t *) spp_service_buffer;
+    sdp_create_spp_service( (uint8_t*) &service_record_item->service_record, 1, "loopback");
+    log_info("SDP service buffer size: %u\n\r", (uint16_t) (sizeof(service_record_item_t) + de_get_len((uint8_t*) &service_record_item->service_record)));
+    sdp_register_service_internal(NULL, service_record_item);
+#endif
+    
+    // set one-shot timer
+    timer_source_t heartbeat;
+    heartbeat.process = &heartbeat_handler;
+    run_loop_set_timer(&heartbeat, HEARTBEAT_PERIOD_MS);
+    run_loop_add_timer(&heartbeat);
+    
+    log_info("SPP loopback demo...\n\r");
+
+     // turn on!
+    hci_power_control(HCI_POWER_ON);
+
+    // go!
+    run_loop_execute();    
+
+    return 0;
+}