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@2:2c4daaf8b15c, 2014-04-11 (annotated)

Committer:
ppr2013G2
Date:
Fri Apr 11 15:56:07 2014 +0000
Revision:
2:2c4daaf8b15c
Parent:
1:335bce3448c3 
final commit;

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