Simons Wii controlled m3pi program

Dependencies:   mbed m3pi ID12RFIDIRQ

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers BTRuntime.cpp Source File

BTRuntime.cpp

00001 /*
00002 Copyright (c) 2010 Peter Barrett
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 #include "mbed.h"
00024 #include "USBHost.h"
00025 #include "Utils.h"
00026 
00027 #include <stdio.h>
00028 #include <stdlib.h>
00029 #include <stdio.h>
00030 #include <string.h>
00031 
00032 #include "Utils.h"
00033 #include "USBHost.h"
00034 #include "hci.h"
00035 
00036 extern void wii_data(char *data);
00037 
00038 void printf(const BD_ADDR* addr) {
00039     const u8* a = addr->addr;
00040     printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
00041 }
00042 
00043 #define MAX_HCL_SIZE 260
00044 #define MAX_ACL_SIZE 400
00045 
00046 class HCITransportUSB : public HCITransport {
00047     int _device;
00048     u8* _hciBuffer;
00049     u8* _aclBuffer;
00050 
00051 public:
00052     void Open(int device, u8* hciBuffer, u8* aclBuffer) {
00053         _device = device;
00054         _hciBuffer = hciBuffer;
00055         _aclBuffer = aclBuffer;
00056         USBInterruptTransfer(_device,0x81,_hciBuffer,MAX_HCL_SIZE,HciCallback,this);
00057         USBBulkTransfer(_device,0x82,_aclBuffer,MAX_ACL_SIZE,AclCallback,this);
00058     }
00059 
00060     static void HciCallback(int device, int endpoint, int status, u8* data, int len, void* userData) {
00061         HCI* t = ((HCITransportUSB*)userData)->_target;
00062         if (t)
00063             t->HCIRecv(data,len);
00064         USBInterruptTransfer(device,0x81,data,MAX_HCL_SIZE,HciCallback,userData);
00065     }
00066 
00067     static void AclCallback(int device, int endpoint, int status, u8* data, int len, void* userData) {
00068         HCI* t = ((HCITransportUSB*)userData)->_target;
00069         if (t)
00070             t->ACLRecv(data,len);
00071         USBBulkTransfer(device,0x82,data,MAX_ACL_SIZE,AclCallback,userData);
00072     }
00073 
00074     virtual void HCISend(const u8* data, int len) {
00075         USBControlTransfer(_device,REQUEST_TYPE_CLASS, 0, 0, 0,(u8*)data,len);
00076     }
00077 
00078     virtual void ACLSend(const u8* data, int len) {
00079         USBBulkTransfer(_device,0x02,(u8*)data,len);
00080     }
00081 };
00082 
00083 
00084 #define WII_REMOTE 0x042500
00085 
00086 class HIDBluetooth {
00087     int _control;   // Sockets for control (out) and interrupt (in)
00088     int _interrupt;
00089     int _devClass;
00090     BD_ADDR _addr;
00091     u8  _pad[2];    // Struct align
00092 
00093 public:
00094     HIDBluetooth() : _control(0),_interrupt(0),_devClass(0) {};
00095 
00096     bool InUse() {
00097         return _control != 0;
00098     }
00099 
00100     static void OnHidInterrupt(int socket, SocketState state, const u8* data, int len, void* userData) {
00101         HIDBluetooth* t = (HIDBluetooth*)userData;
00102         if (data) {
00103             if (t->_devClass == WII_REMOTE && data[1] == 0x30) {
00104                 //printf("================wii====================\n");
00105                 t->Led();
00106                 t->Hid();   // ask for accelerometer
00107                 t->_devClass = 0;
00108             }
00109 
00110             const u8* d = data;
00111             switch (d[1]) {
00112 
00113                 case 0x02: {
00114                     int x = (signed char)d[3];
00115                     int y = (signed char)d[4];
00116                     printf("Mouse %2X dx:%d dy:%d\n",d[2],x,y);
00117                 }
00118                 break;
00119 
00120                 case 0x37: {
00121                     wii_data((char*)&d[2]);
00122 /*
00123                     char *dd = (char*)&d[2];
00124 
00125                     Wiimote wii;
00126                     wii.decode(dd);
00127                     wii.dump();
00128 
00129                     float dir = 0;
00130                     if(wii.one) {
00131                         dir = -1;
00132                     }
00133                     if(wii.two) {
00134                         dir = 1;
00135                        }
00136                         
00137                    if (dir != 0) {
00138                          float f = wii.wheel / 200.0f;                                                    
00139                         
00140                         if(f < 0) { 
00141                             m3pi.left_motor(dir * (0.6 + abs(f)));
00142                             m3pi.right_motor(dir * (0.6 - (abs(f) / 2)));
00143                         } else {
00144                             m3pi.right_motor(dir * (0.6 + abs(f)));
00145                             m3pi.left_motor(dir * (0.6 - (abs(f) / 2)));
00146                         }
00147                     } else {
00148                         m3pi.left_motor(0);
00149                         m3pi.right_motor(0);
00150                     }
00151                     
00152                     */
00153                 }
00154             }
00155         }
00156     }
00157     
00158     static void OnHidControl(int socket, SocketState state, const u8* data, int len, void* userData) {
00159         printf("OnHidControl\n");
00160         if (data)
00161             printHex(data,len);
00162     }
00163 
00164     void Open(BD_ADDR* bdAddr, inquiry_info* info) {
00165         printf("L2CAPAddr size %d\n",sizeof(L2CAPAddr));
00166         _addr = *bdAddr;
00167         L2CAPAddr sockAddr;
00168         sockAddr.bdaddr = _addr;
00169         sockAddr.psm = L2CAP_PSM_HID_INTR;
00170         printf("Socket_Open size %d\n",sizeof(L2CAPAddr));
00171         _interrupt = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidInterrupt,this);
00172         sockAddr.psm = L2CAP_PSM_HID_CNTL;
00173         _control = Socket_Open(SOCKET_L2CAP,&sockAddr.hdr,OnHidControl,this);
00174 
00175         printfBytes("OPEN DEVICE CLASS",info->dev_class,3);
00176         _devClass = (info->dev_class[0] << 16) | (info->dev_class[1] << 8) | info->dev_class[2];
00177     }
00178 
00179     void Close() {
00180         if (_control)
00181             Socket_Close(_control);
00182         if (_interrupt)
00183             Socket_Close(_interrupt);
00184         _control = _interrupt = 0;
00185     }
00186 
00187     void Led(int id = 0x10) {
00188         u8 led[3] = {0x52, 0x11, id};
00189         if (_control)
00190             Socket_Send(_control,led,3);
00191     }
00192 
00193     void Hid(int report = 0x37) {
00194         u8 hid[4] = { 0x52, 0x12, 0x00, report };
00195         if (_control != -1)
00196             Socket_Send(_control,hid,4);
00197     }
00198 };
00199 
00200 
00201 HCI* gHCI = 0;
00202 
00203 #define MAX_HID_DEVICES 8
00204 
00205 class ShellApp {
00206     char _line[64];
00207     HIDBluetooth    _hids[MAX_HID_DEVICES];
00208 
00209 public:
00210     void Ready() {
00211         printf("HIDBluetooth %d\n",sizeof(HIDBluetooth));
00212         memset(_hids,0,sizeof(_hids));
00213         Inquiry();
00214 
00215     }
00216 
00217     //  We have connected to a device
00218     void ConnectionComplete(HCI* hci, connection_info* info) {
00219         printf("ConnectionComplete ");
00220         BD_ADDR* a = &info->bdaddr;
00221         //printf(a);
00222         BTDevice* bt = hci->Find(a);
00223         HIDBluetooth* hid = NewHIDBluetooth();
00224         printf("%08x %08x\n",bt,hid);
00225         if (hid)
00226             hid->Open(a,&bt->_info);
00227     }
00228 
00229     HIDBluetooth* NewHIDBluetooth() {
00230         for (int i = 0; i < MAX_HID_DEVICES; i++)
00231             if (!_hids[i].InUse())
00232                 return _hids+i;
00233         return 0;
00234     }
00235 
00236     void ConnectDevices() {
00237         BTDevice* devs[8];
00238         int count = gHCI->GetDevices(devs,8);
00239         for (int i = 0; i < count; i++) {
00240             printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
00241             if (devs[i]->_handle == 0) {
00242                 BD_ADDR* bd = &devs[i]->_info.bdaddr;
00243                 printf("Connecting to ");
00244                 //printf(bd);
00245                 printf("\n");
00246                 gHCI->CreateConnection(bd);
00247             }
00248         }
00249     }
00250 /*
00251     const char* ReadLine() {
00252         int i;
00253         for (i = 0; i < 255; ) {
00254             USBLoop();
00255             int c = GetConsoleChar();
00256             if (c == -1)
00257                 continue;
00258             if (c == '\n' || c == 13)
00259                 break;
00260             _line[i++] = c;
00261         }
00262         _line[i] = 0;
00263         return _line;
00264     }
00265 */
00266     void Inquiry() {
00267         printf("Inquiry..\n");
00268         gHCI->Inquiry();
00269     }
00270 
00271     void List() {
00272 #if 0
00273         printf("%d devices\n",_deviceCount);
00274         for (int i = 0; i < _deviceCount; i++) {
00275             printf(&_devices[i].info.bdaddr);
00276             printf("\n");
00277         }
00278 #endif
00279     }
00280 
00281     void Connect() {
00282         ConnectDevices();
00283     }
00284 
00285     void Disconnect() {
00286         gHCI->DisconnectAll();
00287     }
00288 
00289     void CloseMouse() {
00290     }
00291 
00292     void Quit() {
00293         CloseMouse();
00294     }
00295 
00296 };
00297 
00298 //  Instance
00299 ShellApp gApp;
00300 
00301 static int HciCallback(HCI* hci, HCI_CALLBACK_EVENT evt, const u8* data, int len) {
00302     switch (evt) {
00303         case CALLBACK_READY:
00304             printf("CALLBACK_READY\n");
00305             gApp.Ready();
00306             break;
00307 
00308         case CALLBACK_INQUIRY_RESULT:
00309             printf("CALLBACK_INQUIRY_RESULT ");
00310             //printf((BD_ADDR*)data);
00311             printf("\n");
00312             break;
00313 
00314         case CALLBACK_INQUIRY_DONE:
00315             printf("CALLBACK_INQUIRY_DONE\n");
00316             gApp.ConnectDevices();
00317             break;
00318 
00319         case CALLBACK_REMOTE_NAME: {
00320             BD_ADDR* addr = (BD_ADDR*)data;
00321             const char* name = (const char*)(data + 6);
00322             //printf(addr);
00323             printf(" % s\n",name);
00324         }
00325         break;
00326 
00327         case CALLBACK_CONNECTION_COMPLETE:
00328             gApp.ConnectionComplete(hci,(connection_info*)data);
00329             break;
00330     };
00331     return 0;
00332 }
00333 
00334 //  these should be placed in the DMA SRAM
00335 typedef struct {
00336     u8 _hciBuffer[MAX_HCL_SIZE];
00337     u8 _aclBuffer[MAX_ACL_SIZE];
00338 } SRAMPlacement;
00339 
00340 HCITransportUSB _HCITransportUSB;
00341 HCI _HCI;
00342 
00343 u8* USBGetBuffer(u32* len);
00344 int OnBluetoothInsert(int device) {
00345     printf("Bluetooth inserted of %d\n",device);
00346     u32 sramLen;
00347     u8* sram =  USBGetBuffer(&sramLen);
00348     sram = (u8*)(((u32)sram + 1023) & ~1023);
00349     SRAMPlacement* s = (SRAMPlacement*)sram;
00350     _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);
00351     _HCI.Open(&_HCITransportUSB,HciCallback);
00352     RegisterSocketHandler(SOCKET_L2CAP,&_HCI);
00353     gHCI = &_HCI;
00354     gApp.Inquiry();
00355     return 0;
00356 }
00357