visus
/
teensy-usbhost
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

Update to joystick and example

main
Mike S 5 years ago
parent
0833431bb1
commit
cf6edd1a25
3 changed files with 198 additions and 62 deletions
Split View Show Diff Stats
  1. +14
    -14
      examples/JoystickBT/JoystickBT.ino
  2. +65
    -0
      examples/JoystickBT/helperPS.ino
  3. +119
    -48
      joystick.cpp

+ 14
- 14
examples/JoystickBT/JoystickBT.ino View File

@@ -59,7 +59,7 @@ void setup()
Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
Serial4.println("\n********\n T4 connected Serial4 *******\n");
*/
Serial1.begin(2000000);
Serial1.begin(1843200);
while (!Serial) ; // wait for Arduino Serial Monitor
//debTraceShow_tt( -2, "", "", "" );
//Serial4.println("\n" __FILE__ " " __DATE__ " " __TIME__);
@@ -184,7 +184,7 @@ void loop()
//}
//Serial.println();
delay(100);
joystick1.joystickDataClear();
}

@@ -205,19 +205,19 @@ void loop()

void displayPS4Data()
{
Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[1], psAxis[2], psAxis[3], psAxis[4]);
Serial.printf("L-Trig: %d, R-Trig: %d, Trig-Button: %d \r\n", psAxis[8], psAxis[9], psAxis[6]);
Serial.printf("Buttons: %d, PS: %d\r\n", psAxis[5], psAxis[7]);
Serial.printf("Arrows: %d\r\n", psAxis[0]);
Serial.printf("Battery level percentage: %2f.0 \r\n", (((float) psAxis[12])/255.0f)*100.0f);
buttons = joystick1.getButtons();
Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);
Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
Serial.printf("Buttons: %x\r\n", buttons);
Serial.printf("Battery Status: %d\n", ((psAxis[30] & (1 << 4)) - 1)*10);
printAngles();
Serial.println();

uint8_t ltv;
uint8_t rtv;

ltv = psAxis[8];
rtv = psAxis[9];
ltv = psAxis[3];
rtv = psAxis[4];

if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
joystick_left_trigger_value = ltv;
@@ -225,13 +225,13 @@ void displayPS4Data()
Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
joystick1.setRumble(ltv, rtv);
}
/* Arrow Buttons (psAxis[0]):
* 0x08 is released,
* 0=N, 1=NE, 2=E, 3=SE, 4=S,
* 5=SW, 6=W, 7=NW)
*/
/*
if (psAxis[5] != buttons_prev) {
uint8_t lr = (psAxis[5] & 1) ? 0xff : 0; //Srq
uint8_t lg = (psAxis[5] & 4) ? 0xff : 0; //Cir
@@ -241,10 +241,10 @@ void displayPS4Data()
Serial.print(lr); Serial.print(", ");
Serial.print(lg); Serial.print(", ");
Serial.println(lb);

joystick1.setLEDs(lr, lg, lb);
buttons_prev =psAxis[5];
buttons_prev =psAxis[5];
}
*/
}

void displayPS3Data()

+ 65
- 0
examples/JoystickBT/helperPS.ino View File

@@ -0,0 +1,65 @@
float pitch, roll;
float gx, gy, gz;
uint16_t xc, yc;
uint8_t isTouch;
float ax, ay, az;
int16_t xc_old, yc_old;

void printAngles(){
//test function calls
float gx, gy, gz;
getAccel(ax, ay, az);
Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
getGyro(gx, gy, gz);
Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);

getAngles(pitch, roll);
Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);

getCoords(xc, yc, isTouch);
}

void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){

//uint8_t finger = 0; //only getting finger 1
uint8_t Id = 0;


// Trackpad touch 1: id, active, x, y
xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
yc = psAxis[38] << 4 | ((psAxis[37] & 0xf0) >> 4),

isTouch = psAxis[35] >> 7;
if(xc != xc_old || yc != yc_old){
Serial.printf("Touch: %d, %d, %d, %d\n", psAxis[33], isTouch, xc, yc);
xc_old = xc;
yc_old = yc;
}
}

void getAccel( float &ax, float &ay, float &az){
int accelx = (int16_t)(psAxis[20]<<8) | psAxis[19];
int accelz = (int16_t)(psAxis[22]<<8) | psAxis[21];
int accely = (int16_t)(psAxis[24]<<8) | psAxis[23];

ax = (float) accelx/8192;
ay = (float) accely/8192;
az = (float) accelz/8192;
}

void getAngles(float &p, float &r){
getAccel( ax, ay, az);
p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
}

void getGyro(float &gx, float &gy, float &gz){
int gyroy = (int16_t)(psAxis[14]<<8) | psAxis[13];
int gyroz = (int16_t)(psAxis[16]<<8) | psAxis[15];
int gyrox = (int16_t)(psAxis[18]<<8) | psAxis[17];

gx = (float) gyrox * RAD_TO_DEG/1024;
gy = (float) gyroy * RAD_TO_DEG/1024;
gz = (float) gyroz * RAD_TO_DEG/1024;
}

+ 119
- 48
joystick.cpp View File

@@ -746,7 +746,17 @@ bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_
axis_changed_mask_ |= (1<<5);
axis[5] = data[9];
}

if (axis[3] != data[18]) {
axis_changed_mask_ |= (1<<3);
axis[3] = data[18];
}
if (axis[4] != data[19]) {
axis_changed_mask_ |= (1<<4);
axis[4] = data[19];
}
// Then rest of data
mask = 0x1 << 10; // setup for other bits
for (uint16_t i = 10; i < length; i++ ) {
@@ -780,55 +790,116 @@ bool JoystickController::process_bluetooth_HID_data(const uint8_t *data, uint16_
return true;

} else if(data[0] == 0x11){
DBGPrintf(" Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;
if (data[0] == 1) {
//print(" Joystick Data: ");
//print_hexbytes(data, length);
// DBGPrintf(" Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;

if (length > TOTAL_AXIS_COUNT) length = TOTAL_AXIS_COUNT; // don't overflow arrays...
for (uint16_t i = 0; i < length; i++ ) {
axis_mask_ |= mask;
if(data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = data[i];
}
mask <<= 1; // shift down the mask.
// DBGPrintf("%02x ", axis[i]);
}
// DBGPrintf("\n");
joystickEvent = true;
connected_ = true;
return true;
} else if(data[0] == 0x11){
DBGPrintf("\n Joystick Data: ");
uint64_t mask = 0x1;
axis_mask_ = 0;
axis_changed_mask_ = 0;
//This moves data to be equivalent to what we see for
//data[0] = 0x01
uint8_t tmp_data[length-2];
for (uint16_t i = 0; i < (length-2); i++ ) {
tmp_data[i] = 0;
tmp_data[i] = data[i+2];
}
/*
* [1] LX, [2] = LY, [3] = RX, [4] = RY
* [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3
* [6] R3,L3, opt, share, R2, L2, R1, L1
* [7] Counter (bit7-2), T-PAD, PS
* [8] Left Trigger, [9] Right Trigger
* [10-11] Timestamp
* [12] Battery (0 to 0xff)
* [13-14] acceleration x
* [15-16] acceleration y
* [17-18] acceleration z
* [19-20] gyro x
* [21-22] gyro y
* [23-24] gyro z
* [25-29] unknown
* [30] 0x00,phone,mic, usb, battery level (4bits)
* rest is trackpad? to do implement?
*/
//PS Bit
tmp_data[7] = (tmp_data[7] >> 0) & 1;
//set arrow buttons to axis[0]
tmp_data[10] = tmp_data[5] & ((1 << 4) - 1);
//set buttons for last 4bits in the axis[5]
tmp_data[5] = tmp_data[5] >> 4;
//This moves data to be equivalent to what we see for
//data[0] = 0x01
uint8_t tmp_data[length-2];
for (uint16_t i = 0; i < (length-2); i++ ) {
tmp_data[i] = data[i+2];
DBGPrintf("%02x ",tmp_data[i]);
}
DBGPrintf("\n");
/*
* [1] LX, [2] = LY, [3] = RX, [4] = RY
* [5] combo, tri, cir, x, sqr, D-PAD (4bits, 0-3
* [6] R3,L3, opt, share, R2, L2, R1, L1
* [7] Counter (bit7-2), T-PAD, PS
* [8] Left Trigger, [9] Right Trigger
* [10-11] Timestamp
* [12] Battery (0 to 0xff)
* [13-14] acceleration x
* [15-16] acceleration y
* [17-18] acceleration z
* [19-20] gyro x
* [21-22] gyro y
* [23-24] gyro z
* [25-29] unknown
* [30] 0x00,phone,mic, usb, battery level (4bits)
* rest is trackpad? to do implement?
*/
//PS Bit
tmp_data[7] = (tmp_data[7] >> 0) & 1;
//set arrow buttons to axis[0]
tmp_data[0] = tmp_data[5] & ((1 << 4) - 1);
//set buttons for last 4bits in the axis[5]
tmp_data[5] = tmp_data[5] >> 4;

for (uint16_t i = 0; i < (length-2); i++ ) {
if(tmp_data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = tmp_data[i];
}
mask <<= 1; // shift down the mask.
//DBGPrintf("%02x ", axis[i]);
// Quick and dirty hack to match PS4 HID data
uint32_t cur_buttons = tmp_data[7] | (tmp_data[10]) | ((tmp_data[6]*10)) | ((uint16_t)tmp_data[5] << 16) ;
if (cur_buttons != buttons) {
buttons = cur_buttons;
joystickEvent = true; // something changed.
}
mask = 0x1;
axis_mask_ = 0x27; // assume bits 0, 1, 2, 5
for (uint16_t i = 0; i < 3; i++) {
if (axis[i] != tmp_data[i+1]) {
axis_changed_mask_ |= mask;
axis[i] = tmp_data[i+1];
}
mask <<= 1; // shift down the mask.
}
if (axis[5] != tmp_data[4]) {
axis_changed_mask_ |= (1<<5);
axis[5] = tmp_data[4];
}
if (axis[3] != tmp_data[8]) {
axis_changed_mask_ |= (1<<3);
axis[3] = tmp_data[8];
}
if (axis[4] != tmp_data[9]) {
axis_changed_mask_ |= (1<<4);
axis[4] = tmp_data[9];
}
//limit for masking
mask = 0x1;
for (uint16_t i = 6; i < (64); i++ ) {
axis_mask_ |= mask;
if(tmp_data[i] != axis[i]) {
axis_changed_mask_ |= mask;
axis[i] = tmp_data[i];
}
mask <<= 1; // shift down the mask.
DBGPrintf("%02x ", axis[i]);
}
DBGPrintf("\n");
//DBGPrintf("Axis Mask (axis_mask_, axis_changed_mask_; %d, %d\n", axis_mask_,axis_changed_mask_);
joystickEvent = true;
connected_ = true;
}
//DBGPrintf("\n");
joystickEvent = true;
connected_ = true;
}
return false;
}

Write Preview
Loading…
Cancel
Save