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);
}
