Gong Chen
Fork BlueUSB lib for NHK robot contest 2014 TEU JAPAN
Dependencies: C12832_lcd FatFileSystem TextLCD mbed
Fork of
PS3_BlueUSB
by 
Bart Janssens
Diff: Ps3USB.cpp
- Revision:
- 1:335bce3448c3
- Parent:
- 0:99a111b75cb4
- Child:
- 2:2c4daaf8b15c
--- a/Ps3USB.cpp Tue Apr 26 16:09:17 2011 +0000
+++ b/Ps3USB.cpp Sun Jan 12 07:08:34 2014 +0000
@@ -23,6 +23,9 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+//#include <TextLCD.h>//for orange borad
+#include "C12832_lcd.h"
+
#include "USBHost.h"
#include "hci.h"
@@ -37,207 +40,480 @@
#define PS3EVT AUTOEVT(CLASS_HID,0,0)
#define byteswap(x) ((x >> 8) | (x << 8))
+#define M_1_CW_MIN 63
+#define M_2_CW_MIN 191
+#define M_1_CCW_MIN 65
+#define M_2_CCW_MIN 193
+
+#define M_1_CW_MAX 1
+#define M_2_CW_MAX 128
+#define M_1_CCW_MAX 127
+#define M_2_CCW_MAX 255
+
+
+#define M_1_STOP 64
+#define M_2_STOP 192
+
+
+
+
u8 ps3_data[48];
+Serial sabertooth1(p13,p14);
+Serial sabertooth2(p28,p27);
+Serial pro(USBTX, USBRX);
+//TextLCD lcd(p24,p26,p27,p28,p29,p30);//for orange board
+C12832_LCD lcd;
+
+PwmOut air1(p21);
+PwmOut air2(p22);
+PwmOut rgb(p23);
+
+AnalogIn pot1(p19);
+
+int map(int val, int b_min, int b_max, int a_min, int a_max)
+{
+ return (val - b_max) * (a_min - a_max) / (b_min - b_max) + a_max;
+}
Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
{
- printf("Creating new sixaxis \r\n");
- _device = device;
- _configuration = configuration;
- _interfaceNumber = interfaceNumber;
- printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
- int result;
- int err;
-
- _count = 1;
-
- u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
- 0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
- 0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
- 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
- memcpy(ledrumble,abuffer,48);
-
- EndpointDescriptor* ep;
-
- u8 buffer[255];
- err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
- if (err < 0)
- printf("Failed to get descriptor\r\n");
-
+ printf("Creating new sixaxis \r\n");
+ _device = device;
+ _configuration = configuration;
+ _interfaceNumber = interfaceNumber;
+ printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
+ int result;
+ int err;
+
+ _count = 1;
+
+ u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
+ 0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
+ 0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+ };
+ memcpy(ledrumble,abuffer,48);
+
+ EndpointDescriptor* ep;
+
+ u8 buffer[255];
+ err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
+ if (err < 0)
+ printf("Failed to get descriptor\r\n");
+
int len = buffer[2] | (buffer[3] << 8);
u8* d = buffer;
u8* end = d + len;
- while (d < end)
- {
- if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
- {
+ while (d < end) {
+ if (d[1] == DESCRIPTOR_TYPE_INTERFACE) {
InterfaceDescriptor* id = (InterfaceDescriptor*)d;
- if (id->bInterfaceNumber == _interfaceNumber)
- {
- d += d[0];
- while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
- {
+ if (id->bInterfaceNumber == _interfaceNumber) {
+ d += d[0];
+ while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE) {
if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
ep = (EndpointDescriptor*)d;
-
- if (ep->bEndpointAddress == 0x02) {
- printf("PS3 input endpoint (0x02) found\r\n");
- input_ep = 0x02;
-
- }
- if (ep->bEndpointAddress == 0x81) {
- printf("PS3 output endpoint (0x81) found\r\n");
- output_ep = 0x81;
- //AddAutoEvent(device,id,(EndpointDescriptor*)d);
- }
+
+ if (ep->bEndpointAddress == 0x02) {
+ printf("PS3 input endpoint (0x02) found\r\n");
+ input_ep = 0x02;
+
+ }
+ if (ep->bEndpointAddress == 0x81) {
+ printf("PS3 output endpoint (0x81) found\r\n");
+ output_ep = 0x81;
+ //AddAutoEvent(device,id,(EndpointDescriptor*)d);
+ }
d += d[0];
}
}
}
d += d[0];
}
-
- }
-
- int Ps3USB::Enable()
- {
- int err;
-
- u8 enable[4] = {0x42,0x0c,0x00,0x00};
+
+}
+
+int Ps3USB::Enable()
+{
+ int err;
+
+ u8 enable[4] = {0x42,0x0c,0x00,0x00};
+
+
+ err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
+ //printf("set report result = %d\r\n", err);
+ _count ++;
+ if (_count == 25) _count = 1;
+
+ err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
+ wait_ms(4);
+
+ return 0;
+
+}
+
+
+
+
+int Ps3USB::SetPair(u8* bdAddr)
+{
+ int err;
+
+ u8 buf[8];
+ u8 buf2[6];
+
+ memcpy(buf2,bdAddr,6);
+
+ buf[0] = 0x01;
+ buf[1] = 0x00;
+ buf[2] = buf2[0];
+ buf[3] = buf2[1];
+ buf[4] = buf2[2];
+ buf[5] = buf2[3];
+ buf[6] = buf2[4];
+ buf[7] = buf2[5];
-
- err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
- //printf("set report result = %d\r\n", err);
- _count ++;
- if (_count == 25) _count = 1;
-
- err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
- wait_ms(4);
-
- return 0;
-
- }
-
-
-
-
- int Ps3USB::SetPair(u8* bdAddr)
- {
- int err;
-
- u8 buf[8];
- u8 buf2[6];
-
- memcpy(buf2,bdAddr,6);
-
- buf[0] = 0x01;
- buf[1] = 0x00;
- buf[2] = buf2[0];
- buf[3] = buf2[1];
- buf[4] = buf2[2];
- buf[5] = buf2[3];
- buf[6] = buf2[4];
- buf[7] = buf2[5];
-
- //set Mac address
- err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
- wait_ms(4);
- 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);
+ //set Mac address
+ err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
+ wait_ms(4);
+ 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);
+
+ return 0;
+}
+
+int Ps3USB::ShowPair()
+{
+ int err;
+
+ u8 buf[8];
+ //get Mac address
+ err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
+ wait_ms(4);
+ 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);
+
+
+ return 0;
+}
+
+int Ps3USB::Led(int i)
+{
+ int err;
+ u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
+ u8 buf[48];
+
+ if (i < 7) ledrumble[9] = ledpattern[i];
+ memcpy(buf, ledrumble, 48);
+
+ err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
+ wait_ms(4);
+
+ return 0;
+}
+
+// left and right: duration and power, both from 0 to 255
+int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
+{
+ int err;
+ u8 buf[48];
- return 0;
- }
+ memcpy(buf, ledrumble, 48);
+ buf[1] = duration_right;
+ buf[2] = power_right;
+ buf[3] = duration_left;
+ buf[4] = power_left;
+
+ err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
+ wait_ms(4);
+
+ return 0;
+}
+
+
+void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
+{
+ Ps3USB* t = (Ps3USB*)userData;
+
+ t->_count ++;
+ if (t->_count == 25) t->_count = 1;
+
+ ParsePs3Result(data, sizeof(ps3report),t->_count);
+ USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
+ wait_ms(4);
+
+}
+
+int ParsePs3Result(const u8* data,int len,int count)
+{
+ ps3report* _ps3report = (ps3report*)data;
+
+ //int pot=pot2.read();
+ float pot =pot1.read()*100.0;
+ int pot_int=(int)pot+1;
- int Ps3USB::ShowPair()
- {
- int err;
-
- u8 buf[8];
- //get Mac address
- err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
- wait_ms(4);
- 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);
+ sabertooth1.baud(19200);
+ sabertooth2.baud(19200);
-
- return 0;
- }
-
- int Ps3USB::Led(int i)
- {
- int err;
- u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
- u8 buf[48];
+ u8 P_UP =_ps3report->PressureUp;
+ u8 P_DOWN =_ps3report->PressureDown;
+ u8 P_LEFT =_ps3report->PressureLeft;
+ u8 P_RIGHT=_ps3report->PressureRight;
+
+ u8 L2=_ps3report->PressureL2;
+ u8 R2=_ps3report->PressureR2;
- if (i < 7) ledrumble[9] = ledpattern[i];
- memcpy(buf, ledrumble, 48);
-
- err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
- wait_ms(4);
-
- return 0;
- }
+ u8 L1=_ps3report->PressureL1;
+ u8 R1=_ps3report->PressureR1;
- // left and right: duration and power, both from 0 to 255
- int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
- {
- int err;
- u8 buf[48];
-
- memcpy(buf, ledrumble, 48);
- buf[1] = duration_right;
- buf[2] = power_right;
- buf[3] = duration_left;
- buf[4] = power_left;
-
- err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
- wait_ms(4);
-
- return 0;
+ u8 Cir=_ps3report->PressureCircle;
+
+// u8 L_S_X=_ps3report->LeftStickX;
+// u8 L_S_Y=_ps3report->LeftStickY;
+
+ int sb1_1=0;
+ int sb1_2=0;
+ int sb2_1=0;
+ int sb2_2=0;
+
+ u8 M_1_CW_BIG = M_1_CW_MAX;
+ u8 M_2_CW_BIG = M_2_CW_MAX;
+ u8 M_1_CCW_BIG = M_1_CCW_MAX;
+ u8 M_2_CCW_BIG = M_2_CCW_MAX;
+
+ bool up_lock=false;
+ bool down_lock=false;
+ bool left_lock=false;
+ bool right_lock=false;
+ bool clockwise_lock=false;
+ bool c_clockwise_lock=false;
+ bool left_stick_lock=false;
+
+ M_1_CW_BIG=map(pot_int,0,100,62,1);
+ M_2_CW_BIG=map(pot_int,0,100,190,128);
+ M_1_CCW_BIG=map(pot_int,0,100,66,127);
+ M_2_CCW_BIG=map(pot_int,0,100,194,255);
+
+// M_1_CW_MAX *= pot;
+// M_2_CW_MAX *=pot;
+// M_1_CCW_MAX *=pot;
+// M_2_CCW_MAX *=pot;
+
+ if(Cir!=0){
+ air1.write(1.0f);
+ air2.write(0.0f);
+ rgb.write(0.0);
+ }else{
+ air1.write(0.0f);
+ air2.write(0.0f);
+ rgb.write(1.0);
}
-
- void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
- {
- Ps3USB* t = (Ps3USB*)userData;
+ if(P_UP==0&&P_DOWN==0&&P_LEFT==0&&P_RIGHT==0) {
+// up_lock=false;
+// down_lock=false;
+// left_lock=false;
+// right_lock=false;
+// clockwise_lock=false;
+// c_clockwise_lock=false;
- t->_count ++;
- if (t->_count == 25) t->_count = 1;
-
- ParsePs3Result(data, sizeof(ps3report),t->_count);
- USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
- wait_ms(4);
+ sb1_1=M_1_STOP;
+ sb1_2=M_2_STOP;
+ sb2_1=M_1_STOP;
+ sb2_2=M_2_STOP;
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+
}
-
- int ParsePs3Result(const u8* data,int len,int count)
- {
- ps3report* _ps3report = (ps3report*)data;
- 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");
- printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
- _ps3report->LeftStickX,
- _ps3report->LeftStickY,
- _ps3report->RightStickX,
- _ps3report->RightStickY,
- _ps3report->PressureUp,
- _ps3report->PressureRight,
- _ps3report->PressureDown,
- _ps3report->PressureLeft,
- _ps3report->PressureL2,
- _ps3report->PressureR2,
- _ps3report->PressureL1,
- _ps3report->PressureR1,
- _ps3report->PressureTriangle,
- _ps3report->PressureCircle,
- _ps3report->PressureCross,
- _ps3report->PressureSquare,
- (_ps3report->AccelX),
- (_ps3report->AccelY),
- (_ps3report->AccelZ),
- (_ps3report->GyroZ));
- //printfBytes("data",data,len);
+ if(P_UP!=0 && !down_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
+// up_lock=true;
+
+ sb1_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb1_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);
+ sb2_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb2_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_DOWN!=0 && !up_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
+// down_lock=true;
+
+ sb1_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb1_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+ sb2_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb2_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_LEFT!=0 && !up_lock && !down_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
+// left_lock=true;
+
+ sb1_1=map(P_LEFT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb1_2=map(P_LEFT,0,255,M_2_CW_MIN,M_2_CW_BIG);
+ sb2_1=map(P_LEFT,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb2_2=map(P_LEFT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_RIGHT!=0 && !up_lock && !down_lock && !left_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
+// right_lock=true;
+
+ sb1_1=map(P_RIGHT,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb1_2=map(P_RIGHT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+ sb2_1=map(P_RIGHT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb2_2=map(P_RIGHT,0,255,M_2_CW_MIN,M_2_CW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(L1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !clockwise_lock && !left_stick_lock) {
+// c_clockwise_lock=true;
+
+ sb1_1=map(L1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb1_2=map(L1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+ sb2_1=map(L1,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb2_2=map(L1,0,255,M_2_CW_MIN,M_2_CW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(R1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !c_clockwise_lock &&!left_stick_lock) {
+// clockwise_lock=true;
+
+ sb1_1=map(R1,0,255,M_1_CW_MIN,M_1_CW_BIG);
+ sb1_2=map(R1,0,255,M_2_CW_MIN,M_2_CW_BIG);
+ sb2_1=map(R1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb2_2=map(R1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(L2==255) {
+ sb1_1=M_1_STOP;
+ sb1_2=M_2_STOP;
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
}
+ if(R2==255) {
+ sb2_1=M_1_STOP;
+ sb2_2=M_2_STOP;
-
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_UP!=0 && P_LEFT!=0) {
+ sb1_1=M_1_STOP;
+ sb1_2=map(P_UP+P_LEFT,0,510,M_2_CW_MIN,M_2_CW_BIG);
+ sb2_1=map(P_UP+P_LEFT,0,510,M_1_CW_MIN,M_1_CW_BIG);
+ sb2_2=M_2_STOP;
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_DOWN!=0 && P_LEFT!=0) {
+ sb1_1=map(P_DOWN+P_LEFT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb1_2=M_2_STOP;
+ sb2_1=M_1_STOP;
+ sb2_2=map(P_DOWN+P_LEFT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_UP!=0 && P_RIGHT!=0) {
+ sb1_1=map(P_UP+P_RIGHT,0,510,M_1_CW_MIN,M_1_CW_BIG);
+ sb1_2=M_2_STOP;
+ sb2_1=M_1_STOP;
+ sb2_2=map(P_UP+P_RIGHT,0,510,M_2_CW_MIN,M_2_CW_BIG);
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+ if(P_DOWN!=0 && P_RIGHT!=0) {
+ sb1_1=M_1_STOP;
+ sb1_2=map(P_DOWN+P_RIGHT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);
+ sb2_1=map(P_DOWN+P_RIGHT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
+ sb2_2=M_2_STOP;
+
+ sabertooth1.putc(sb1_1);
+ sabertooth1.putc(sb1_2);
+
+ sabertooth2.putc(sb2_1);
+ sabertooth2.putc(sb2_2);
+ }
+// 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){
+// left_stick_lock=true;
+//
+// sb1_1=map(L_S_X+L_S_Y)
+// }
+
+ lcd.locate(1,1);
+// 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);
+ lcd.printf("p=%f,p_i=%d,Cir=%u",pot,pot_int,Cir);
+ if(pro.writeable()>0) {
+ 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);
+ }
+// sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 0, 127));
+// sabertooth.putc(map(((ps3report*)(data+1))->LeftStickY, 0, 255, 193,254));
+// 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");
+// printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
+// _ps3report->LeftStickX,
+// _ps3report->LeftStickY,
+// _ps3report->RightStickX,
+// _ps3report->RightStickY,
+// _ps3report->PressureUp,
+// _ps3report->PressureRight,
+// _ps3report->PressureDown,
+// _ps3report->PressureLeft,
+// _ps3report->PressureL2,
+// _ps3report->PressureR2,
+// _ps3report->PressureL1,
+// _ps3report->PressureR1,
+// _ps3report->PressureTriangle,
+// _ps3report->PressureCircle,
+// _ps3report->PressureCross,
+// _ps3report->PressureSquare,
+// (_ps3report->AccelX),
+// (_ps3report->AccelY),
+// (_ps3report->AccelZ),
+// (_ps3report->GyroZ));
+ //printfBytes("data",data,len);
+}
+
+