task main() { initialize(); // Run one-time initializations. while (true) { getJoystickSettings(joystick); if (!joystick.StopPgm && joystick.UserMode) { // Update joystick variables leftJoyY = joystick.joy1_y1; rightJoyY = joystick.joy1_y2; leftJoyX = joystick.joy1_x1; // Control drive motors motor[leftFootA] = (deadzone(leftJoyY/slowDownFactor)*joySwap); motor[leftFootB] = (deadzone(leftJoyY/slowDownFactor)*joySwap); motor[rightFootA] = (deadzone(rightJoyY/slowDownFactor)*joySwap); motor[rightFootB] = (deadzone(rightJoyY/slowDownFactor)*joySwap); // Servo control section servo[head] = leftJoyX + 127; if (joy1Btn(5)){ servo[leftShoulderA] += 1; servo[leftShoulderB] -= 1; } else { servo[leftShoulderA] -= 1; servo[leftShoulderB] += 1; } if (joy1Btn(6)){ servo[rightShoulderA] += 1; servo[rightShoulderB] -= 1; } else { servo[rightShoulderA] -= 1; servo[rightShoulderB] += 1; } if (joy1Btn(7)){ servo[leftElbow] += 1; } else { servo[leftElbow] -= 1; } if (joy1Btn(8)){ servo[rightElbow] += 1; } else { servo[rightElbow] -= 1; } } } }
void TeleopPeriodic(void ) { /* * Code placed in here will be called only when a new packet of information * has been received by the Driver Station. Any code which needs new information * from the DS should go in here */ //Start compressor compressor->Start(); driveTrainValues(); deadzone(); //Drivetrain..... //When button eight is pressed robot drives at 25% speed printf("right: %f and left: %f\n", useright, useleft); if (gamepad->GetRawButton(8)) { drivetrain->TankDrive((-0.5*(useleft)), (-0.5*(useright))); //Negative for switched wires } else { drivetrain->SetLeftRightMotorOutputs(-useleft, -useright); //Normal driving //Negative for switched wires } }
DWORD Controller::GetState(XINPUT_STATE* pState) { // Passthrough? if (m_passthrough) return XInputModuleManager::Get().XInputGetState(m_passthroughindex, pState); if (!ControllerManager::Get().XInputEnabled()) { // Clear state if (pState) ZeroMemory(pState, sizeof(XINPUT_STATE)); return ERROR_SUCCESS; } // If state haven't changed yet then... HRESULT hr = UpdateState(); #if 0 PrintLog("UpdateState %u %u", dwUserIndex, hr); #endif if (FAILED(hr)) return ERROR_DEVICE_NOT_CONNECTED; pState->Gamepad.wButtons = 0; pState->Gamepad.bLeftTrigger = 0; pState->Gamepad.bRightTrigger = 0; pState->Gamepad.sThumbLX = 0; pState->Gamepad.sThumbLY = 0; pState->Gamepad.sThumbRX = 0; pState->Gamepad.sThumbRY = 0; // timestamp packet pState->dwPacketNumber = GetTickCount(); if (m_stateChanged != true) { // If first state is not aquired yet. if (m_emptyStateIsSet != true) { m_emptyState = m_state; m_emptyStateIsSet = true; } // Compare two states. int compareResult = memcmp(&m_emptyState, &m_state, sizeof(struct DIJOYSTATE2)); // If nothing changed then... if (compareResult == 0) { // Return. return ERROR_SUCCESS; } else { // Allow to use values. m_stateChanged = true; } } bool dPadButtons[16]; for (int i = 0; i < _countof(dPadButtons); ++i) dPadButtons[i] = false; // Loop trough D-Pad button states. for (int d = 0; d < _countof(m_state.rgdwPOV); ++d) { // No more than 4 D-Pads. if (d >= 4) break; int povdeg = m_state.rgdwPOV[d]; if (povdeg >= 0) { // Split PoV degrees into 8 groups by // converting PoV degree from 0 to 36000 to number from 0 to 7. // This will allow to have more flexible degree values mapped to buttons. s8 y = ((2250 + povdeg) / 4500) % 8; // XINPUT_GAMEPAD_DPAD_UP dPadButtons[d * 4 + 0] = (y >= 0 && y <= 1) || y == 7; // XINPUT_GAMEPAD_DPAD_RIGHT dPadButtons[d * 4 + 1] = (y >= 1 && y <= 3); // XINPUT_GAMEPAD_DPAD_DOWN dPadButtons[d * 4 + 2] = (y >= 3 && y <= 5); // XINPUT_GAMEPAD_DPAD_LEFT dPadButtons[d * 4 + 3] = (y >= 5 && y <= 7); } } // --- Map POV to the D-pad --- if (m_mapping.DpadPOV > 0 && m_mapping.PovIsButton == false) { //INT pov = POVState(m_mapping.DpadPOV,dwUserIndex,Gamepad[dwUserIndex].povrotation); s8 dPadIndex = m_mapping.DpadPOV - 1; if (dPadButtons[dPadIndex * 4 + 0]) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; if (dPadButtons[dPadIndex * 4 + 1]) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; if (dPadButtons[dPadIndex * 4 + 2]) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; if (dPadButtons[dPadIndex * 4 + 3]) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; } //else if (m_mapping.DpadPOV > 0 && m_mapping.PovIsButton == true) //{ // for (int i = 0; i < _countof(m_mapping.pov); ++i) // { // if (ButtonPressed(m_mapping.pov[i])) // { // pState->Gamepad.wButtons |= Config::povIDs[i]; // } // } //} // Created so we can refer to each axis with an ID s32 axis[] = { m_state.lX, m_state.lY, m_state.lZ, m_state.lRx, m_state.lRy, m_state.lRz }; s32 slider[] = { m_state.rglSlider[0], m_state.rglSlider[1] }; // --- Map buttons --- if (ButtonPressed(m_mapping.guide)) pState->Gamepad.wButtons |= 0x400; for (u32 i = 0; i < _countof(m_mapping.Button); ++i) { s8 mapId = m_mapping.Button[i].id; // Skip invalid mappings if (mapId == 0) continue; u32 mapIndex = std::abs(mapId) - 1; Config::MappingType buttonType = m_mapping.Button[i].type; if (buttonType == Config::DIGITAL) { if (ButtonPressed(mapId - 1)) pState->Gamepad.wButtons |= Config::buttonIDs[i]; } // D-Pad button to normal button. else if (buttonType == Config::DPADBUTTON) { if (mapIndex < _countof(dPadButtons) && dPadButtons[mapIndex]) pState->Gamepad.wButtons |= Config::buttonIDs[i]; } else { s32 *values; bool isRange = false; bool isHalf = false; switch (buttonType) { case Config::AXIS: case Config::HAXIS: case Config::CBUT: values = axis; break; case Config::SLIDER: case Config::HSLIDER: values = slider; break; default: values = axis; break; } switch (buttonType) { // Full range case Config::AXIS: case Config::SLIDER: isRange = true; break; // Half range case Config::HAXIS: case Config::HSLIDER: case Config::CBUT: isRange = true; isHalf = true; break; default: break; } s32 v = 0; s8 id = m_mapping.Button[i].id; u32 index = std::abs(id) - 1; if (id != 0) { v = values[index]; } // [ 32768 steps | 32768 steps ] // DInput [ 0 32767 | 32768 65535 ] // XInput [ -32768 -1 | 0 32767 ] // u16 deadZone = (u16)m_mapping.Button[i].buttondz; bool invert = id < 0; s32 min = -32768; s32 max = 32767; s32 diValue; if (isHalf) { diValue = (invert) ? -1 - v : v; } else { diValue = (invert) ? max - v : v - min; deadZone = deadZone * 2; } if (isRange) { PrintLog("Axis/Slider: %d, invert = %d, half = %d, deadZone %d diValue %d", v, invert, isHalf, deadZone, diValue); if (diValue > deadZone) { pState->Gamepad.wButtons |= Config::buttonIDs[i]; } } } } // --- Map triggers --- u8 *targetTrigger[] = { &pState->Gamepad.bLeftTrigger, &pState->Gamepad.bRightTrigger }; for (u32 i = 0; i < _countof(m_mapping.Trigger); ++i) { s8 triggerMapId = m_mapping.Trigger[i].id; // Skip invalid mappings if (triggerMapId == 0) continue; s8 triggerMapIndex = (s8)std::abs(triggerMapId) - 1; Config::MappingType triggerType = m_mapping.Trigger[i].type; if (triggerType == Config::DIGITAL) { if (ButtonPressed(triggerMapIndex)) *(targetTrigger[i]) = 255; } else if (triggerType == Config::DPADBUTTON) { if (triggerMapIndex < _countof(dPadButtons) && dPadButtons[triggerMapIndex]) *(targetTrigger[i]) = 255; } else { s32 *values; switch (triggerType) { case Config::AXIS: case Config::HAXIS: case Config::CBUT: values = axis; break; case Config::SLIDER: case Config::HSLIDER: values = slider; break; default: values = axis; break; } s32 v = 0; if (m_mapping.Trigger[i].id > 0) { v = values[m_mapping.Trigger[i].id - 1]; } else if (m_mapping.Trigger[i].id < 0) { v = -values[-m_mapping.Trigger[i].id - 1] - 1; } /* FIXME: axis negative max should be -32768 --- v is the full range (-32768 .. +32767) that should be projected to 0...255 --- Full ranges AXIS: ( 0 to 255 from -32768 to 32767) using axis SLIDER: ( 0 to 255 from -32768 to 32767) using slider ------------------------------------------------------------------------------ --- Half ranges HAXIS: ( 0 to 255 from 0 to 32767) using axis HSLIDER: ( 0 to 255 from 0 to 32767) using slider */ s32 v2 = 0; s32 offset = 0; s32 scaling = 1; switch (triggerType) { // Full range case Config::AXIS: case Config::SLIDER: scaling = 255; offset = 32767; break; // Half range case Config::HAXIS: case Config::HSLIDER: case Config::CBUT: // add ///////////////////////////////////////////////////////// scaling = 127; offset = 0; break; default: scaling = 1; offset = 0; break; } //v2 = (v + offset) / scaling; // Add deadzones //*(targetTrigger[i]) = (BYTE) deadzone(v2, 0, 255, pController->triggerdz, 255); ///////////////////////////////////////////////////////////////////////////////////////// if (triggerType == Config::CBUT) { if (ButtonPressed(m_mapping.Trigger[0].but) && ButtonPressed(m_mapping.Trigger[1].but)) { *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255; } if (ButtonPressed(m_mapping.Trigger[0].but) && !ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset - v) / scaling; *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255 - (u8)deadzone(v2, 0, 255, m_mapping.Trigger[1].triggerdz, 255); } if (!ButtonPressed(m_mapping.Trigger[0].but) && ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset + v) / scaling; *(targetTrigger[0]) = 255 - (u8)deadzone(v2, 0, 255, m_mapping.Trigger[0].triggerdz, 255); *(targetTrigger[1]) = 255; } if (!ButtonPressed(m_mapping.Trigger[0].but) && !ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset + v) / scaling; *(targetTrigger[i]) = (u8)deadzone(v2, 0, 255, m_mapping.Trigger[i].triggerdz, 255); } } else { v2 = (offset + v) / scaling; *(targetTrigger[i]) = (u8)deadzone(v2, 0, 255, m_mapping.Trigger[i].triggerdz, 255); } ///////////////////////////////////////////////////////////////////////////////////////// } } // --- Map thumbsticks --- // Created so we can refer to each axis with an ID SHORT *targetAxis[4] = { &pState->Gamepad.sThumbLX, &pState->Gamepad.sThumbLY, &pState->Gamepad.sThumbRX, &pState->Gamepad.sThumbRY }; for (u32 i = 0; i < _countof(m_mapping.Axis); ++i) { if (m_mapping.Axis[i].axistodpad == 0) { s8 axisMapId = m_mapping.Axis[i].id; // Skip invalid mappings if (axisMapId != 0) { u32 index = std::abs(axisMapId) - 1; s32 value = axis[index]; // Analog input if (m_mapping.Axis[i].analogType == Config::AXIS) value = axis[index]; if (m_mapping.Axis[i].analogType == Config::SLIDER) value = slider[index]; if (m_mapping.Axis[i].analogType != Config::NONE) { // [ 32768 steps | 32768 steps ] // DInput [ 0 32767 | 32768 65535 ] // XInput [ -32768 -1 | 0 32767 ] // //int xInput = dInputValue; s32 xInput = value; s32 deadZone = (s32)m_mapping.Axis[i].axisdeadzone; s32 antiDeadZone = (s32)m_mapping.Axis[i].antideadzone; s32 linear = (s32)m_mapping.Axis[i].axislinear; s32 min = -32768; s32 max = 32767; bool invert = axisMapId < 0; // If axis should be inverted, convert [-32768;32767] -> [32767;-32768] if (invert) xInput = -1 - xInput; // The following sections expect xInput values in range [0;32767] // So, convert to positive: [-32768;-1] -> [32767;0] bool negative = xInput < 0; if (negative) xInput = -1 - xInput; // If deadzone value is set then... if (deadZone > 0) { if (xInput > deadZone) { // [deadZone;32767] => [0;32767]; xInput = (s32)((float)(xInput - deadZone) / (float)(max - deadZone) * (float)max); } else { xInput = 0; } } // If anti-deadzone value is set then... if (antiDeadZone > 0) { if (xInput > 0) { // [0;32767] => [antiDeadZone;32767]; xInput = (s32)((float)(xInput) / (float)max * (float)(max - antiDeadZone) + antiDeadZone); } } // If linear value is set then... if (linear != 0 && xInput > 0) { // [antiDeadZone;32767] => [0;32767]; float xInputF = (float)(xInput - antiDeadZone) / (float)(max - antiDeadZone) * (float)max; float linearF = (float)linear / 100.f; xInputF = ConvertToFloat((short)xInputF); float x = -xInputF; if (linearF < 0.f) x = 1.f + x; float v = ((float)sqrt(1.f - x * x)); if (linearF < 0.f) v = 1.f - v; xInputF = xInputF + (2.f - v - xInputF - 1.f) * abs(linearF); xInput = ConvertToShort(xInputF); // [0;32767] => [antiDeadZone;32767]; xInput = (s32)((float)(xInput) / (float)max * (float)(max - antiDeadZone) + antiDeadZone); } // If originally negative, convert back: [32767;0] -> [-32768;-1] if (negative) xInput = -1 - xInput; *(targetAxis[i]) = (s16)clamp(xInput, min, max); //return (short)xInput; } } Config::MappingType posType = m_mapping.Axis[i].positiveType; Config::MappingType negType = m_mapping.Axis[i].negativeType; s8 posMapId = m_mapping.Axis[i].positiveButtonID - 1; s8 negMapId = m_mapping.Axis[i].negativeButtonID - 1; // Map button to positive axis direction. if (posType == Config::DIGITAL && posMapId >= 0 && ButtonPressed(posMapId)) *(targetAxis[i]) = 32767; // Map button to negative axis direction. if (negType == Config::DIGITAL && negMapId >= 0 && ButtonPressed(negMapId)) *(targetAxis[i]) = -32768; // Map D-Pad button to positive axis direction. if (posType == Config::DPADBUTTON && posMapId >= 0 && posMapId < _countof(dPadButtons) && dPadButtons[posMapId]) *(targetAxis[i]) = 32767; // Map D-Pad button to negative axis direction. if (negType == Config::DPADBUTTON && negMapId >= 0 && negMapId < _countof(dPadButtons) && dPadButtons[negMapId]) *(targetAxis[i]) = -32768; } else { //PrintLog("x: %d, y: %d, z: %d",Gamepad[dwUserIndex].state.lX,Gamepad[dwUserIndex].state.lY,Gamepad[dwUserIndex].state.lZ); if (m_state.lX - m_mapping.Axis[i].a2doffset > m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; if (m_state.lX - m_mapping.Axis[i].a2doffset < -m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; if (m_state.lY - m_mapping.Axis[i].a2doffset < -m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; if (m_state.lY - m_mapping.Axis[i].a2doffset > m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; } } // prevent sleep SetThreadExecutionState(ES_SYSTEM_REQUIRED | ES_DISPLAY_REQUIRED); return ERROR_SUCCESS; }
extern "C" DWORD WINAPI XInputGetState(DWORD dwUserIndex, XINPUT_STATE* pState) { //PrintLog("XInputGetState"); if(g_bDisable) return ERROR_DEVICE_NOT_CONNECTED; if((dwUserIndex+1 > g_Devices.size() || g_Devices[dwUserIndex].passthrough) && XInputInitialize()) return xinput.XInputGetState(dwUserIndex, pState); DInputDevice& device = g_Devices[dwUserIndex]; if (!pState || !(dwUserIndex < XUSER_MAX_COUNT)) return ERROR_BAD_ARGUMENTS; HRESULT hr = E_FAIL; if(hMsgWnd == NULL) CreateMsgWnd(); if(device.device == NULL && device.dwUserIndex == dwUserIndex) DeviceInitialize(device); if(!device.device) return ERROR_DEVICE_NOT_CONNECTED; //Update device state if enabled or we not use enable if(XInputIsEnabled.bEnabled || !XInputIsEnabled.bUseEnabled) hr = UpdateState(device); #if defined(DEBUG) | defined(_DEBUG) PrintLog("UpdateState %d %d",dwUserIndex,hr); #endif if(FAILED(hr)) return ERROR_DEVICE_NOT_CONNECTED; Mapping& mapping = g_Mappings[dwUserIndex]; XINPUT_STATE& xstate = *pState; xstate.Gamepad.wButtons = 0; xstate.Gamepad.bLeftTrigger = 0; xstate.Gamepad.bRightTrigger = 0; xstate.Gamepad.sThumbLX = 0; xstate.Gamepad.sThumbLY = 0; xstate.Gamepad.sThumbRX = 0; xstate.Gamepad.sThumbRY = 0; if(!XInputIsEnabled.bEnabled && XInputIsEnabled.bUseEnabled) return ERROR_SUCCESS; // timestamp packet xstate.dwPacketNumber=GetTickCount(); // --- Map buttons --- for (int i = 0; i < 10; ++i) { if (((int)mapping.Button[i] >= 0) && ButtonPressed(mapping.Button[i],device)) xstate.Gamepad.wButtons |= buttonIDs[i]; } // --- Map POV to the D-pad --- if (mapping.DpadPOV > 0 && mapping.PovIsButton == false) { //INT pov = POVState(mapping.DpadPOV,dwUserIndex,Gamepad[dwUserIndex].povrotation); int povdeg = device.state.rgdwPOV[mapping.DpadPOV-1]; if(povdeg >= 0) { // Up-left, up, up-right, up (at 360 degrees) if (IN_RANGE2(povdeg,mapping.pov[GAMEPAD_DPAD_LEFT]+1,mapping.pov[GAMEPAD_DPAD_UP]) || IN_RANGE2(povdeg,0,mapping.pov[GAMEPAD_DPAD_RIGHT]-1)) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; // Up-right, right, down-right if (IN_RANGE(povdeg,0,mapping.pov[GAMEPAD_DPAD_DOWN])) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; // Down-right, down, down-left if (IN_RANGE(povdeg,mapping.pov[GAMEPAD_DPAD_RIGHT],mapping.pov[GAMEPAD_DPAD_LEFT])) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; // Down-left, left, up-left if (IN_RANGE(povdeg,mapping.pov[GAMEPAD_DPAD_DOWN],mapping.pov[GAMEPAD_DPAD_UP])) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; } } else if(mapping.PovIsButton == true) { for (int i = 0; i < 4; ++i) { if (((int)mapping.pov[i] >= 0) && ButtonPressed(mapping.pov[i],device)) { xstate.Gamepad.wButtons |= povIDs[i]; } } } // Created so we can refer to each axis with an ID LONG axis[] = { device.state.lX, device.state.lY, device.state.lZ, device.state.lRx, device.state.lRy, device.state.lRz, 0 }; LONG slider[] = { device.state.rglSlider[0], device.state.rglSlider[1] }; // --- Map triggers --- BYTE *targetTrigger[] = { & xstate.Gamepad.bLeftTrigger, & xstate.Gamepad.bRightTrigger }; for (size_t i = 0; i < 2; ++i) { MappingType triggerType = mapping.Trigger[i].type; if (triggerType == DIGITAL) { if(ButtonPressed(mapping.Trigger[i].id,device))*(targetTrigger[i]) = 255; } else { LONG *values; switch (triggerType) { case AXIS: case HAXIS: case CBUT: values = axis; break; case SLIDER: case HSLIDER: values = slider; break; default: values = axis; break; } LONG v = 0; if(mapping.Trigger[i].id > 0) v = values[mapping.Trigger[i].id -1]; else v = -values[-mapping.Trigger[i].id -1] - 1; /* FIXME: axis negative max should be -32768 --- v is the full range (-32767 .. +32767) that should be projected to 0...255 --- Full ranges AXIS: ( 0 to 255 from -32767 to 32767) using axis SLIDER: ( 0 to 255 from -32767 to 32767) using slider ------------------------------------------------------------------------------ --- Half ranges HAXIS: ( 0 to 255 from 0 to 32767) using axis HSLIDER: ( 0 to 255 from 0 to 32767) using slider */ LONG v2=0; LONG offset=0; LONG scaling=1; switch (triggerType) { // Full range case AXIS: case SLIDER: scaling = 255; offset = 32767; break; // Half range case HAXIS: case HSLIDER: case CBUT: // add ///////////////////////////////////////////////////////// scaling = 127; offset = 0; break; default: scaling = 1; offset = 0; break; } //v2 = (v + offset) / scaling; // Add deadzones //*(targetTrigger[i]) = (BYTE) deadzone(v2, 0, 255, device.triggerdz, 255); ///////////////////////////////////////////////////////////////////////////////////////// if (triggerType == CBUT) { if (ButtonPressed(mapping.Trigger[0].but,device) && ButtonPressed(mapping.Trigger[1].but,device)) { *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255; } if (ButtonPressed(mapping.Trigger[0].but,device) && !ButtonPressed(mapping.Trigger[1].but,device)) { v2 = (offset-v) / scaling; *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255 - (BYTE) deadzone(v2, 0, 255, device.triggerdz[1], 255); } if (!ButtonPressed(mapping.Trigger[0].but,device) && ButtonPressed(mapping.Trigger[1].but,device)) { v2 = (offset+v) / scaling; *(targetTrigger[0]) = 255 - (BYTE) deadzone(v2, 0, 255, device.triggerdz[0], 255); *(targetTrigger[1]) = 255; } if (!ButtonPressed(mapping.Trigger[0].but,device) && !ButtonPressed(mapping.Trigger[1].but,device)) { v2 = (offset+v) / scaling; *(targetTrigger[i]) = (BYTE) deadzone(v2, 0, 255, device.triggerdz[i], 255); } } else { v2 = (offset+v) / scaling; *(targetTrigger[i]) = (BYTE) deadzone(v2, 0, 255, device.triggerdz[i], 255); } ///////////////////////////////////////////////////////////////////////////////////////// } } // --- Map thumbsticks --- // Created so we can refer to each axis with an ID SHORT *targetAxis[4] = { & xstate.Gamepad.sThumbLX, & xstate.Gamepad.sThumbLY, & xstate.Gamepad.sThumbRX, & xstate.Gamepad.sThumbRY }; // NOTE: Could add symbolic constants as indexers, such as // THUMB_LX_AXIS, THUMB_LX_POSITIVE, THUMB_LX_NEGATIVE if(device.axistodpad==0) { for (INT i = 0; i < 4; ++i) { LONG *values = axis; // Analog input if (mapping.Axis[i].analogType == AXIS) values = axis; if (mapping.Axis[i].analogType == SLIDER) values = slider; if (mapping.Axis[i].analogType != NONE) { if(mapping.Axis[i].id > 0 ) { SHORT val = (SHORT) values[mapping.Axis[i].id - 1]; *(targetAxis[i])= (SHORT) clamp(val,-32767,32767); } else if(mapping.Axis[i].id < 0 ) { SHORT val = (SHORT) -values[-mapping.Axis[i].id - 1]; *(targetAxis[i]) = (SHORT) clamp(val,-32767,32767); } } // Digital input, positive direction if (mapping.Axis[i].hasDigital && mapping.Axis[i].positiveButtonID >= 0) { if (ButtonPressed(mapping.Axis[i].positiveButtonID,device)) *(targetAxis[i]) = 32767; } // Digital input, negative direction if (mapping.Axis[i].hasDigital && mapping.Axis[i].negativeButtonID >= 0) { if (ButtonPressed(mapping.Axis[i].negativeButtonID,device)) *(targetAxis[i]) = -32767; } } } //WILDS - Axis to D-Pad if(device.axistodpad==1) { //PrintLog("x: %d, y: %d, z: %d",Gamepad[dwUserIndex].state.lX,Gamepad[dwUserIndex].state.lY,Gamepad[dwUserIndex].state.lZ); if(device.state.lX - device.a2doffset > device.a2ddeadzone) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; if(device.state.lX - device.a2doffset < -device.a2ddeadzone) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; if(device.state.lY - device.a2doffset < -device.a2ddeadzone) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; if(device.state.lY - device.a2doffset > device.a2ddeadzone) xstate.Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; } //WILDS END for (int i = 0; i < 4; ++i) { if (device.antideadzone[i]) { SHORT antidz = device.antideadzone[i]; LONG val = *(targetAxis[i]); SHORT direction = val > 0 ? 1 : -1; val = (LONG)(abs(val) / (32767 / (32767 - antidz * 1.0)) + antidz); val = min(val, 32767); if(val == device.antideadzone[i] || val == -device.antideadzone[i]) val = 0; *(targetAxis[i]) = (SHORT) (direction * val); } if (device.axisdeadzone[i]) { SHORT dz = device.axisdeadzone[i]; LONG val = *(targetAxis[i]); if((val <= dz) && (val >= -dz) ) val = 0; *(targetAxis[i]) = (SHORT) clamp(val,-32767,32767); } // --- Do Linears --- if (device.axislinear[i]) { SHORT absval = (SHORT)((abs(*(targetAxis[i])) + (((32767.0 / 2.0) - (((abs((abs(*(targetAxis[i]))) - (32767.0 / 2.0)))))) * (device.axislinear[i] * 0.01)))); *(targetAxis[i]) = *(targetAxis[i]) > 0 ? absval : -absval; } } return ERROR_SUCCESS; }
DWORD Controller::GetState(XINPUT_STATE* pState) { // Passthrough? if (m_passthrough) return XInputModuleManager::Get().XInputGetState(m_passthroughindex, pState); if (!ControllerManager::Get().XInputEnabled()) { // Clear state if (pState) ZeroMemory(pState, sizeof(XINPUT_STATE)); return ERROR_SUCCESS; } HRESULT hr = UpdateState(); #if 0 PrintLog("UpdateState %u %u", dwUserIndex, hr); #endif if (FAILED(hr)) return ERROR_DEVICE_NOT_CONNECTED; pState->Gamepad.wButtons = 0; pState->Gamepad.bLeftTrigger = 0; pState->Gamepad.bRightTrigger = 0; pState->Gamepad.sThumbLX = 0; pState->Gamepad.sThumbLY = 0; pState->Gamepad.sThumbRX = 0; pState->Gamepad.sThumbRY = 0; // timestamp packet pState->dwPacketNumber = GetTickCount(); // --- Map POV to the D-pad --- if (m_mapping.DpadPOV > 0 && m_mapping.PovIsButton == false) { //INT pov = POVState(m_mapping.DpadPOV,dwUserIndex,Gamepad[dwUserIndex].povrotation); int povdeg = m_state.rgdwPOV[m_mapping.DpadPOV - 1]; if (povdeg >= 0) { // Up-left, up, up-right, up (at 360 degrees) if (IN_RANGE2(povdeg, m_mapping.pov[Config::DPAD_LEFT] + 1, m_mapping.pov[Config::DPAD_UP]) || IN_RANGE2(povdeg, 0, m_mapping.pov[Config::DPAD_RIGHT] - 1)) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; // Up-right, right, down-right if (IN_RANGE(povdeg, 0, m_mapping.pov[Config::DPAD_DOWN])) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; // Down-right, down, down-left if (IN_RANGE(povdeg, m_mapping.pov[Config::DPAD_RIGHT], m_mapping.pov[Config::DPAD_LEFT])) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; // Down-left, left, up-left if (IN_RANGE(povdeg, m_mapping.pov[Config::DPAD_DOWN], m_mapping.pov[Config::DPAD_UP])) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; } } else if (m_mapping.PovIsButton == true) { for (int i = 0; i < _countof(m_mapping.pov); ++i) { if (ButtonPressed(m_mapping.pov[i])) { pState->Gamepad.wButtons |= Config::povIDs[i]; } } } // Created so we can refer to each axis with an ID s32 axis[] = { m_state.lX, m_state.lY, m_state.lZ, m_state.lRx, m_state.lRy, m_state.lRz }; s32 slider[] = { m_state.rglSlider[0], m_state.rglSlider[1] }; // --- Map buttons --- if (ButtonPressed(m_mapping.guide)) pState->Gamepad.wButtons |= 0x400; for (u32 i = 0; i < _countof(m_mapping.Button); ++i) { // Skip invalid mappings if (m_mapping.Button[i].id == 0) continue; Config::MappingType buttonType = m_mapping.Button[i].type; if (buttonType == Config::DIGITAL) { if (ButtonPressed(m_mapping.Button[i].id -1)) pState->Gamepad.wButtons |= Config::buttonIDs[i]; } else { s32 *values; switch (buttonType) { case Config::AXIS: case Config::HAXIS: case Config::CBUT: values = axis; break; case Config::SLIDER: case Config::HSLIDER: values = slider; break; default: values = axis; break; } s32 v = 0; if (m_mapping.Button[i].id > 0) { v = values[m_mapping.Button[i].id - 1]; } else if (m_mapping.Button[i].id < 0) { v = -values[-m_mapping.Button[i].id - 1] - 1; } s32 v2 = 0; s32 offset = 0; s32 scaling = 1; switch (buttonType) { // Full range case Config::AXIS: case Config::SLIDER: scaling = 255; offset = 32767; break; // Half range case Config::HAXIS: case Config::HSLIDER: case Config::CBUT: scaling = 127; offset = 0; break; default: scaling = 1; offset = 0; break; } v2 = (offset + v) / scaling; if (deadzone(v2, 0, 1, m_mapping.Button[i].buttondz, 1) > 0) { pState->Gamepad.wButtons |= Config::buttonIDs[i]; } } } // --- Map triggers --- u8 *targetTrigger[] = { &pState->Gamepad.bLeftTrigger, &pState->Gamepad.bRightTrigger }; for (u32 i = 0; i < _countof(m_mapping.Trigger); ++i) { // Skip invalid mappings if (m_mapping.Trigger[i].id == 0) continue; Config::MappingType triggerType = m_mapping.Trigger[i].type; if (triggerType == Config::DIGITAL) { if (ButtonPressed(m_mapping.Trigger[i].id - 1))*(targetTrigger[i]) = 255; } else { s32 *values; switch (triggerType) { case Config::AXIS: case Config::HAXIS: case Config::CBUT: values = axis; break; case Config::SLIDER: case Config::HSLIDER: values = slider; break; default: values = axis; break; } s32 v = 0; if (m_mapping.Trigger[i].id > 0) { v = values[m_mapping.Trigger[i].id - 1]; } else if (m_mapping.Trigger[i].id < 0) { v = -values[-m_mapping.Trigger[i].id - 1] - 1; } /* FIXME: axis negative max should be -32768 --- v is the full range (-32768 .. +32767) that should be projected to 0...255 --- Full ranges AXIS: ( 0 to 255 from -32768 to 32767) using axis SLIDER: ( 0 to 255 from -32768 to 32767) using slider ------------------------------------------------------------------------------ --- Half ranges HAXIS: ( 0 to 255 from 0 to 32767) using axis HSLIDER: ( 0 to 255 from 0 to 32767) using slider */ s32 v2 = 0; s32 offset = 0; s32 scaling = 1; switch (triggerType) { // Full range case Config::AXIS: case Config::SLIDER: scaling = 255; offset = 32767; break; // Half range case Config::HAXIS: case Config::HSLIDER: case Config::CBUT: // add ///////////////////////////////////////////////////////// scaling = 127; offset = 0; break; default: scaling = 1; offset = 0; break; } //v2 = (v + offset) / scaling; // Add deadzones //*(targetTrigger[i]) = (BYTE) deadzone(v2, 0, 255, pController->triggerdz, 255); ///////////////////////////////////////////////////////////////////////////////////////// if (triggerType == Config::CBUT) { if (ButtonPressed(m_mapping.Trigger[0].but) && ButtonPressed(m_mapping.Trigger[1].but)) { *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255; } if (ButtonPressed(m_mapping.Trigger[0].but) && !ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset - v) / scaling; *(targetTrigger[0]) = 255; *(targetTrigger[1]) = 255 - (u8)deadzone(v2, 0, 255, m_mapping.Trigger[1].triggerdz, 255); } if (!ButtonPressed(m_mapping.Trigger[0].but) && ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset + v) / scaling; *(targetTrigger[0]) = 255 - (u8)deadzone(v2, 0, 255, m_mapping.Trigger[0].triggerdz, 255); *(targetTrigger[1]) = 255; } if (!ButtonPressed(m_mapping.Trigger[0].but) && !ButtonPressed(m_mapping.Trigger[1].but)) { v2 = (offset + v) / scaling; *(targetTrigger[i]) = (u8)deadzone(v2, 0, 255, m_mapping.Trigger[i].triggerdz, 255); } } else { v2 = (offset + v) / scaling; *(targetTrigger[i]) = (u8)deadzone(v2, 0, 255, m_mapping.Trigger[i].triggerdz, 255); } ///////////////////////////////////////////////////////////////////////////////////////// } } // --- Map thumbsticks --- // Created so we can refer to each axis with an ID SHORT *targetAxis[4] = { &pState->Gamepad.sThumbLX, &pState->Gamepad.sThumbLY, &pState->Gamepad.sThumbRX, &pState->Gamepad.sThumbRY }; for (u32 i = 0; i < _countof(m_mapping.Axis); ++i) { if (m_mapping.Axis[i].axistodpad == 0) { // Skip invalid mappings if (m_mapping.Axis[i].id != 0) { u32 index = std::abs(m_mapping.Axis[i].id) - 1; s32 value = axis[index]; // Analog input if (m_mapping.Axis[i].analogType == Config::AXIS) value = axis[index]; if (m_mapping.Axis[i].analogType == Config::SLIDER) value = slider[index]; if (m_mapping.Axis[i].analogType != Config::NONE) { // [ 32768 steps | 32768 steps ] // DInput [ 0 32767 | 32768 65535 ] // XInput [ 32768 -1 | 0 32767 ] // //int xInput = dInputValue; s32 xInput = value; s32 deadZone = (s32)m_mapping.Axis[i].axisdeadzone; s32 antiDeadZone = (s32)m_mapping.Axis[i].antideadzone; s32 linear = (s32)m_mapping.Axis[i].axislinear; s32 min = -32768; s32 max = 32767; bool invert = m_mapping.Axis[i].id < 0; // If axis should be inverted, convert [-32768;32767] -> [32767;-32768] if (invert) xInput = -1 - xInput; // The following sections expect xInput values in range [0;32767] // So, convert to positive: [-32768;-1] -> [32767;0] bool negative = xInput < 0; if (negative) xInput = -1 - xInput; // If deadzone value is set then... if (deadZone > 0) { if (xInput > deadZone) { // [deadZone;32767] => [0;32767]; xInput = (s32)((float)(xInput - deadZone) / (float)(max - deadZone) * (float)max); } else { xInput = 0; } } // If anti-deadzone value is set then... if (antiDeadZone > 0) { if (xInput > 0) { // [0;32767] => [antiDeadZone;32767]; xInput = (s32)((float)(xInput) / (float)max * (float)(max - antiDeadZone) + antiDeadZone); } } // If linear value is set then... if (linear != 0 && xInput > 0) { // [antiDeadZone;32767] => [0;32767]; float xInputF = (float)(xInput - antiDeadZone) / (float)(max - antiDeadZone) * (float)max; float linearF = (float)linear / 100.f; xInputF = ConvertToFloat((short)xInputF); float x = -xInputF; if (linearF < 0.f) x = 1.f + x; float v = ((float)sqrt(1.f - x * x)); if (linearF < 0.f) v = 1.f - v; xInputF = xInputF + (2.f - v - xInputF - 1.f) * abs(linearF); xInput = ConvertToShort(xInputF); // [0;32767] => [antiDeadZone;32767]; xInput = (s32)((float)(xInput) / (float)max * (float)(max - antiDeadZone) + antiDeadZone); } // If originally negative, convert back: [32767;0] -> [-32768;-1] if (negative) xInput = -1 - xInput; *(targetAxis[i]) = (s16)clamp(xInput, min, max); //return (short)xInput; } } // Map axis to Button: Digital input, positive direction if (m_mapping.Axis[i].hasDigital && m_mapping.Axis[i].positiveButtonID >= 0) { if (ButtonPressed(m_mapping.Axis[i].positiveButtonID)) *(targetAxis[i]) = 32767; } // Map axis to Button: Digital input, negative direction if (m_mapping.Axis[i].hasDigital && m_mapping.Axis[i].negativeButtonID >= 0) { if (ButtonPressed(m_mapping.Axis[i].negativeButtonID)) *(targetAxis[i]) = -32768; } } else { //PrintLog("x: %d, y: %d, z: %d",Gamepad[dwUserIndex].state.lX,Gamepad[dwUserIndex].state.lY,Gamepad[dwUserIndex].state.lZ); if (m_state.lX - m_mapping.Axis[i].a2doffset > m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_LEFT; if (m_state.lX - m_mapping.Axis[i].a2doffset < -m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_RIGHT; if (m_state.lY - m_mapping.Axis[i].a2doffset < -m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_UP; if (m_state.lY - m_mapping.Axis[i].a2doffset > m_mapping.Axis[i].a2ddeadzone) pState->Gamepad.wButtons |= XINPUT_GAMEPAD_DPAD_DOWN; } } return ERROR_SUCCESS; }
void state_game_event(void *ptr) { SDL_Event e = ((SDL_Event *)ptr)[0]; if(e.type == SDL_KEYDOWN) { switch(e.key.keysym.sym) { case SDLK_ESCAPE: state_queue_pop(); break; case SDLK_SPACE: entity_jump(pos, PLAYER_JUMPSPEED); break; case SDLK_i: state_queue_push(INVENTORY, 0); break; case SDLK_v: lines = !lines; break; case SDLK_m: takeinput = !takeinput; break; case SDLK_c: info("Coords x: %f y: %f z: %f \n", posptr->x, posptr->y, posptr->z); break; case SDLK_t: { vec3_t top = *posptr; top.y = worldgen_get_height_of_pos(0, posptr->x, posptr->z)+1; entity_pos_set(pos, top); break; } case SDLK_f: flying = !flying; break; case SDLK_u: updating = !updating; info("UPDATING: %i\n", updating); break; case SDLK_p: pp = !pp; if(!pp) { glBindFramebuffer(GL_FRAMEBUFFER, 0); glEnable(GL_DEPTH_TEST); glUseProgram(drawprogram); } break; } } else if(e.type == SDL_CONTROLLERBUTTONDOWN) { switch(e.cbutton.button) { case SDL_CONTROLLER_BUTTON_A: entity_jump(pos, PLAYER_JUMPSPEED); break; } } else if(e.type == SDL_CONTROLLERAXISMOTION) { switch(e.caxis.axis) { case SDL_CONTROLLER_AXIS_LEFTX: leftstick.x = deadzone(e.caxis.value / 32768.0); break; case SDL_CONTROLLER_AXIS_LEFTY: leftstick.y = deadzone(e.caxis.value / 32768.0); break; case SDL_CONTROLLER_AXIS_RIGHTX: rightstick.x = deadzone(e.caxis.value / 32768.0); break; case SDL_CONTROLLER_AXIS_RIGHTY: rightstick.y = deadzone(e.caxis.value / 32768.0); break; } } else if(e.type == SDL_MOUSEBUTTONDOWN) { if(e.button.button == SDL_BUTTON_LEFT) { block_t b; b.id = WATER_GEN; world_ray_set(&headpos, &forwardcamera, b, 1, 1, 1000); } else if(e.button.button == SDL_BUTTON_RIGHT) { world_ray_del(&headpos, &forwardcamera, 1, 1000); } } else if(e.type == SDL_WINDOWEVENT) { if(e.window.event == SDL_WINDOWEVENT_RESIZED) state_game_window_resize(); } }
//================================================================== task main() { waitForStart(); servo[shrub] = servo_stop; clearTimer(T3); //unjam timer while(1){ //Control Processing prev1Btns = joystick.joy1_Buttons; prev2Btns = joystick.joy2_Buttons; getJoystickSettings(joystick); toggle1Btns = (toggle1Btns^((joystick.joy1_Buttons) & (~prev1Btns)));//invert toggle1Btns when the button goes from low to high toggle2Btns = (toggle2Btns^((joystick.joy2_Buttons) & (~prev2Btns)));//invert toggle2Btns when the button goes from low to high //===============================Drive=============================== #if single_joystick_drive if(1){//circle clamping float magnitude = sqrt(sq(left_drive_control)+sq(rght_drive_control));//the magnitude of the joystick vector if(magnitude >= threshold) { //maximum speed*(the magnitude of the joystick mapped from threshold to 1 to 0 to 1)*(the normalized joystick) //= maximum speed*(fraction speed)*(direction) float speed = (magnitude-threshold)/(1-threshold); speed = speed_mod*quadBezier(speed, min_drive, bezier_drive_control, max_drive); int left_vIs = clamp(speed*(left_drive_control/magnitude), -100, 100); int right_vIs = clamp(speed*(rght_drive_control/magnitude), -100, 100); motor[driveL] = left_vIs; motor[driveR] = right_vIs; } else { motor[driveL] = 0; motor[driveR] = 0; } if(fast_button){ if(speed_mod != 1.25){ speed_mod = 1.25; } else{ speed_mod = 1; } } else if(slow_button){ if(speed_mod != 0.4){ speed_mod = 0.4; } else{ speed_mod = 1; } } } #else {//dual joystick drive, square clamping int l = max_drive*thresholdify(left_drive_control); int r = max_drive*thresholdify(rght_drive_control); motor[driveL] = l; motor[driveR] = r; } #endif //==============================Launcher============================= int dt = time1[T4]; clearTimer(T4); const int unjam_total = 500; if(launcher_unjam || jam_time >= unjam_wait) { jam_time = 0; unjam_time = unjam_total; intake_on = 0; nMotorEncoder[launcher] = 0; } if(unjam_time > 0) { motor[intake] = 0; if(nMotorEncoder[launcher] > -100) { motor[launcher] = -40;//Fix timer reset, stop intake when jammed } else { motor[launcher] = 0; } unjam_time -= dt; } else { //===============t================Intake============================== if(intake_back_control) { motor[intake] = -55; } else { motor[intake] = intake_control*70; } launcher_time -= dt; if(launcher_control) { old_launcher_position = new_launcher_position; new_launcher_position = nMotorEncoder[launcher]; if(new_launcher_position-old_launcher_position <= 1) { jam_time += dt; } else { jam_time = 0; } launcher_time = launcher_slow_time; } else { jam_time = 0; } float new_launcher_power = lerp(((float)launcher_time)/launcher_slow_time, 0, 100); if(new_launcher_power < 0) { new_launcher_power = 0; } motor[launcher] = new_launcher_power; } //================================Lift=============================== float lift_vel = deadzone(joystick.joy2_y1)*100.0/128.0; motor[liftL] = lift_vel; //===========================Goal Mechanism========================== if(goal_open_btn){ goal_down = 0; } if(goal_close_btn){ goal_down = 1; } /*if(goal_toggle) { goal_down = !goal_down; }*/ servo[goal] = goal_open + (goal_close-goal_open)*goal_down; servo[side_lock] = side_open + (side_close-side_open)*side_lock_btn; //===========================Net Mechanism========================== if(net_open_btn){ net_down = 0; } if(net_close_btn){ net_down = 1; } if(net_small_btn){ net_down = 2; } if(net_pressed) { net_down = !net_down; } const unsigned short net_positions[3] = {net_open, net_close, net_small}; servo[net] = net_positions[net_down]; //===============================Shrub=============================== if(!checkConnection())//Prevent damage upon fcs failure, MAKE SURE THIS IS WORKING. IT COULD COST US A MATCH { servo[shrub] = 100 * joy1toggle(btnX) + 127; } } }
const T expo_deadzone(const T &value, const T &e, const T &dz) { return expo(deadzone(value, dz), e); }
inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz) { return expo(deadzone(value, dz), e); }