2018_Project-R / Mbed 2 deprecated PS3_BlueUSB_downstate

PS3下の階

Dependencies:   FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed

Fork of PS3_BlueUSB by Bart Janssens

Ps3USB.cpp@1:b2063ffa4927, 2018-01-11 (annotated)

Committer:
e5115026
Date:
Thu Jan 11 07:47:26 2018 +0000
Revision:
1:b2063ffa4927
Parent:
0:99a111b75cb4 
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BartJanssens 0:99a111b75cb4 1 /*
BartJanssens 0:99a111b75cb4 2 Copyright (c) 2011 Bart Janssens
BartJanssens 0:99a111b75cb4 3
BartJanssens 0:99a111b75cb4 4 Permission is hereby granted, free of charge, to any person obtaining a copy
BartJanssens 0:99a111b75cb4 5 of this software and associated documentation files (the "Software"), to deal
BartJanssens 0:99a111b75cb4 6 in the Software without restriction, including without limitation the rights
BartJanssens 0:99a111b75cb4 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BartJanssens 0:99a111b75cb4 8 copies of the Software, and to permit persons to whom the Software is
BartJanssens 0:99a111b75cb4 9 furnished to do so, subject to the following conditions:
BartJanssens 0:99a111b75cb4 10
BartJanssens 0:99a111b75cb4 11 The above copyright notice and this permission notice shall be included in
BartJanssens 0:99a111b75cb4 12 all copies or substantial portions of the Software.
BartJanssens 0:99a111b75cb4 13
BartJanssens 0:99a111b75cb4 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BartJanssens 0:99a111b75cb4 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BartJanssens 0:99a111b75cb4 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BartJanssens 0:99a111b75cb4 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BartJanssens 0:99a111b75cb4 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BartJanssens 0:99a111b75cb4 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BartJanssens 0:99a111b75cb4 20 THE SOFTWARE.
BartJanssens 0:99a111b75cb4 21 */
BartJanssens 0:99a111b75cb4 22
BartJanssens 0:99a111b75cb4 23 #include <stdio.h>
BartJanssens 0:99a111b75cb4 24 #include <stdlib.h>
BartJanssens 0:99a111b75cb4 25 #include <string.h>
BartJanssens 0:99a111b75cb4 26
BartJanssens 0:99a111b75cb4 27 #include "USBHost.h"
BartJanssens 0:99a111b75cb4 28 #include "hci.h"
BartJanssens 0:99a111b75cb4 29 #include "Utils.h"
BartJanssens 0:99a111b75cb4 30 #include "ps3.h"
BartJanssens 0:99a111b75cb4 31
BartJanssens 0:99a111b75cb4 32 #include "mbed.h"
BartJanssens 0:99a111b75cb4 33
BartJanssens 0:99a111b75cb4 34
BartJanssens 0:99a111b75cb4 35
BartJanssens 0:99a111b75cb4 36 #define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
BartJanssens 0:99a111b75cb4 37 #define PS3EVT AUTOEVT(CLASS_HID,0,0)
BartJanssens 0:99a111b75cb4 38 #define byteswap(x) ((x >> 8) | (x << 8))
e5115026 1:b2063ffa4927 39 #define device_baud 115200 //上のマイコンにするバンド数
e5115026 1:b2063ffa4927 40 #define send_device_rate 0.03 //割り込みレート
e5115026 1:b2063ffa4927 41
BartJanssens 0:99a111b75cb4 42
BartJanssens 0:99a111b75cb4 43 u8 ps3_data[48];
e5115026 1:b2063ffa4927 44 Serial order_device(p28,p27);//上のマイコンに反応させるための初期化宣言
BartJanssens 0:99a111b75cb4 45
BartJanssens 0:99a111b75cb4 46
e5115026 1:b2063ffa4927 47 //*************追加項目ピン設定
e5115026 1:b2063ffa4927 48 PwmOut led1(LED1);
e5115026 1:b2063ffa4927 49 PwmOut led2(LED2);
e5115026 1:b2063ffa4927 50 PwmOut led3(LED3);
e5115026 1:b2063ffa4927 51 PwmOut led4(LED4);
e5115026 1:b2063ffa4927 52 int joyL_x =0;
e5115026 1:b2063ffa4927 53 int joyL_y =0;
e5115026 1:b2063ffa4927 54 int joyR_x =0;
e5115026 1:b2063ffa4927 55 int joyR_y =0;
e5115026 1:b2063ffa4927 56 int L1 =0;
e5115026 1:b2063ffa4927 57 int R1 =0;
e5115026 1:b2063ffa4927 58 int up =0;
e5115026 1:b2063ffa4927 59 int down =0;
e5115026 1:b2063ffa4927 60 int right =0;
e5115026 1:b2063ffa4927 61 int left =0;
e5115026 1:b2063ffa4927 62 int Xmove =0;
e5115026 1:b2063ffa4927 63 int Ymove =0;
e5115026 1:b2063ffa4927 64 int angle_omni =0;
e5115026 1:b2063ffa4927 65
e5115026 1:b2063ffa4927 66 int speed_mode =1;
e5115026 1:b2063ffa4927 67
e5115026 1:b2063ffa4927 68 void omni_move(double x_speed,double y_speed,double angle);
e5115026 1:b2063ffa4927 69 char Buf[8];
e5115026 1:b2063ffa4927 70
e5115026 1:b2063ffa4927 71
e5115026 1:b2063ffa4927 72 //**************
e5115026 1:b2063ffa4927 73
e5115026 1:b2063ffa4927 74 //以下項目上への指令を送るためのBuf
e5115026 1:b2063ffa4927 75 /*
e5115026 1:b2063ffa4927 76 void send_device(){
e5115026 1:b2063ffa4927 77 char Buf[8];
e5115026 1:b2063ffa4927 78 Buf[0] = up;
e5115026 1:b2063ffa4927 79 Buf[1] = down;
e5115026 1:b2063ffa4927 80 Buf[2] = right;
e5115026 1:b2063ffa4927 81 Buf[3] = left;
e5115026 1:b2063ffa4927 82 Buf[4] = 0;
e5115026 1:b2063ffa4927 83 Buf[5] = 0;
e5115026 1:b2063ffa4927 84 order_device.putc(Buf[0]);
e5115026 1:b2063ffa4927 85 order_device.putc(Buf[1]);
e5115026 1:b2063ffa4927 86 order_device.putc(Buf[2]);
e5115026 1:b2063ffa4927 87 order_device.putc(Buf[3]);
e5115026 1:b2063ffa4927 88 order_device.putc(Buf[4]);
e5115026 1:b2063ffa4927 89 order_device.putc(Buf[5]);
e5115026 1:b2063ffa4927 90 }
e5115026 1:b2063ffa4927 91 */
e5115026 1:b2063ffa4927 92 //*********まで
e5115026 1:b2063ffa4927 93
BartJanssens 0:99a111b75cb4 94 Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
BartJanssens 0:99a111b75cb4 95 {
BartJanssens 0:99a111b75cb4 96 printf("Creating new sixaxis \r\n");
BartJanssens 0:99a111b75cb4 97 _device = device;
BartJanssens 0:99a111b75cb4 98 _configuration = configuration;
BartJanssens 0:99a111b75cb4 99 _interfaceNumber = interfaceNumber;
BartJanssens 0:99a111b75cb4 100 printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
BartJanssens 0:99a111b75cb4 101 int result;
BartJanssens 0:99a111b75cb4 102 int err;
BartJanssens 0:99a111b75cb4 103
BartJanssens 0:99a111b75cb4 104 _count = 1;
BartJanssens 0:99a111b75cb4 105
BartJanssens 0:99a111b75cb4 106 u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
BartJanssens 0:99a111b75cb4 107 0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
BartJanssens 0:99a111b75cb4 108 0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
BartJanssens 0:99a111b75cb4 109 0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
BartJanssens 0:99a111b75cb4 110 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
BartJanssens 0:99a111b75cb4 111 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
BartJanssens 0:99a111b75cb4 112 memcpy(ledrumble,abuffer,48);
BartJanssens 0:99a111b75cb4 113
BartJanssens 0:99a111b75cb4 114 EndpointDescriptor* ep;
BartJanssens 0:99a111b75cb4 115
BartJanssens 0:99a111b75cb4 116 u8 buffer[255];
BartJanssens 0:99a111b75cb4 117 err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
BartJanssens 0:99a111b75cb4 118 if (err < 0)
BartJanssens 0:99a111b75cb4 119 printf("Failed to get descriptor\r\n");
BartJanssens 0:99a111b75cb4 120
BartJanssens 0:99a111b75cb4 121
BartJanssens 0:99a111b75cb4 122 int len = buffer[2] | (buffer[3] << 8);
BartJanssens 0:99a111b75cb4 123 u8* d = buffer;
BartJanssens 0:99a111b75cb4 124 u8* end = d + len;
BartJanssens 0:99a111b75cb4 125 while (d < end)
BartJanssens 0:99a111b75cb4 126 {
BartJanssens 0:99a111b75cb4 127 if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
BartJanssens 0:99a111b75cb4 128 {
BartJanssens 0:99a111b75cb4 129 InterfaceDescriptor* id = (InterfaceDescriptor*)d;
BartJanssens 0:99a111b75cb4 130 if (id->bInterfaceNumber == _interfaceNumber)
BartJanssens 0:99a111b75cb4 131 {
BartJanssens 0:99a111b75cb4 132 d += d[0];
BartJanssens 0:99a111b75cb4 133 while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
BartJanssens 0:99a111b75cb4 134 {
BartJanssens 0:99a111b75cb4 135 if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
BartJanssens 0:99a111b75cb4 136 ep = (EndpointDescriptor*)d;
BartJanssens 0:99a111b75cb4 137
BartJanssens 0:99a111b75cb4 138 if (ep->bEndpointAddress == 0x02) {
BartJanssens 0:99a111b75cb4 139 printf("PS3 input endpoint (0x02) found\r\n");
BartJanssens 0:99a111b75cb4 140 input_ep = 0x02;
BartJanssens 0:99a111b75cb4 141
BartJanssens 0:99a111b75cb4 142 }
BartJanssens 0:99a111b75cb4 143 if (ep->bEndpointAddress == 0x81) {
BartJanssens 0:99a111b75cb4 144 printf("PS3 output endpoint (0x81) found\r\n");
BartJanssens 0:99a111b75cb4 145 output_ep = 0x81;
BartJanssens 0:99a111b75cb4 146 //AddAutoEvent(device,id,(EndpointDescriptor*)d);
BartJanssens 0:99a111b75cb4 147 }
BartJanssens 0:99a111b75cb4 148 d += d[0];
BartJanssens 0:99a111b75cb4 149 }
BartJanssens 0:99a111b75cb4 150 }
BartJanssens 0:99a111b75cb4 151 }
BartJanssens 0:99a111b75cb4 152 d += d[0];
BartJanssens 0:99a111b75cb4 153 }
BartJanssens 0:99a111b75cb4 154
BartJanssens 0:99a111b75cb4 155 }
BartJanssens 0:99a111b75cb4 156
BartJanssens 0:99a111b75cb4 157 int Ps3USB::Enable()
BartJanssens 0:99a111b75cb4 158 {
BartJanssens 0:99a111b75cb4 159 int err;
BartJanssens 0:99a111b75cb4 160
BartJanssens 0:99a111b75cb4 161 u8 enable[4] = {0x42,0x0c,0x00,0x00};
BartJanssens 0:99a111b75cb4 162
BartJanssens 0:99a111b75cb4 163
BartJanssens 0:99a111b75cb4 164 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
BartJanssens 0:99a111b75cb4 165 //printf("set report result = %d\r\n", err);
BartJanssens 0:99a111b75cb4 166 _count ++;
BartJanssens 0:99a111b75cb4 167 if (_count == 25) _count = 1;
BartJanssens 0:99a111b75cb4 168
BartJanssens 0:99a111b75cb4 169 err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
BartJanssens 0:99a111b75cb4 170 wait_ms(4);
BartJanssens 0:99a111b75cb4 171
BartJanssens 0:99a111b75cb4 172 return 0;
BartJanssens 0:99a111b75cb4 173
BartJanssens 0:99a111b75cb4 174 }
BartJanssens 0:99a111b75cb4 175
BartJanssens 0:99a111b75cb4 176
BartJanssens 0:99a111b75cb4 177
BartJanssens 0:99a111b75cb4 178
BartJanssens 0:99a111b75cb4 179 int Ps3USB::SetPair(u8* bdAddr)
BartJanssens 0:99a111b75cb4 180 {
BartJanssens 0:99a111b75cb4 181 int err;
BartJanssens 0:99a111b75cb4 182
BartJanssens 0:99a111b75cb4 183 u8 buf[8];
BartJanssens 0:99a111b75cb4 184 u8 buf2[6];
BartJanssens 0:99a111b75cb4 185
BartJanssens 0:99a111b75cb4 186 memcpy(buf2,bdAddr,6);
BartJanssens 0:99a111b75cb4 187
BartJanssens 0:99a111b75cb4 188 buf[0] = 0x01;
BartJanssens 0:99a111b75cb4 189 buf[1] = 0x00;
BartJanssens 0:99a111b75cb4 190 buf[2] = buf2[0];
BartJanssens 0:99a111b75cb4 191 buf[3] = buf2[1];
BartJanssens 0:99a111b75cb4 192 buf[4] = buf2[2];
BartJanssens 0:99a111b75cb4 193 buf[5] = buf2[3];
BartJanssens 0:99a111b75cb4 194 buf[6] = buf2[4];
BartJanssens 0:99a111b75cb4 195 buf[7] = buf2[5];
BartJanssens 0:99a111b75cb4 196
BartJanssens 0:99a111b75cb4 197 //set Mac address
BartJanssens 0:99a111b75cb4 198 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
BartJanssens 0:99a111b75cb4 199 wait_ms(4);
BartJanssens 0:99a111b75cb4 200 printf("set Mac address to %02X:%02X:%02X:%02X:%02X:%02X , result = %d\r\n", buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], err);
BartJanssens 0:99a111b75cb4 201
BartJanssens 0:99a111b75cb4 202 return 0;
BartJanssens 0:99a111b75cb4 203 }
BartJanssens 0:99a111b75cb4 204
BartJanssens 0:99a111b75cb4 205 int Ps3USB::ShowPair()
BartJanssens 0:99a111b75cb4 206 {
BartJanssens 0:99a111b75cb4 207 int err;
BartJanssens 0:99a111b75cb4 208
BartJanssens 0:99a111b75cb4 209 u8 buf[8];
BartJanssens 0:99a111b75cb4 210 //get Mac address
BartJanssens 0:99a111b75cb4 211 err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
BartJanssens 0:99a111b75cb4 212 wait_ms(4);
BartJanssens 0:99a111b75cb4 213 printf("Mac address is set to %02X:%02X:%02X:%02X:%02X:%02X , result = %d\r\n",buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], err);
BartJanssens 0:99a111b75cb4 214
BartJanssens 0:99a111b75cb4 215
BartJanssens 0:99a111b75cb4 216 return 0;
BartJanssens 0:99a111b75cb4 217 }
BartJanssens 0:99a111b75cb4 218
BartJanssens 0:99a111b75cb4 219 int Ps3USB::Led(int i)
BartJanssens 0:99a111b75cb4 220 {
BartJanssens 0:99a111b75cb4 221 int err;
BartJanssens 0:99a111b75cb4 222 u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
BartJanssens 0:99a111b75cb4 223 u8 buf[48];
BartJanssens 0:99a111b75cb4 224
BartJanssens 0:99a111b75cb4 225 if (i < 7) ledrumble[9] = ledpattern[i];
BartJanssens 0:99a111b75cb4 226 memcpy(buf, ledrumble, 48);
BartJanssens 0:99a111b75cb4 227
BartJanssens 0:99a111b75cb4 228 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
BartJanssens 0:99a111b75cb4 229 wait_ms(4);
BartJanssens 0:99a111b75cb4 230
BartJanssens 0:99a111b75cb4 231 return 0;
BartJanssens 0:99a111b75cb4 232 }
BartJanssens 0:99a111b75cb4 233
BartJanssens 0:99a111b75cb4 234 // left and right: duration and power, both from 0 to 255
BartJanssens 0:99a111b75cb4 235 int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
BartJanssens 0:99a111b75cb4 236 {
BartJanssens 0:99a111b75cb4 237 int err;
BartJanssens 0:99a111b75cb4 238 u8 buf[48];
BartJanssens 0:99a111b75cb4 239
BartJanssens 0:99a111b75cb4 240 memcpy(buf, ledrumble, 48);
BartJanssens 0:99a111b75cb4 241 buf[1] = duration_right;
BartJanssens 0:99a111b75cb4 242 buf[2] = power_right;
BartJanssens 0:99a111b75cb4 243 buf[3] = duration_left;
BartJanssens 0:99a111b75cb4 244 buf[4] = power_left;
BartJanssens 0:99a111b75cb4 245
BartJanssens 0:99a111b75cb4 246 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
BartJanssens 0:99a111b75cb4 247 wait_ms(4);
BartJanssens 0:99a111b75cb4 248
BartJanssens 0:99a111b75cb4 249 return 0;
BartJanssens 0:99a111b75cb4 250 }
BartJanssens 0:99a111b75cb4 251
BartJanssens 0:99a111b75cb4 252
BartJanssens 0:99a111b75cb4 253 void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
BartJanssens 0:99a111b75cb4 254 {
BartJanssens 0:99a111b75cb4 255 Ps3USB* t = (Ps3USB*)userData;
BartJanssens 0:99a111b75cb4 256
BartJanssens 0:99a111b75cb4 257 t->_count ++;
BartJanssens 0:99a111b75cb4 258 if (t->_count == 25) t->_count = 1;
BartJanssens 0:99a111b75cb4 259
BartJanssens 0:99a111b75cb4 260 ParsePs3Result(data, sizeof(ps3report),t->_count);
BartJanssens 0:99a111b75cb4 261 USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
BartJanssens 0:99a111b75cb4 262 wait_ms(4);
BartJanssens 0:99a111b75cb4 263
BartJanssens 0:99a111b75cb4 264 }
e5115026 1:b2063ffa4927 265 /*
e5115026 1:b2063ffa4927 266 void Connect(){
e5115026 1:b2063ffa4927 267 order_device.baud(device_baud);//上のマイコンとのbaud指定
e5115026 1:b2063ffa4927 268 tick_device.attach(&send_device,send_device_rate);//割り込み
e5115026 1:b2063ffa4927 269 }
e5115026 1:b2063ffa4927 270 */
e5115026 1:b2063ffa4927 271
e5115026 1:b2063ffa4927 272
BartJanssens 0:99a111b75cb4 273
BartJanssens 0:99a111b75cb4 274 int ParsePs3Result(const u8* data,int len,int count)
e5115026 1:b2063ffa4927 275 {
e5115026 1:b2063ffa4927 276 order_device.baud(device_baud);
BartJanssens 0:99a111b75cb4 277 ps3report* _ps3report = (ps3report*)data;
BartJanssens 0:99a111b75cb4 278 if (count == 24) printf("LSX LSY RSX RSY UPA RPA DPA RPA L2 R2 L1 R1 TRI CIR CRO SQU ACX ACY ACZ GYZ \r\n");
BartJanssens 0:99a111b75cb4 279 printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
BartJanssens 0:99a111b75cb4 280 _ps3report->LeftStickX,
BartJanssens 0:99a111b75cb4 281 _ps3report->LeftStickY,
BartJanssens 0:99a111b75cb4 282 _ps3report->RightStickX,
BartJanssens 0:99a111b75cb4 283 _ps3report->RightStickY,
BartJanssens 0:99a111b75cb4 284 _ps3report->PressureUp,
BartJanssens 0:99a111b75cb4 285 _ps3report->PressureRight,
BartJanssens 0:99a111b75cb4 286 _ps3report->PressureDown,
BartJanssens 0:99a111b75cb4 287 _ps3report->PressureLeft,
BartJanssens 0:99a111b75cb4 288 _ps3report->PressureL2,
BartJanssens 0:99a111b75cb4 289 _ps3report->PressureR2,
BartJanssens 0:99a111b75cb4 290 _ps3report->PressureL1,
BartJanssens 0:99a111b75cb4 291 _ps3report->PressureR1,
BartJanssens 0:99a111b75cb4 292 _ps3report->PressureTriangle,
BartJanssens 0:99a111b75cb4 293 _ps3report->PressureCircle,
BartJanssens 0:99a111b75cb4 294 _ps3report->PressureCross,
BartJanssens 0:99a111b75cb4 295 _ps3report->PressureSquare,
BartJanssens 0:99a111b75cb4 296 (_ps3report->AccelX),
BartJanssens 0:99a111b75cb4 297 (_ps3report->AccelY),
BartJanssens 0:99a111b75cb4 298 (_ps3report->AccelZ),
BartJanssens 0:99a111b75cb4 299 (_ps3report->GyroZ));
BartJanssens 0:99a111b75cb4 300 //printfBytes("data",data,len);
e5115026 1:b2063ffa4927 301 joyL_x =_ps3report->LeftStickX;
e5115026 1:b2063ffa4927 302 joyL_y =_ps3report->LeftStickY;
e5115026 1:b2063ffa4927 303 joyR_x =_ps3report->RightStickY;
e5115026 1:b2063ffa4927 304 joyR_y =_ps3report->RightStickX;
e5115026 1:b2063ffa4927 305 L1 =_ps3report->PressureL1;
e5115026 1:b2063ffa4927 306 R1 =_ps3report->PressureR1;
e5115026 1:b2063ffa4927 307 up =_ps3report->PressureUp;
e5115026 1:b2063ffa4927 308 down =_ps3report->PressureDown;
e5115026 1:b2063ffa4927 309 right =_ps3report->PressureRight;
e5115026 1:b2063ffa4927 310 left =_ps3report->PressureLeft;
e5115026 1:b2063ffa4927 311
e5115026 1:b2063ffa4927 312
e5115026 1:b2063ffa4927 313 //デットバンドの設定
e5115026 1:b2063ffa4927 314 if(120<=joyL_x&&joyL_x<=129)
e5115026 1:b2063ffa4927 315 {
e5115026 1:b2063ffa4927 316 joyL_x=126;
e5115026 1:b2063ffa4927 317 }
e5115026 1:b2063ffa4927 318 if(120<=joyL_y&&joyL_y<=129)
e5115026 1:b2063ffa4927 319 {
e5115026 1:b2063ffa4927 320 joyL_y=126;
e5115026 1:b2063ffa4927 321 }
e5115026 1:b2063ffa4927 322 if(120<=joyR_x&&joyR_x<=129)
e5115026 1:b2063ffa4927 323 {
e5115026 1:b2063ffa4927 324 joyR_x=126;
e5115026 1:b2063ffa4927 325 }
e5115026 1:b2063ffa4927 326 if(120<=joyR_y&&joyR_y<=129)
e5115026 1:b2063ffa4927 327 {
e5115026 1:b2063ffa4927 328 joyR_y=126;
e5115026 1:b2063ffa4927 329 }
e5115026 1:b2063ffa4927 330
e5115026 1:b2063ffa4927 331
e5115026 1:b2063ffa4927 332 //低速モードと高速モード設定
e5115026 1:b2063ffa4927 333 if(L1>=100){
e5115026 1:b2063ffa4927 334 speed_mode=2;
e5115026 1:b2063ffa4927 335 }
e5115026 1:b2063ffa4927 336 else{
e5115026 1:b2063ffa4927 337 speed_mode=20;
e5115026 1:b2063ffa4927 338 }
e5115026 1:b2063ffa4927 339
e5115026 1:b2063ffa4927 340
e5115026 1:b2063ffa4927 341 order_device.printf("%d,%d,%d,%d\n", up, down, right, left);
e5115026 1:b2063ffa4927 342
e5115026 1:b2063ffa4927 343
e5115026 1:b2063ffa4927 344 Xmove =(joyL_x-126)*speed_mode;
e5115026 1:b2063ffa4927 345 Ymove =(joyL_y-126)*speed_mode;
e5115026 1:b2063ffa4927 346 angle_omni =(joyR_y-126)*speed_mode;
e5115026 1:b2063ffa4927 347
e5115026 1:b2063ffa4927 348 omni_move(angle_omni,Ymove,Xmove);
e5115026 1:b2063ffa4927 349 led1=joyL_x/255.0;
e5115026 1:b2063ffa4927 350 led2=joyL_y/255.0;
e5115026 1:b2063ffa4927 351 led3=joyR_y/255.0;
e5115026 1:b2063ffa4927 352
e5115026 1:b2063ffa4927 353 printf("joyL_x:%d joyL_y:%d angle_omni:%d \r\n" , Xmove, Ymove, angle_omni);
e5115026 1:b2063ffa4927 354 }
BartJanssens 0:99a111b75cb4 355
BartJanssens 0:99a111b75cb4 356