うおーるぼっとをWiiリモコンでコントロールする新しいプログラムです。 以前のものより、Wiiリモコンが早く繋がる様になりました。 It is a program which controls A with the Wii remote. ※ A Bluetooth dongle and a Wii remote control are needed.
Dependencies: USBHost mbed FATFileSystem mbed-rtos
main.cpp
- Committer:
- jksoft
- Date:
- 2013-06-10
- Revision:
- 0:fccb789424fc
File content as of revision 0:fccb789424fc:
#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; }