Gong Chen / Mbed 2 deprecated LPC_PS3_BlueUSB_four

Fork BlueUSB lib for NHK robot contest 2014 TEU JAPAN

Dependencies:   C12832_lcd FatFileSystem TextLCD mbed

Fork of PS3_BlueUSB by Bart Janssens

Ps3USB.cpp@1:335bce3448c3, 2014-01-12 (annotated)

Committer:
ppr2013G2
Date:
Sun Jan 12 07:08:34 2014 +0000
Revision:
1:335bce3448c3
Parent:
0:99a111b75cb4
Child:
2:2c4daaf8b15c
add air part

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>
ppr2013G2 1:335bce3448c3 26 //#include <TextLCD.h>//for orange borad
ppr2013G2 1:335bce3448c3 27 #include "C12832_lcd.h"
ppr2013G2 1:335bce3448c3 28
BartJanssens 0:99a111b75cb4 29
BartJanssens 0:99a111b75cb4 30 #include "USBHost.h"
BartJanssens 0:99a111b75cb4 31 #include "hci.h"
BartJanssens 0:99a111b75cb4 32 #include "Utils.h"
BartJanssens 0:99a111b75cb4 33 #include "ps3.h"
BartJanssens 0:99a111b75cb4 34
BartJanssens 0:99a111b75cb4 35 #include "mbed.h"
BartJanssens 0:99a111b75cb4 36
BartJanssens 0:99a111b75cb4 37
BartJanssens 0:99a111b75cb4 38
BartJanssens 0:99a111b75cb4 39 #define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
BartJanssens 0:99a111b75cb4 40 #define PS3EVT AUTOEVT(CLASS_HID,0,0)
BartJanssens 0:99a111b75cb4 41 #define byteswap(x) ((x >> 8) | (x << 8))
BartJanssens 0:99a111b75cb4 42
ppr2013G2 1:335bce3448c3 43 #define M_1_CW_MIN 63
ppr2013G2 1:335bce3448c3 44 #define M_2_CW_MIN 191
ppr2013G2 1:335bce3448c3 45 #define M_1_CCW_MIN 65
ppr2013G2 1:335bce3448c3 46 #define M_2_CCW_MIN 193
ppr2013G2 1:335bce3448c3 47
ppr2013G2 1:335bce3448c3 48 #define M_1_CW_MAX 1
ppr2013G2 1:335bce3448c3 49 #define M_2_CW_MAX 128
ppr2013G2 1:335bce3448c3 50 #define M_1_CCW_MAX 127
ppr2013G2 1:335bce3448c3 51 #define M_2_CCW_MAX 255
ppr2013G2 1:335bce3448c3 52
ppr2013G2 1:335bce3448c3 53
ppr2013G2 1:335bce3448c3 54 #define M_1_STOP 64
ppr2013G2 1:335bce3448c3 55 #define M_2_STOP 192
ppr2013G2 1:335bce3448c3 56
ppr2013G2 1:335bce3448c3 57
ppr2013G2 1:335bce3448c3 58
ppr2013G2 1:335bce3448c3 59
BartJanssens 0:99a111b75cb4 60 u8 ps3_data[48];
ppr2013G2 1:335bce3448c3 61 Serial sabertooth1(p13,p14);
ppr2013G2 1:335bce3448c3 62 Serial sabertooth2(p28,p27);
BartJanssens 0:99a111b75cb4 63
ppr2013G2 1:335bce3448c3 64 Serial pro(USBTX, USBRX);
ppr2013G2 1:335bce3448c3 65 //TextLCD lcd(p24,p26,p27,p28,p29,p30);//for orange board
ppr2013G2 1:335bce3448c3 66 C12832_LCD lcd;
ppr2013G2 1:335bce3448c3 67
ppr2013G2 1:335bce3448c3 68 PwmOut air1(p21);
ppr2013G2 1:335bce3448c3 69 PwmOut air2(p22);
ppr2013G2 1:335bce3448c3 70 PwmOut rgb(p23);
ppr2013G2 1:335bce3448c3 71
ppr2013G2 1:335bce3448c3 72 AnalogIn pot1(p19);
ppr2013G2 1:335bce3448c3 73
ppr2013G2 1:335bce3448c3 74 int map(int val, int b_min, int b_max, int a_min, int a_max)
ppr2013G2 1:335bce3448c3 75 {
ppr2013G2 1:335bce3448c3 76 return (val - b_max) * (a_min - a_max) / (b_min - b_max) + a_max;
ppr2013G2 1:335bce3448c3 77 }
BartJanssens 0:99a111b75cb4 78
BartJanssens 0:99a111b75cb4 79 Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
BartJanssens 0:99a111b75cb4 80 {
ppr2013G2 1:335bce3448c3 81 printf("Creating new sixaxis \r\n");
ppr2013G2 1:335bce3448c3 82 _device = device;
ppr2013G2 1:335bce3448c3 83 _configuration = configuration;
ppr2013G2 1:335bce3448c3 84 _interfaceNumber = interfaceNumber;
ppr2013G2 1:335bce3448c3 85 printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
ppr2013G2 1:335bce3448c3 86 int result;
ppr2013G2 1:335bce3448c3 87 int err;
ppr2013G2 1:335bce3448c3 88
ppr2013G2 1:335bce3448c3 89 _count = 1;
ppr2013G2 1:335bce3448c3 90
ppr2013G2 1:335bce3448c3 91 u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
ppr2013G2 1:335bce3448c3 92 0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
ppr2013G2 1:335bce3448c3 93 0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
ppr2013G2 1:335bce3448c3 94 0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
ppr2013G2 1:335bce3448c3 95 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
ppr2013G2 1:335bce3448c3 96 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
ppr2013G2 1:335bce3448c3 97 };
ppr2013G2 1:335bce3448c3 98 memcpy(ledrumble,abuffer,48);
ppr2013G2 1:335bce3448c3 99
ppr2013G2 1:335bce3448c3 100 EndpointDescriptor* ep;
ppr2013G2 1:335bce3448c3 101
ppr2013G2 1:335bce3448c3 102 u8 buffer[255];
ppr2013G2 1:335bce3448c3 103 err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
ppr2013G2 1:335bce3448c3 104 if (err < 0)
ppr2013G2 1:335bce3448c3 105 printf("Failed to get descriptor\r\n");
ppr2013G2 1:335bce3448c3 106
BartJanssens 0:99a111b75cb4 107
BartJanssens 0:99a111b75cb4 108 int len = buffer[2] | (buffer[3] << 8);
BartJanssens 0:99a111b75cb4 109 u8* d = buffer;
BartJanssens 0:99a111b75cb4 110 u8* end = d + len;
ppr2013G2 1:335bce3448c3 111 while (d < end) {
ppr2013G2 1:335bce3448c3 112 if (d[1] == DESCRIPTOR_TYPE_INTERFACE) {
BartJanssens 0:99a111b75cb4 113 InterfaceDescriptor* id = (InterfaceDescriptor*)d;
ppr2013G2 1:335bce3448c3 114 if (id->bInterfaceNumber == _interfaceNumber) {
ppr2013G2 1:335bce3448c3 115 d += d[0];
ppr2013G2 1:335bce3448c3 116 while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE) {
BartJanssens 0:99a111b75cb4 117 if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
BartJanssens 0:99a111b75cb4 118 ep = (EndpointDescriptor*)d;
ppr2013G2 1:335bce3448c3 119
ppr2013G2 1:335bce3448c3 120 if (ep->bEndpointAddress == 0x02) {
ppr2013G2 1:335bce3448c3 121 printf("PS3 input endpoint (0x02) found\r\n");
ppr2013G2 1:335bce3448c3 122 input_ep = 0x02;
ppr2013G2 1:335bce3448c3 123
ppr2013G2 1:335bce3448c3 124 }
ppr2013G2 1:335bce3448c3 125 if (ep->bEndpointAddress == 0x81) {
ppr2013G2 1:335bce3448c3 126 printf("PS3 output endpoint (0x81) found\r\n");
ppr2013G2 1:335bce3448c3 127 output_ep = 0x81;
ppr2013G2 1:335bce3448c3 128 //AddAutoEvent(device,id,(EndpointDescriptor*)d);
ppr2013G2 1:335bce3448c3 129 }
BartJanssens 0:99a111b75cb4 130 d += d[0];
BartJanssens 0:99a111b75cb4 131 }
BartJanssens 0:99a111b75cb4 132 }
BartJanssens 0:99a111b75cb4 133 }
BartJanssens 0:99a111b75cb4 134 d += d[0];
BartJanssens 0:99a111b75cb4 135 }
ppr2013G2 1:335bce3448c3 136
ppr2013G2 1:335bce3448c3 137 }
ppr2013G2 1:335bce3448c3 138
ppr2013G2 1:335bce3448c3 139 int Ps3USB::Enable()
ppr2013G2 1:335bce3448c3 140 {
ppr2013G2 1:335bce3448c3 141 int err;
ppr2013G2 1:335bce3448c3 142
ppr2013G2 1:335bce3448c3 143 u8 enable[4] = {0x42,0x0c,0x00,0x00};
ppr2013G2 1:335bce3448c3 144
ppr2013G2 1:335bce3448c3 145
ppr2013G2 1:335bce3448c3 146 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
ppr2013G2 1:335bce3448c3 147 //printf("set report result = %d\r\n", err);
ppr2013G2 1:335bce3448c3 148 _count ++;
ppr2013G2 1:335bce3448c3 149 if (_count == 25) _count = 1;
ppr2013G2 1:335bce3448c3 150
ppr2013G2 1:335bce3448c3 151 err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
ppr2013G2 1:335bce3448c3 152 wait_ms(4);
ppr2013G2 1:335bce3448c3 153
ppr2013G2 1:335bce3448c3 154 return 0;
ppr2013G2 1:335bce3448c3 155
ppr2013G2 1:335bce3448c3 156 }
ppr2013G2 1:335bce3448c3 157
ppr2013G2 1:335bce3448c3 158
ppr2013G2 1:335bce3448c3 159
ppr2013G2 1:335bce3448c3 160
ppr2013G2 1:335bce3448c3 161 int Ps3USB::SetPair(u8* bdAddr)
ppr2013G2 1:335bce3448c3 162 {
ppr2013G2 1:335bce3448c3 163 int err;
ppr2013G2 1:335bce3448c3 164
ppr2013G2 1:335bce3448c3 165 u8 buf[8];
ppr2013G2 1:335bce3448c3 166 u8 buf2[6];
ppr2013G2 1:335bce3448c3 167
ppr2013G2 1:335bce3448c3 168 memcpy(buf2,bdAddr,6);
ppr2013G2 1:335bce3448c3 169
ppr2013G2 1:335bce3448c3 170 buf[0] = 0x01;
ppr2013G2 1:335bce3448c3 171 buf[1] = 0x00;
ppr2013G2 1:335bce3448c3 172 buf[2] = buf2[0];
ppr2013G2 1:335bce3448c3 173 buf[3] = buf2[1];
ppr2013G2 1:335bce3448c3 174 buf[4] = buf2[2];
ppr2013G2 1:335bce3448c3 175 buf[5] = buf2[3];
ppr2013G2 1:335bce3448c3 176 buf[6] = buf2[4];
ppr2013G2 1:335bce3448c3 177 buf[7] = buf2[5];
BartJanssens 0:99a111b75cb4 178
ppr2013G2 1:335bce3448c3 179 //set Mac address
ppr2013G2 1:335bce3448c3 180 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
ppr2013G2 1:335bce3448c3 181 wait_ms(4);
ppr2013G2 1:335bce3448c3 182 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);
ppr2013G2 1:335bce3448c3 183
ppr2013G2 1:335bce3448c3 184 return 0;
ppr2013G2 1:335bce3448c3 185 }
ppr2013G2 1:335bce3448c3 186
ppr2013G2 1:335bce3448c3 187 int Ps3USB::ShowPair()
ppr2013G2 1:335bce3448c3 188 {
ppr2013G2 1:335bce3448c3 189 int err;
ppr2013G2 1:335bce3448c3 190
ppr2013G2 1:335bce3448c3 191 u8 buf[8];
ppr2013G2 1:335bce3448c3 192 //get Mac address
ppr2013G2 1:335bce3448c3 193 err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
ppr2013G2 1:335bce3448c3 194 wait_ms(4);
ppr2013G2 1:335bce3448c3 195 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);
ppr2013G2 1:335bce3448c3 196
ppr2013G2 1:335bce3448c3 197
ppr2013G2 1:335bce3448c3 198 return 0;
ppr2013G2 1:335bce3448c3 199 }
ppr2013G2 1:335bce3448c3 200
ppr2013G2 1:335bce3448c3 201 int Ps3USB::Led(int i)
ppr2013G2 1:335bce3448c3 202 {
ppr2013G2 1:335bce3448c3 203 int err;
ppr2013G2 1:335bce3448c3 204 u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
ppr2013G2 1:335bce3448c3 205 u8 buf[48];
ppr2013G2 1:335bce3448c3 206
ppr2013G2 1:335bce3448c3 207 if (i < 7) ledrumble[9] = ledpattern[i];
ppr2013G2 1:335bce3448c3 208 memcpy(buf, ledrumble, 48);
ppr2013G2 1:335bce3448c3 209
ppr2013G2 1:335bce3448c3 210 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
ppr2013G2 1:335bce3448c3 211 wait_ms(4);
ppr2013G2 1:335bce3448c3 212
ppr2013G2 1:335bce3448c3 213 return 0;
ppr2013G2 1:335bce3448c3 214 }
ppr2013G2 1:335bce3448c3 215
ppr2013G2 1:335bce3448c3 216 // left and right: duration and power, both from 0 to 255
ppr2013G2 1:335bce3448c3 217 int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
ppr2013G2 1:335bce3448c3 218 {
ppr2013G2 1:335bce3448c3 219 int err;
ppr2013G2 1:335bce3448c3 220 u8 buf[48];
BartJanssens 0:99a111b75cb4 221
ppr2013G2 1:335bce3448c3 222 memcpy(buf, ledrumble, 48);
ppr2013G2 1:335bce3448c3 223 buf[1] = duration_right;
ppr2013G2 1:335bce3448c3 224 buf[2] = power_right;
ppr2013G2 1:335bce3448c3 225 buf[3] = duration_left;
ppr2013G2 1:335bce3448c3 226 buf[4] = power_left;
ppr2013G2 1:335bce3448c3 227
ppr2013G2 1:335bce3448c3 228 err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
ppr2013G2 1:335bce3448c3 229 wait_ms(4);
ppr2013G2 1:335bce3448c3 230
ppr2013G2 1:335bce3448c3 231 return 0;
ppr2013G2 1:335bce3448c3 232 }
ppr2013G2 1:335bce3448c3 233
ppr2013G2 1:335bce3448c3 234
ppr2013G2 1:335bce3448c3 235 void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
ppr2013G2 1:335bce3448c3 236 {
ppr2013G2 1:335bce3448c3 237 Ps3USB* t = (Ps3USB*)userData;
ppr2013G2 1:335bce3448c3 238
ppr2013G2 1:335bce3448c3 239 t->_count ++;
ppr2013G2 1:335bce3448c3 240 if (t->_count == 25) t->_count = 1;
ppr2013G2 1:335bce3448c3 241
ppr2013G2 1:335bce3448c3 242 ParsePs3Result(data, sizeof(ps3report),t->_count);
ppr2013G2 1:335bce3448c3 243 USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
ppr2013G2 1:335bce3448c3 244 wait_ms(4);
ppr2013G2 1:335bce3448c3 245
ppr2013G2 1:335bce3448c3 246 }
ppr2013G2 1:335bce3448c3 247
ppr2013G2 1:335bce3448c3 248 int ParsePs3Result(const u8* data,int len,int count)
ppr2013G2 1:335bce3448c3 249 {
ppr2013G2 1:335bce3448c3 250 ps3report* _ps3report = (ps3report*)data;
ppr2013G2 1:335bce3448c3 251
ppr2013G2 1:335bce3448c3 252 //int pot=pot2.read();
ppr2013G2 1:335bce3448c3 253 float pot =pot1.read()*100.0;
ppr2013G2 1:335bce3448c3 254 int pot_int=(int)pot+1;
BartJanssens 0:99a111b75cb4 255
ppr2013G2 1:335bce3448c3 256 sabertooth1.baud(19200);
ppr2013G2 1:335bce3448c3 257 sabertooth2.baud(19200);
BartJanssens 0:99a111b75cb4 258
ppr2013G2 1:335bce3448c3 259 u8 P_UP =_ps3report->PressureUp;
ppr2013G2 1:335bce3448c3 260 u8 P_DOWN =_ps3report->PressureDown;
ppr2013G2 1:335bce3448c3 261 u8 P_LEFT =_ps3report->PressureLeft;
ppr2013G2 1:335bce3448c3 262 u8 P_RIGHT=_ps3report->PressureRight;
ppr2013G2 1:335bce3448c3 263
ppr2013G2 1:335bce3448c3 264 u8 L2=_ps3report->PressureL2;
ppr2013G2 1:335bce3448c3 265 u8 R2=_ps3report->PressureR2;
BartJanssens 0:99a111b75cb4 266
ppr2013G2 1:335bce3448c3 267 u8 L1=_ps3report->PressureL1;
ppr2013G2 1:335bce3448c3 268 u8 R1=_ps3report->PressureR1;
BartJanssens 0:99a111b75cb4 269
ppr2013G2 1:335bce3448c3 270 u8 Cir=_ps3report->PressureCircle;
ppr2013G2 1:335bce3448c3 271
ppr2013G2 1:335bce3448c3 272 // u8 L_S_X=_ps3report->LeftStickX;
ppr2013G2 1:335bce3448c3 273 // u8 L_S_Y=_ps3report->LeftStickY;
ppr2013G2 1:335bce3448c3 274
ppr2013G2 1:335bce3448c3 275 int sb1_1=0;
ppr2013G2 1:335bce3448c3 276 int sb1_2=0;
ppr2013G2 1:335bce3448c3 277 int sb2_1=0;
ppr2013G2 1:335bce3448c3 278 int sb2_2=0;
ppr2013G2 1:335bce3448c3 279
ppr2013G2 1:335bce3448c3 280 u8 M_1_CW_BIG = M_1_CW_MAX;
ppr2013G2 1:335bce3448c3 281 u8 M_2_CW_BIG = M_2_CW_MAX;
ppr2013G2 1:335bce3448c3 282 u8 M_1_CCW_BIG = M_1_CCW_MAX;
ppr2013G2 1:335bce3448c3 283 u8 M_2_CCW_BIG = M_2_CCW_MAX;
ppr2013G2 1:335bce3448c3 284
ppr2013G2 1:335bce3448c3 285 bool up_lock=false;
ppr2013G2 1:335bce3448c3 286 bool down_lock=false;
ppr2013G2 1:335bce3448c3 287 bool left_lock=false;
ppr2013G2 1:335bce3448c3 288 bool right_lock=false;
ppr2013G2 1:335bce3448c3 289 bool clockwise_lock=false;
ppr2013G2 1:335bce3448c3 290 bool c_clockwise_lock=false;
ppr2013G2 1:335bce3448c3 291 bool left_stick_lock=false;
ppr2013G2 1:335bce3448c3 292
ppr2013G2 1:335bce3448c3 293 M_1_CW_BIG=map(pot_int,0,100,62,1);
ppr2013G2 1:335bce3448c3 294 M_2_CW_BIG=map(pot_int,0,100,190,128);
ppr2013G2 1:335bce3448c3 295 M_1_CCW_BIG=map(pot_int,0,100,66,127);
ppr2013G2 1:335bce3448c3 296 M_2_CCW_BIG=map(pot_int,0,100,194,255);
ppr2013G2 1:335bce3448c3 297
ppr2013G2 1:335bce3448c3 298 // M_1_CW_MAX *= pot;
ppr2013G2 1:335bce3448c3 299 // M_2_CW_MAX *=pot;
ppr2013G2 1:335bce3448c3 300 // M_1_CCW_MAX *=pot;
ppr2013G2 1:335bce3448c3 301 // M_2_CCW_MAX *=pot;
ppr2013G2 1:335bce3448c3 302
ppr2013G2 1:335bce3448c3 303 if(Cir!=0){
ppr2013G2 1:335bce3448c3 304 air1.write(1.0f);
ppr2013G2 1:335bce3448c3 305 air2.write(0.0f);
ppr2013G2 1:335bce3448c3 306 rgb.write(0.0);
ppr2013G2 1:335bce3448c3 307 }else{
ppr2013G2 1:335bce3448c3 308 air1.write(0.0f);
ppr2013G2 1:335bce3448c3 309 air2.write(0.0f);
ppr2013G2 1:335bce3448c3 310 rgb.write(1.0);
BartJanssens 0:99a111b75cb4 311 }
BartJanssens 0:99a111b75cb4 312
ppr2013G2 1:335bce3448c3 313 if(P_UP==0&&P_DOWN==0&&P_LEFT==0&&P_RIGHT==0) {
ppr2013G2 1:335bce3448c3 314 // up_lock=false;
ppr2013G2 1:335bce3448c3 315 // down_lock=false;
ppr2013G2 1:335bce3448c3 316 // left_lock=false;
ppr2013G2 1:335bce3448c3 317 // right_lock=false;
ppr2013G2 1:335bce3448c3 318 // clockwise_lock=false;
ppr2013G2 1:335bce3448c3 319 // c_clockwise_lock=false;
BartJanssens 0:99a111b75cb4 320
ppr2013G2 1:335bce3448c3 321 sb1_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 322 sb1_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 323 sb2_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 324 sb2_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 325
ppr2013G2 1:335bce3448c3 326 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 327 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 328
ppr2013G2 1:335bce3448c3 329 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 330 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 331
BartJanssens 0:99a111b75cb4 332
BartJanssens 0:99a111b75cb4 333 }
ppr2013G2 1:335bce3448c3 334 if(P_UP!=0 && !down_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
ppr2013G2 1:335bce3448c3 335 // up_lock=true;
ppr2013G2 1:335bce3448c3 336
ppr2013G2 1:335bce3448c3 337 sb1_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 338 sb1_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 339 sb2_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 340 sb2_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 341
ppr2013G2 1:335bce3448c3 342 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 343 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 344
ppr2013G2 1:335bce3448c3 345 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 346 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 347 }
ppr2013G2 1:335bce3448c3 348 if(P_DOWN!=0 && !up_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
ppr2013G2 1:335bce3448c3 349 // down_lock=true;
ppr2013G2 1:335bce3448c3 350
ppr2013G2 1:335bce3448c3 351 sb1_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 352 sb1_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 353 sb2_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 354 sb2_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 355
ppr2013G2 1:335bce3448c3 356 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 357 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 358
ppr2013G2 1:335bce3448c3 359 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 360 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 361 }
ppr2013G2 1:335bce3448c3 362 if(P_LEFT!=0 && !up_lock && !down_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
ppr2013G2 1:335bce3448c3 363 // left_lock=true;
ppr2013G2 1:335bce3448c3 364
ppr2013G2 1:335bce3448c3 365 sb1_1=map(P_LEFT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 366 sb1_2=map(P_LEFT,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 367 sb2_1=map(P_LEFT,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 368 sb2_2=map(P_LEFT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 369
ppr2013G2 1:335bce3448c3 370 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 371 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 372
ppr2013G2 1:335bce3448c3 373 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 374 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 375 }
ppr2013G2 1:335bce3448c3 376 if(P_RIGHT!=0 && !up_lock && !down_lock && !left_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
ppr2013G2 1:335bce3448c3 377 // right_lock=true;
ppr2013G2 1:335bce3448c3 378
ppr2013G2 1:335bce3448c3 379 sb1_1=map(P_RIGHT,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 380 sb1_2=map(P_RIGHT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 381 sb2_1=map(P_RIGHT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 382 sb2_2=map(P_RIGHT,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 383
ppr2013G2 1:335bce3448c3 384 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 385 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 386
ppr2013G2 1:335bce3448c3 387 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 388 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 389 }
ppr2013G2 1:335bce3448c3 390 if(L1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !clockwise_lock && !left_stick_lock) {
ppr2013G2 1:335bce3448c3 391 // c_clockwise_lock=true;
ppr2013G2 1:335bce3448c3 392
ppr2013G2 1:335bce3448c3 393 sb1_1=map(L1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 394 sb1_2=map(L1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 395 sb2_1=map(L1,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 396 sb2_2=map(L1,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 397
ppr2013G2 1:335bce3448c3 398 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 399 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 400
ppr2013G2 1:335bce3448c3 401 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 402 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 403 }
ppr2013G2 1:335bce3448c3 404 if(R1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !c_clockwise_lock &&!left_stick_lock) {
ppr2013G2 1:335bce3448c3 405 // clockwise_lock=true;
ppr2013G2 1:335bce3448c3 406
ppr2013G2 1:335bce3448c3 407 sb1_1=map(R1,0,255,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 408 sb1_2=map(R1,0,255,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 409 sb2_1=map(R1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 410 sb2_2=map(R1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 411
ppr2013G2 1:335bce3448c3 412 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 413 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 414
ppr2013G2 1:335bce3448c3 415 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 416 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 417 }
ppr2013G2 1:335bce3448c3 418 if(L2==255) {
ppr2013G2 1:335bce3448c3 419 sb1_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 420 sb1_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 421
ppr2013G2 1:335bce3448c3 422 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 423 sabertooth1.putc(sb1_2);
BartJanssens 0:99a111b75cb4 424 }
ppr2013G2 1:335bce3448c3 425 if(R2==255) {
ppr2013G2 1:335bce3448c3 426 sb2_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 427 sb2_2=M_2_STOP;
BartJanssens 0:99a111b75cb4 428
ppr2013G2 1:335bce3448c3 429 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 430 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 431 }
ppr2013G2 1:335bce3448c3 432 if(P_UP!=0 && P_LEFT!=0) {
ppr2013G2 1:335bce3448c3 433 sb1_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 434 sb1_2=map(P_UP+P_LEFT,0,510,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 435 sb2_1=map(P_UP+P_LEFT,0,510,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 436 sb2_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 437
ppr2013G2 1:335bce3448c3 438 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 439 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 440
ppr2013G2 1:335bce3448c3 441 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 442 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 443 }
ppr2013G2 1:335bce3448c3 444 if(P_DOWN!=0 && P_LEFT!=0) {
ppr2013G2 1:335bce3448c3 445 sb1_1=map(P_DOWN+P_LEFT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 446 sb1_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 447 sb2_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 448 sb2_2=map(P_DOWN+P_LEFT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 449
ppr2013G2 1:335bce3448c3 450 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 451 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 452
ppr2013G2 1:335bce3448c3 453 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 454 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 455 }
ppr2013G2 1:335bce3448c3 456 if(P_UP!=0 && P_RIGHT!=0) {
ppr2013G2 1:335bce3448c3 457 sb1_1=map(P_UP+P_RIGHT,0,510,M_1_CW_MIN,M_1_CW_BIG);
ppr2013G2 1:335bce3448c3 458 sb1_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 459 sb2_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 460 sb2_2=map(P_UP+P_RIGHT,0,510,M_2_CW_MIN,M_2_CW_BIG);
ppr2013G2 1:335bce3448c3 461
ppr2013G2 1:335bce3448c3 462 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 463 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 464
ppr2013G2 1:335bce3448c3 465 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 466 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 467 }
ppr2013G2 1:335bce3448c3 468 if(P_DOWN!=0 && P_RIGHT!=0) {
ppr2013G2 1:335bce3448c3 469 sb1_1=M_1_STOP;
ppr2013G2 1:335bce3448c3 470 sb1_2=map(P_DOWN+P_RIGHT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);
ppr2013G2 1:335bce3448c3 471 sb2_1=map(P_DOWN+P_RIGHT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
ppr2013G2 1:335bce3448c3 472 sb2_2=M_2_STOP;
ppr2013G2 1:335bce3448c3 473
ppr2013G2 1:335bce3448c3 474 sabertooth1.putc(sb1_1);
ppr2013G2 1:335bce3448c3 475 sabertooth1.putc(sb1_2);
ppr2013G2 1:335bce3448c3 476
ppr2013G2 1:335bce3448c3 477 sabertooth2.putc(sb2_1);
ppr2013G2 1:335bce3448c3 478 sabertooth2.putc(sb2_2);
ppr2013G2 1:335bce3448c3 479 }
ppr2013G2 1:335bce3448c3 480 // if((L_S_X<110 || L_S_X>140 || L_S_Y<120 || L_S_Y>140) && !up_lock && !down_lock && !left_lock && !right_lock && !c_clockwise_lock && !clockwise_lock){
ppr2013G2 1:335bce3448c3 481 // left_stick_lock=true;
ppr2013G2 1:335bce3448c3 482 //
ppr2013G2 1:335bce3448c3 483 // sb1_1=map(L_S_X+L_S_Y)
ppr2013G2 1:335bce3448c3 484 // }
ppr2013G2 1:335bce3448c3 485
ppr2013G2 1:335bce3448c3 486 lcd.locate(1,1);
ppr2013G2 1:335bce3448c3 487 // lcd.printf("1_1=%u 1_2=%u 2_1=%u \n 2_2=%u \n L2=%u R2=%u L1=%u R1=%u",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1);
ppr2013G2 1:335bce3448c3 488 lcd.printf("p=%f,p_i=%d,Cir=%u",pot,pot_int,Cir);
ppr2013G2 1:335bce3448c3 489 if(pro.writeable()>0) {
ppr2013G2 1:335bce3448c3 490 pro.printf("%u,%u,%u,%u,%u,%u,%u,%u,%d\n",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1,pot_int);
ppr2013G2 1:335bce3448c3 491 }
ppr2013G2 1:335bce3448c3 492 // sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 0, 127));
ppr2013G2 1:335bce3448c3 493 // sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 193,254));
ppr2013G2 1:335bce3448c3 494 // 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");
ppr2013G2 1:335bce3448c3 495 // printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
ppr2013G2 1:335bce3448c3 496 // _ps3report->LeftStickX,
ppr2013G2 1:335bce3448c3 497 // _ps3report->LeftStickY,
ppr2013G2 1:335bce3448c3 498 // _ps3report->RightStickX,
ppr2013G2 1:335bce3448c3 499 // _ps3report->RightStickY,
ppr2013G2 1:335bce3448c3 500 // _ps3report->PressureUp,
ppr2013G2 1:335bce3448c3 501 // _ps3report->PressureRight,
ppr2013G2 1:335bce3448c3 502 // _ps3report->PressureDown,
ppr2013G2 1:335bce3448c3 503 // _ps3report->PressureLeft,
ppr2013G2 1:335bce3448c3 504 // _ps3report->PressureL2,
ppr2013G2 1:335bce3448c3 505 // _ps3report->PressureR2,
ppr2013G2 1:335bce3448c3 506 // _ps3report->PressureL1,
ppr2013G2 1:335bce3448c3 507 // _ps3report->PressureR1,
ppr2013G2 1:335bce3448c3 508 // _ps3report->PressureTriangle,
ppr2013G2 1:335bce3448c3 509 // _ps3report->PressureCircle,
ppr2013G2 1:335bce3448c3 510 // _ps3report->PressureCross,
ppr2013G2 1:335bce3448c3 511 // _ps3report->PressureSquare,
ppr2013G2 1:335bce3448c3 512 // (_ps3report->AccelX),
ppr2013G2 1:335bce3448c3 513 // (_ps3report->AccelY),
ppr2013G2 1:335bce3448c3 514 // (_ps3report->AccelZ),
ppr2013G2 1:335bce3448c3 515 // (_ps3report->GyroZ));
ppr2013G2 1:335bce3448c3 516 //printfBytes("data",data,len);
ppr2013G2 1:335bce3448c3 517 }
ppr2013G2 1:335bce3448c3 518
ppr2013G2 1:335bce3448c3 519