diff --git a/switch.ino b/switch.ino index ce38031..b75258e 100644 --- a/switch.ino +++ b/switch.ino @@ -3,46 +3,39 @@ boolean shouldAccelerate(float freq) { } void sendGameData(float freq, float yaw) { - //TODO: Clean Up! - - digitalRead(pinButtonR); - digitalRead(pinButtonAdd); - digitalRead(pinButtonA); - digitalRead(pinButtonB); - Joystick.setButton(4, pressOrReleaseValue(pinButtonL)); // L Joystick.setButton(5, pressOrReleaseValue(pinButtonR)); // R Joystick.setButton(9, pressOrReleaseValue(pinButtonAdd)); // + Joystick.setButton(1, pressOrReleaseValue(pinButtonB)); // B Joystick.setButton(2, (((digitalRead(pinButtonA) == LOW) || shouldAccelerate(freq))) ? 1 : 0); // A - - if (freq > 0) { - // Move - float filteredYaw = yaw; + setSteerFromYaw(yaw); + } else { + Joystick.setXAxis(128); + Joystick.setYAxis(128); + } + + Joystick.sendState(); +} + +void setSteerFromYaw(float yaw) { + float filteredYaw = yaw; if(filteredYaw > 1.57079633) { // > 90 degrees filteredYaw = 1.57079633; } else if(filteredYaw < -1.57079633){ // < -90 degrees filteredYaw = -1.57079633; } + + float exaggeratedYaw = filteredYaw * 1.20; - float u = 128 * sin(filteredYaw), - v = 128 * cos(filteredYaw); + float u = 128 * sin(exaggeratedYaw), + v = 128 * cos(exaggeratedYaw); int x = 128 + ((int) u), y = 128 - ((int) v); Joystick.setXAxis(x); Joystick.setYAxis(y); - } else { - // Stay in the center - Joystick.setXAxis(128); - Joystick.setYAxis(128); - } - - - - Joystick.sendState(); } int pressOrReleaseValue(int button) {