2021-03-31 17:17:43 +02:00
/* Copyright (C) 2013-2015 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 ( GPL2 ) as published by the Free Software
Foundation and appearing in the file GPL2 . TXT included in the packaging of
this file . Please note that GPL2 Section 2 [ b ] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ( " Copyleft " ) .
Contact information
- - - - - - - - - - - - - - - - - - -
Kristian Lauszus , TKJ Electronics
Web : http : //www.tkjelectronics.dk
e - mail : kristianl @ tkjelectronics . dk
*/
# if defined(ENABLE_USB) || defined(ENABLE_SPEKTRUM)
static uint8_t ps3OldLed , wiiOldLed , ps4OldBatteryLevel ;
# ifdef ENABLE_XBOX
static LEDEnum xboxOldLed ;
# endif
# ifdef ENABLE_SPP
void readSPPData ( ) {
if ( SerialBT . connected ) { // The SPP connection won't return data as fast as the controllers, so we will handle it separately
if ( SerialBT . available ( ) ) {
uint8_t i = 0 ;
while ( 1 ) {
dataInput [ i ] = SerialBT . read ( ) ;
if ( dataInput [ i ] = = - 1 ) // Error while reading the string
return ;
if ( dataInput [ i ] = = ' ; ' ) // Keep reading until it reads a semicolon
break ;
if ( + + i > = sizeof ( dataInput ) / sizeof ( dataInput [ 0 ] ) ) // String is too long
return ;
}
bluetoothData = true ;
setValues ( dataInput ) ;
}
}
}
# endif // ENABLE_SPP
void readUsb ( ) {
# ifdef ENABLE_USB
Usb . Task ( ) ; // The SPP data is actually not send until this is called, one could call SerialBT.send() directly as well
if ( Usb . getUsbTaskState ( ) = = USB_STATE_ERROR & & layingDown ) { // Check if the USB state machine is in an error state, but also make sure the robot is laying down
Serial . println ( F ( " USB fail " ) ) ;
Usb . vbusPower ( vbus_off ) ;
delay ( 1000 ) ;
Usb . vbusPower ( vbus_on ) ;
Usb . setUsbTaskState ( USB_DETACHED_SUBSTATE_WAIT_FOR_DEVICE ) ; // Reset state machine
}
# endif // ENABLE_USB
# ifdef ENABLE_SPP
readSPPData ( ) ;
# endif // ENABLE_SPP
if ( millis ( ) > ( receiveControlTimer + receiveControlTimeout ) ) {
commandSent = false ; // We use this to detect when there has already been sent a command by one of the controllers
# ifdef ENABLE_PS3
if ( PS3 . PS3Connected ) {
if ( PS3 . getButtonPress ( SELECT ) ) {
stopAndReset ( ) ;
while ( ! PS3 . getButtonPress ( START ) )
Usb . Task ( ) ;
}
else if ( PS3 . getButtonPress ( CROSS ) | | ( PS3 . getAnalogHat ( LeftHatY ) < 117 | | PS3 . getAnalogHat ( RightHatY ) < 117 | | PS3 . getAnalogHat ( LeftHatY ) > 137 | | PS3 . getAnalogHat ( RightHatY ) > 137 ) )
steer ( updatePS3 ) ;
} else if ( PS3 . PS3NavigationConnected & & ( PS3 . getAnalogHat ( LeftHatX ) > 200 | | PS3 . getAnalogHat ( LeftHatX ) < 55 | | PS3 . getAnalogHat ( LeftHatY ) > 137 | | PS3 . getAnalogHat ( LeftHatY ) < 117 ) )
steer ( updatePS3 ) ;
# endif // ENABLE_PS3
# ifdef ENABLE_PS4
if ( PS4 . connected ( ) & & ! commandSent ) {
if ( PS4 . getButtonPress ( SHARE ) ) {
stopAndReset ( ) ;
while ( ! PS4 . getButtonPress ( OPTIONS ) )
Usb . Task ( ) ;
}
else if ( PS4 . getButtonPress ( CROSS ) | | PS4 . getAnalogHat ( LeftHatY ) < 117 | | PS4 . getAnalogHat ( RightHatY ) < 117 | | PS4 . getAnalogHat ( LeftHatY ) > 137 | | PS4 . getAnalogHat ( RightHatY ) > 137 )
steer ( updatePS4 ) ;
}
# endif // ENABLE_PS4
# ifdef ENABLE_WII
if ( Wii . wiimoteConnected & & ! Wii . wiiUProControllerConnected & & ! commandSent ) {
if ( Wii . getButtonPress ( B ) | | ( Wii . nunchuckConnected & & ( Wii . getAnalogHat ( HatX ) > 137 | | Wii . getAnalogHat ( HatX ) < 117 | | Wii . getAnalogHat ( HatY ) > 137 | | Wii . getAnalogHat ( HatY ) < 117 ) ) )
steer ( updateWii ) ;
} else if ( Wii . wiiUProControllerConnected & & ! commandSent ) { // The Wii U Pro Controller Joysticks has an range from approximately 800-3200
if ( Wii . getButtonPress ( MINUS ) ) {
stopAndReset ( ) ;
while ( ! Wii . getButtonPress ( PLUS ) )
Usb . Task ( ) ;
}
else if ( Wii . getAnalogHat ( LeftHatY ) > 2200 | | Wii . getAnalogHat ( LeftHatY ) < 1800 | | Wii . getAnalogHat ( RightHatY ) > 2200 | | Wii . getAnalogHat ( RightHatY ) < 1800 )
steer ( updateWii ) ;
}
# endif // ENABLE_WII
# ifdef ENABLE_XBOX
if ( Xbox . Xbox360Connected [ 0 ] & & ! commandSent ) { // We will only read from the first controller, up to four is supported by one receiver
if ( Xbox . getButtonPress ( BACK ) ) {
stopAndReset ( ) ;
while ( ! Xbox . getButtonPress ( START ) )
Usb . Task ( ) ;
}
else if ( Xbox . getAnalogHat ( LeftHatY ) < - 7500 | | Xbox . getAnalogHat ( RightHatY ) < - 7500 | | Xbox . getAnalogHat ( LeftHatY ) > 7500 | | Xbox . getAnalogHat ( RightHatY ) > 7500 )
steer ( updateXbox ) ;
}
# endif // ENABLE_XBOX
# ifdef ENABLE_SPEKTRUM
if ( spekConnected ) {
if ( millis ( ) - spekConnectedTimer > 100 ) // If it has been more than 100ms since last data, then it must be disconnected
spekConnected = false ;
else {
if ( ! commandSent & & ( rcValue [ RC_CHAN_ROLL ] < 1490 | | rcValue [ RC_CHAN_ROLL ] > 1510 | | rcValue [ RC_CHAN_PITCH ] < 1490 | | rcValue [ RC_CHAN_PITCH ] > 1510 ) )
steer ( updateSpektrum ) ;
}
}
# endif // ENABLE_SPEKTRUM
if ( ! commandSent ) // If there hasn't been send a command by now, then send stop
steer ( stop ) ;
}
# ifdef ENABLE_PS3
if ( PS3 . PS3Connected | | PS3 . PS3NavigationConnected ) {
if ( PS3 . getButtonClick ( PS ) ) {
PS3 . disconnect ( ) ;
ps3OldLed = 0 ; // Reset value
}
}
# endif // ENABLE_PS3
# ifdef ENABLE_PS4
if ( PS4 . connected ( ) ) {
if ( PS4 . getButtonClick ( PS ) ) {
PS4 . disconnect ( ) ;
ps4OldBatteryLevel = 0 ; // Reset value
}
}
# endif // ENABLE_PS4
# ifdef ENABLE_WII
if ( Wii . wiimoteConnected | | Wii . wiiUProControllerConnected ) {
if ( Wii . getButtonClick ( HOME ) ) {
Wii . disconnect ( ) ;
wiiOldLed = 0 ; // Reset value
}
}
# endif // ENABLE_WII
# ifdef ENABLE_XBOX
if ( Xbox . Xbox360Connected [ 0 ] ) {
if ( Xbox . getButtonClick ( XBOX ) ) {
Xbox . disconnect ( ) ;
xboxOldLed = OFF ; // Reset value
}
}
# endif // ENABLE_XBOX
# if defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
if ( millis ( ) - ledTimer > 1000 ) { // Update every 1s
ledTimer = millis ( ) ;
updateLEDs ( ) ;
}
# endif // defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
}
# if defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
void updateLEDs ( ) {
uint8_t Led ;
# ifdef ENABLE_PS3
if ( PS3 . PS3Connected ) {
if ( ps3RumbleEnable ) {
ps3RumbleEnable = false ;
PS3 . setRumbleOn ( RumbleLow ) ;
ps3RumbleDisable = true ;
ledTimer - = 500 ; // Turn off again in 500ms
} else if ( ps3RumbleDisable ) {
ps3RumbleDisable = false ;
PS3 . setRumbleOff ( ) ;
ps3OldLed = 0 ; // Reset value
} else {
if ( PS3 . getStatus ( Shutdown ) ) { // Blink all LEDs
if ( ps3OldLed )
Led = 0x00 ; // All off
else
Led = 0x0F ; // All on
}
else if ( PS3 . getStatus ( Dying ) )
Led = 0x01 ; // LED1 on
else if ( PS3 . getStatus ( Low ) )
Led = 0x03 ; // LED1 and LED2 on
else if ( PS3 . getStatus ( High ) )
Led = 0x07 ; // LED1, LED2 and LED3 on
else if ( PS3 . getStatus ( Full ) )
Led = 0x0F ; // LED1, LED2, LED3 and LED4 on
else if ( PS3 . getStatus ( Charging ) )
Led = ( ps3OldLed = = 0x0F ? 0x01 : ( ps3OldLed < < 1 | 1 ) & 0x0F ) ; // Indicate charging using the LEDs
else
Led = ps3OldLed ;
if ( Led ! = ps3OldLed ) {
ps3OldLed = Led ;
PS3 . setLedRaw ( Led ) ;
}
}
} else if ( PS3 . PS3NavigationConnected ) {
if ( PS3 . getStatus ( Shutdown ) )
PS3 . setLedToggle ( LED1 ) ; // Blink LED
else {
Led = 0x01 ; // LED on
if ( Led ! = ps3OldLed ) {
ps3OldLed = Led ;
PS3 . setLedRaw ( Led ) ;
}
}
}
# endif // ENABLE_PS3
# ifdef ENABLE_PS4
if ( PS4 . connected ( ) ) {
if ( ps4RumbleEnabled ) {
ps4RumbleEnabled = false ;
PS4 . setRumbleOff ( ) ;
ps4OldBatteryLevel = 0 ; // Reset value
} else {
uint8_t ps4BatteryLevel = PS4 . getBatteryLevel ( ) ;
if ( ps4BatteryLevel ! = ps4OldBatteryLevel ) {
ps4OldBatteryLevel = ps4BatteryLevel ;
PS4 . setLed ( 0 , 0 , ps4BatteryLevel * 17 ) ; // The battery status is in the range from 0-15, by multiplying we get it in the range 0-255
if ( ps4BatteryLevel < 2 )
PS4 . setLedFlash ( 10 , 10 ) ; // Blink rapidly
else
PS4 . setLedFlash ( 0xFF , 0 ) ; // Turn on
}
}
}
# endif // ENABLE_PS4
# ifdef ENABLE_WII
if ( Wii . wiimoteConnected | | Wii . wiiUProControllerConnected ) {
if ( wiiRumbleEnabled ) {
wiiRumbleEnabled = false ;
Wii . setRumbleOff ( ) ;
wiiOldLed = 0 ; // Reset value
} else {
uint8_t batteryLevel = Wii . getBatteryLevel ( ) ;
if ( batteryLevel < 60 ) { // Blink all LEDs
if ( wiiOldLed )
Led = 0x00 ; // All off
else
Led = 0xF0 ; // All on
}
else if ( batteryLevel < 110 )
Led = 0x10 ; // LED1 on
else if ( batteryLevel < 160 )
Led = 0x30 ; // LED1 and LED2 on
else if ( batteryLevel < 210 )
Led = 0x70 ; // LED1, LED2 and LED3 on
else
Led = 0xF0 ; // LED1, LED2, LED3 and LED4 on
if ( Led ! = wiiOldLed ) {
wiiOldLed = Led ;
Wii . setLedRaw ( Led ) ;
}
}
}
# endif // ENABLE_WII
# ifdef ENABLE_XBOX
if ( Xbox . Xbox360Connected [ 0 ] ) {
if ( xboxRumbleDisable ) {
xboxRumbleDisable = false ;
Xbox . setRumbleOff ( ) ;
xboxOldLed = OFF ; // Reset value
} else {
uint8_t batteryLevel = Xbox . getBatteryLevel ( ) ;
LEDEnum xboxLed ;
if ( batteryLevel = = 0 )
xboxLed = LED1 ;
else if ( batteryLevel = = 1 )
xboxLed = LED2 ;
else if ( batteryLevel = = 2 )
xboxLed = LED3 ;
else
xboxLed = LED4 ;
if ( xboxLed ! = xboxOldLed ) {
xboxOldLed = xboxLed ;
Xbox . setLedOn ( xboxLed ) ;
}
}
}
# endif // ENABLE_XBOX
}
# ifdef ENABLE_PS3
void onInitPS3 ( ) { // This function is called when the controller is first initialized
if ( PS3 . PS3Connected ) { // We only check the normal PS3 controller as the Navigation controller doesn't have rumble support
updateLEDs ( ) ; // Turn the LEDs on according to the voltage level
ledTimer = millis ( ) - 500 ; // Wait 500ms before turning rumble on
ps3RumbleEnable = true ; // We can't sent commands to the PS3 controller that often, so we don't send the command here
}
}
# endif // ENABLE_PS3
# ifdef ENABLE_PS4
void onInitPS4 ( ) { // This function is called when the controller is first initialized
if ( PS4 . connected ( ) ) { // Check if the PS4 controller is connected
updateLEDs ( ) ; // Turn the LEDs on according to the voltage level
ledTimer = millis ( ) - 500 ; // This will turn the rumble off again after 500ms
PS4 . setRumbleOn ( RumbleLow ) ;
ps4RumbleEnabled = true ;
}
}
# endif // ENABLE_PS4
# ifdef ENABLE_WII
void onInitWii ( ) { // This function is called when the controller is first initialized
if ( Wii . wiimoteConnected ) { // Both the Wiimote and the Wii U Pro Controller
updateLEDs ( ) ; // Turn the LEDs on according to the voltage level
ledTimer = millis ( ) - 500 ; // This will turn the rumble off again after 500ms
Wii . setRumbleOn ( ) ;
wiiRumbleEnabled = true ;
}
}
# endif // ENABLE_WII
# ifdef ENABLE_XBOX
void onInitXbox ( ) { // This function is called when the controller is first initialized
if ( Xbox . Xbox360Connected [ 0 ] ) { // Xbox wireless controller
ledTimer = millis ( ) - 500 ; // This will turn the rumble off again after 500ms
Xbox . setRumbleOn ( 0x00 , 0xFF ) ;
xboxRumbleDisable = true ;
}
}
# endif // ENABLE_XBOX
# endif // defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX)
# endif // defined(ENABLE_USB) || defined(ENABLE_SPEKTRUM)
# if defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX) || defined(ENABLE_TOOLS) || defined(ENABLE_SPEKTRUM)
void steer ( Command command ) {
commandSent = true ; // Used to see if there has already been send a command or not
steerStop = false ;
targetOffset = turningOffset = 0 ; // Set both to 0
# if defined(ENABLE_SPP) || defined(ENABLE_TOOLS)
if ( command = = joystick ) {
if ( sppData2 > 0 ) // Forward
targetOffset = scale ( sppData2 , 0 , 1 , 0 , cfg . controlAngleLimit ) ;
else if ( sppData2 < 0 ) // Backward
targetOffset = - scale ( sppData2 , 0 , - 1 , 0 , cfg . controlAngleLimit ) ;
if ( sppData1 > 0 ) // Right
turningOffset = scale ( sppData1 , 0 , 1 , 0 , cfg . turningLimit ) ;
else if ( sppData1 < 0 ) // Left
turningOffset = - scale ( sppData1 , 0 , - 1 , 0 , cfg . turningLimit ) ;
} else if ( command = = imu ) {
if ( sppData1 > 0 ) // Forward
targetOffset = scale ( sppData1 , 0 , 36 , 0 , cfg . controlAngleLimit ) ;
else if ( sppData1 < 0 ) // Backward
targetOffset = - scale ( sppData1 , 0 , - 36 , 0 , cfg . controlAngleLimit ) ;
if ( sppData2 > 0 ) // Right
turningOffset = scale ( sppData2 , 0 , 45 , 0 , cfg . turningLimit ) ;
else if ( sppData2 < 0 ) // Left
turningOffset = - scale ( sppData2 , 0 , - 45 , 0 , cfg . turningLimit ) ;
}
# endif // ENABLE_SPP or ENABLE_TOOLS
# ifdef ENABLE_PS3
if ( command = = updatePS3 ) {
if ( PS3 . PS3Connected ) {
if ( PS3 . getButtonPress ( CROSS ) ) {
if ( PS3 . getAngle ( Pitch ) > 180 ) // Forward
targetOffset = scale ( PS3 . getAngle ( Pitch ) , 180 , 216 , 0 , cfg . controlAngleLimit ) ;
else if ( PS3 . getAngle ( Pitch ) < 180 ) // Backward
targetOffset = - scale ( PS3 . getAngle ( Pitch ) , 180 , 144 , 0 , cfg . controlAngleLimit ) ;
if ( PS3 . getAngle ( Roll ) > 180 ) // Right
turningOffset = scale ( PS3 . getAngle ( Roll ) , 180 , 225 , 0 , cfg . turningLimit ) ;
else if ( PS3 . getAngle ( Roll ) < 180 ) // Left
turningOffset = - scale ( PS3 . getAngle ( Roll ) , 180 , 135 , 0 , cfg . turningLimit ) ;
} else {
if ( PS3 . getAnalogHat ( LeftHatY ) < 117 & & PS3 . getAnalogHat ( RightHatY ) < 117 ) // Forward
targetOffset = scale ( PS3 . getAnalogHat ( LeftHatY ) + PS3 . getAnalogHat ( RightHatY ) , 232 , 0 , 0 , cfg . controlAngleLimit ) ; // Scale from 232-0 to 0-controlAngleLimit
else if ( PS3 . getAnalogHat ( LeftHatY ) > 137 & & PS3 . getAnalogHat ( RightHatY ) > 137 ) // Backward
targetOffset = - scale ( PS3 . getAnalogHat ( LeftHatY ) + PS3 . getAnalogHat ( RightHatY ) , 276 , 510 , 0 , cfg . controlAngleLimit ) ; // Scale from 276-510 to 0-controlAngleLimit
if ( ( ( int16_t ) PS3 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS3 . getAnalogHat ( RightHatY ) ) > 15 ) // Left
turningOffset = - scale ( abs ( ( int16_t ) PS3 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS3 . getAnalogHat ( RightHatY ) ) , 0 , 255 , 0 , cfg . turningLimit ) ; // Scale from 0-255 to 0-turningLimit
else if ( ( ( int16_t ) PS3 . getAnalogHat ( RightHatY ) - ( int16_t ) PS3 . getAnalogHat ( LeftHatY ) ) > 15 ) // Right
turningOffset = scale ( abs ( ( int16_t ) PS3 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS3 . getAnalogHat ( RightHatY ) ) , 0 , 255 , 0 , cfg . turningLimit ) ; // Scale from 0-255 to 0-turningLimit
}
} else { // It must be a Navigation controller then
if ( PS3 . getAnalogHat ( LeftHatY ) < 117 ) // Forward
targetOffset = scale ( PS3 . getAnalogHat ( LeftHatY ) , 116 , 0 , 0 , cfg . controlAngleLimit ) ; // Scale from 116-0 to 0-controlAngleLimit
else if ( PS3 . getAnalogHat ( LeftHatY ) > 137 ) // Backward
targetOffset = - scale ( PS3 . getAnalogHat ( LeftHatY ) , 138 , 255 , 0 , cfg . controlAngleLimit ) ; // Scale from 138-255 to 0-controlAngleLimit
if ( PS3 . getAnalogHat ( LeftHatX ) < 55 ) // Left
turningOffset = - scale ( PS3 . getAnalogHat ( LeftHatX ) , 54 , 0 , 0 , cfg . turningLimit ) ; // Scale from 54-0 to 0-turningLimit
else if ( PS3 . getAnalogHat ( LeftHatX ) > 200 ) // Right
turningOffset = scale ( PS3 . getAnalogHat ( LeftHatX ) , 201 , 255 , 0 , cfg . turningLimit ) ; // Scale from 201-255 to 0-turningLimit
}
}
# endif // ENABLE_PS3
# ifdef ENABLE_PS4
if ( command = = updatePS4 ) {
if ( PS4 . getButtonPress ( CROSS ) ) {
if ( PS4 . getAngle ( Pitch ) > 180 ) // Forward
targetOffset = scale ( PS4 . getAngle ( Pitch ) , 180 , 216 , 0 , cfg . controlAngleLimit ) ;
else if ( PS4 . getAngle ( Pitch ) < 180 ) // Backward
targetOffset = - scale ( PS4 . getAngle ( Pitch ) , 180 , 144 , 0 , cfg . controlAngleLimit ) ;
if ( PS4 . getAngle ( Roll ) < 180 ) // Right
turningOffset = scale ( PS4 . getAngle ( Roll ) , 180 , 135 , 0 , cfg . turningLimit ) ;
else if ( PS4 . getAngle ( Roll ) > 180 ) // Left
turningOffset = - scale ( PS4 . getAngle ( Roll ) , 180 , 225 , 0 , cfg . turningLimit ) ;
} else {
if ( PS4 . getAnalogHat ( LeftHatY ) < 117 & & PS4 . getAnalogHat ( RightHatY ) < 117 ) // Forward
targetOffset = scale ( PS4 . getAnalogHat ( LeftHatY ) + PS4 . getAnalogHat ( RightHatY ) , 232 , 0 , 0 , cfg . controlAngleLimit ) ; // Scale from 232-0 to 0-controlAngleLimit
else if ( PS4 . getAnalogHat ( LeftHatY ) > 137 & & PS4 . getAnalogHat ( RightHatY ) > 137 ) // Backward
targetOffset = - scale ( PS4 . getAnalogHat ( LeftHatY ) + PS4 . getAnalogHat ( RightHatY ) , 276 , 510 , 0 , cfg . controlAngleLimit ) ; // Scale from 276-510 to 0-controlAngleLimit
if ( ( ( int16_t ) PS4 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS4 . getAnalogHat ( RightHatY ) ) > 15 ) // Left
turningOffset = - scale ( abs ( ( int16_t ) PS4 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS4 . getAnalogHat ( RightHatY ) ) , 0 , 255 , 0 , cfg . turningLimit ) ; // Scale from 0-255 to 0-turningLimit
else if ( ( ( int16_t ) PS4 . getAnalogHat ( RightHatY ) - ( int16_t ) PS4 . getAnalogHat ( LeftHatY ) ) > 15 ) // Right
turningOffset = scale ( abs ( ( int16_t ) PS4 . getAnalogHat ( LeftHatY ) - ( int16_t ) PS4 . getAnalogHat ( RightHatY ) ) , 0 , 255 , 0 , cfg . turningLimit ) ; // Scale from 0-255 to 0-turningLimit
}
}
# endif // ENABLE_PS4
# ifdef ENABLE_WII
if ( command = = updateWii ) {
if ( ! Wii . wiiUProControllerConnected ) {
if ( Wii . getButtonPress ( B ) ) {
if ( Wii . getPitch ( ) > 180 ) // Forward
targetOffset = scale ( Wii . getPitch ( ) , 180 , 216 , 0 , cfg . controlAngleLimit ) ;
else if ( Wii . getPitch ( ) < 180 ) // Backward
targetOffset = - scale ( Wii . getPitch ( ) , 180 , 144 , 0 , cfg . controlAngleLimit ) ;
if ( Wii . getRoll ( ) > 180 ) // Right
turningOffset = scale ( Wii . getRoll ( ) , 180 , 225 , 0 , cfg . turningLimit ) ;
else if ( Wii . getRoll ( ) < 180 ) // Left
turningOffset = - scale ( Wii . getRoll ( ) , 180 , 135 , 0 , cfg . turningLimit ) ;
}
else { // Read the Nunchuck controller
if ( Wii . getAnalogHat ( HatY ) > 137 ) // Forward
targetOffset = scale ( Wii . getAnalogHat ( HatY ) , 138 , 230 , 0 , cfg . controlAngleLimit ) ;
else if ( Wii . getAnalogHat ( HatY ) < 117 ) // Backward
targetOffset = - scale ( Wii . getAnalogHat ( HatY ) , 116 , 25 , 0 , cfg . controlAngleLimit ) ;
if ( Wii . getAnalogHat ( HatX ) > 137 ) // Right
turningOffset = scale ( Wii . getAnalogHat ( HatX ) , 138 , 230 , 0 , cfg . turningLimit ) ;
else if ( Wii . getAnalogHat ( HatX ) < 117 ) // Left
turningOffset = - scale ( Wii . getAnalogHat ( HatX ) , 116 , 25 , 0 , cfg . turningLimit ) ;
}
} else { // It must be a Wii U Pro Controller then
if ( Wii . getAnalogHat ( LeftHatY ) > 2200 & & Wii . getAnalogHat ( RightHatY ) > 2200 ) // Forward
targetOffset = scale ( Wii . getAnalogHat ( LeftHatY ) + Wii . getAnalogHat ( RightHatY ) , 4402 , 6400 , 0 , cfg . controlAngleLimit ) ; // Scale from 4402-6400 to 0-controlAngleLimit
else if ( Wii . getAnalogHat ( LeftHatY ) < 1800 & & Wii . getAnalogHat ( RightHatY ) < 1800 ) // Backward
targetOffset = - scale ( Wii . getAnalogHat ( LeftHatY ) + Wii . getAnalogHat ( RightHatY ) , 3598 , 1600 , 0 , cfg . controlAngleLimit ) ; // Scale from 3598-1600 to 0-controlAngleLimit
if ( ( ( int16_t ) Wii . getAnalogHat ( RightHatY ) - ( int16_t ) Wii . getAnalogHat ( LeftHatY ) ) > 200 ) // Left
turningOffset = - scale ( abs ( ( int16_t ) Wii . getAnalogHat ( LeftHatY ) - ( int16_t ) Wii . getAnalogHat ( RightHatY ) ) , 0 , 2400 , 0 , cfg . turningLimit ) ; // Scale from 0-2400 to 0-turningLimit
else if ( ( ( int16_t ) Wii . getAnalogHat ( LeftHatY ) - ( int16_t ) Wii . getAnalogHat ( RightHatY ) ) > 200 ) // Right
turningOffset = scale ( abs ( ( int16_t ) Wii . getAnalogHat ( LeftHatY ) - ( int16_t ) Wii . getAnalogHat ( RightHatY ) ) , 0 , 2400 , 0 , cfg . turningLimit ) ; // Scale from 0-2400 to 0-turningLimit
}
}
# endif // ENABLE_WII
# ifdef ENABLE_XBOX
if ( command = = updateXbox ) {
if ( Xbox . getAnalogHat ( LeftHatY ) > 7500 & & Xbox . getAnalogHat ( RightHatY ) > 7500 ) // Forward
targetOffset = scale ( ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) + ( int32_t ) Xbox . getAnalogHat ( RightHatY ) , 15002 , 65534 , 0 , cfg . controlAngleLimit ) ; // Scale from 15002-65534 to 0-controlAngleLimit
else if ( Xbox . getAnalogHat ( LeftHatY ) < - 7500 & & Xbox . getAnalogHat ( RightHatY ) < - 7500 ) // Backward
targetOffset = - scale ( ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) + ( int32_t ) Xbox . getAnalogHat ( RightHatY ) , - 15002 , - 65536 , 0 , cfg . controlAngleLimit ) ; // Scale from -15002-(-65536) to 0-controlAngleLimit
if ( ( ( int32_t ) Xbox . getAnalogHat ( RightHatY ) - ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) ) > 7500 ) // Left
turningOffset = - scale ( abs ( ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) - ( int32_t ) Xbox . getAnalogHat ( RightHatY ) ) , 0 , 65535 , 0 , cfg . turningLimit ) ; // Scale from 0-65535 to 0-turningLimit
else if ( ( ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) - ( int32_t ) Xbox . getAnalogHat ( RightHatY ) ) > 7500 ) // Right
turningOffset = scale ( abs ( ( int32_t ) Xbox . getAnalogHat ( LeftHatY ) - ( int32_t ) Xbox . getAnalogHat ( RightHatY ) ) , 0 , 65535 , 0 , cfg . turningLimit ) ; // Scale from 0-65535 to 0-turningLimit
}
# endif // ENABLE_XBOX
# ifdef ENABLE_SPEKTRUM
if ( command = = updateSpektrum ) {
if ( rcValue [ RC_CHAN_PITCH ] > 1500 ) // Forward
targetOffset = scale ( rcValue [ RC_CHAN_PITCH ] , 1500 , 2000 , 0 , cfg . controlAngleLimit ) ;
else if ( rcValue [ RC_CHAN_PITCH ] < 1500 ) // Backward
targetOffset = - scale ( rcValue [ RC_CHAN_PITCH ] , 1500 , 1000 , 0 , cfg . controlAngleLimit ) ;
if ( rcValue [ RC_CHAN_ROLL ] < 1500 ) // Left
turningOffset = - scale ( rcValue [ RC_CHAN_ROLL ] , 1500 , 1000 , 0 , cfg . turningLimit ) ;
else if ( rcValue [ RC_CHAN_ROLL ] > 1500 ) // Right
turningOffset = scale ( rcValue [ RC_CHAN_ROLL ] , 1500 , 2000 , 0 , cfg . turningLimit ) ;
}
# endif // ENABLE_SPEKTRUM
if ( command = = stop ) {
steerStop = true ;
if ( lastCommand ! = stop ) { // Set new stop position
targetPosition = getWheelsPosition ( ) ;
stopped = false ;
}
}
lastCommand = command ;
}
float scale ( float input , float inputMin , float inputMax , float outputMin , float outputMax ) { // Like map() just returns a float
float output ;
if ( inputMin < inputMax )
output = ( input - inputMin ) / ( ( inputMax - inputMin ) / ( outputMax - outputMin ) ) ;
else
output = ( inputMin - input ) / ( ( inputMin - inputMax ) / ( outputMax - outputMin ) ) ;
if ( output > outputMax )
output = outputMax ;
else if ( output < outputMin )
output = outputMin ;
return output ;
}
# endif // defined(ENABLE_SPP) || defined(ENABLE_PS3) || defined(ENABLE_PS4) || defined(ENABLE_WII) || defined(ENABLE_XBOX) || defined(ENABLE_TOOLS) || defined(ENABLE_SPEKTRUM)