diff --git a/Readme.md b/Readme.md index 0bf9476..1a6e38b 100644 --- a/Readme.md +++ b/Readme.md @@ -2,12 +2,22 @@ ## Statement +### The v1.0.0 new version library is now available, with the following major updates: + +1.Fixed an issue where frame rates decrease in certain games after connecting a force feedback controller. Due to issues with the mechanism by which some games send force feedback data, the Arduino did not promptly notify the computer upon receiving the force feedback data, resulting in lost frames in these games. To resolve this problem, the reception of force feedback data has been moved to be handled within an ISR (Interrupt Service Routine) on the Arduino, which means that some interfaces have undergone changes compared to v0.9.0. Example code will be provided to demonstrate the corresponding modifications. This fix has been tested and confirmed working in games such as 'Horizon 4' and 'Horizon 5'. + +2.Corrected several errors in the force feedback calculation logic. + +3.A complete demonstration project has been provided for developers to reference. It can be found at the path: examples/YourFirstFFBController. + ### This is a joy library for Atmega32UX chip with force feedback, which can be used to make game handle with vibration, game steering wheel with force feedback, etc.Multi-axis-force-feedback feature is added. ## Usage -### 0. example：`examples/JoystickWithFFB/JoystickWithFFB.ino` +### 0. example： + +`examples\SingleJoystickFFB\SingleJoystickFFB.ino` ### 1. create `JoyStick` object @@ -60,7 +70,7 @@ struct Gains{ */ int8_t setGains(Gains* _gains); ``` -#### example code +#### example code p1 ``` #include "Joystick.h" @@ -118,7 +128,7 @@ struct EffectParams{ int8_t setEffectParams(EffectParams* _effect_params); ``` -#### example code +#### example code p2 ``` #include "Joystick.h" @@ -150,13 +160,12 @@ void loop(){ myeffectparams[1].springPosition = analogRead(A3); Joystick.setEffectParams(myeffectparams); - Joystick.getForce(forces); } ``` -### 4.Finally,get the force value with +### 4.Finally,open ISR to recevie the PID data, than get the force value with `JoyStick.getForce(int32_t* forces)` @@ -170,10 +179,74 @@ return type `void` range`[-255,255]` +#### example code p3 +``` +#include "Joystick.h" + +Joystick_ Joystick(); + +int32_t forces[2]={0}; + +Gains mygains[2]; +EffectParams myeffectparams[2]; + +//set X Axis gains +mygains[0].totalGain = 50; +mygains[0].springGain = 80; + +//set Y Axis gains +mygains[1].totalGain = 50; +mygains[1].springGain = 70; + +void setup(){ + //enable gains REQUIRED + Joystick.setGains(mygains); + + //set Timer3 + cli(); + TCCR3A = 0; //set TCCR1A 0 + TCCR3B = 0; //set TCCR1B 0 + TCNT3 = 0; //counter init + OCR3A = 399; + TCCR3B |= (1 << WGM32); //open CTC mode + TCCR3B |= (1 << CS31); //set CS11 1(8-fold Prescaler) + TIMSK3 |= (1 << OCIE3A); + sei(); +} + +//ISR +ISR(TIMER3_COMPA_vect){ + Joystick.getUSBPID(); +} + +void loop(){ + //set X Axis Spring Effect Param + myeffectparams[0].springMaxPosition = 1023; + myeffectparams[0].springPosition = analogRead(A2); + + //set Y Axis Spring Effect Param + myeffectparams[1].springMaxPosition = 1023; + myeffectparams[1].springPosition = analogRead(A3); + + Joystick.setEffectParams(myeffectparams); + + Joystick.getForce(forces); + Serial.print(forces[0]); + Serial.print(" "); + Serial.println(forces[1]); +} + +``` + #### **Pay Attention!** **`Joystick.setGains(mygains)` and `Joystick.setEffectParams(myeffectparams)` must be invoked before `JoyStick.getForce(int32_t* forces)`** +### 5.DEMO +`examples/YourFirstFFBController` + +![diagramm](examples/YourFirstFFBController/ffbcontroller.png) + ## Ref ### This library is based on [Heironimus](https://github.com/MHeironimus/ArduinoJoystickLibrary) and [hoantv](https://github.com/hoantv/VNWheel) 's work，very grateful for their work. diff --git a/examples/SingleJoystickFFB/SingleJoystickFFB.ino b/examples/SingleJoystickFFB/SingleJoystickFFB.ino index 604fba1..dd08a77 100644 --- a/examples/SingleJoystickFFB/SingleJoystickFFB.ino +++ b/examples/SingleJoystickFFB/SingleJoystickFFB.ino @@ -25,6 +25,20 @@ void setup(){ //enable gains REQUIRED Joystick.setGains(mygains); Joystick.begin(); + + cli(); + TCCR3A = 0; //set TCCR1A 0 + TCCR3B = 0; //set TCCR1B 0 + TCNT3 = 0; //counter init + OCR3A = 399; + TCCR3B |= (1 << WGM32); //open CTC mode + TCCR3B |= (1 << CS31); //set CS11 1(8-fold Prescaler) + TIMSK3 |= (1 << OCIE3A); + sei(); +} + +ISR(TIMER3_COMPA_vect){ + Joystick.getUSBPID(); } void loop(){ @@ -53,5 +67,4 @@ void loop(){ digitalWrite(7,LOW); analogWrite(9,abs(forces[0])); } - delay(1); } diff --git a/examples/YourFirstFFBController/DigitalWriteFast.h b/examples/YourFirstFFBController/DigitalWriteFast.h new file mode 100644 index 0000000..be6b7ec --- /dev/null +++ b/examples/YourFirstFFBController/DigitalWriteFast.h @@ -0,0 +1,394 @@ +/* + Optimized digital functions for AVR microcontrollers + by Watterott electronic (www.watterott.com) + based on http://code.google.com/p/digitalwritefast + */ + +#ifndef __digitalWriteFast_h_ +#define __digitalWriteFast_h_ 1 + +// general macros/defines +#ifndef BIT_READ +# define BIT_READ(value, bit) ((value) & (1UL << (bit))) +#endif +#ifndef BIT_SET +# define BIT_SET(value, bit) ((value) |= (1UL << (bit))) +#endif +#ifndef BIT_CLEAR +# define BIT_CLEAR(value, bit) ((value) &= ~(1UL << (bit))) +#endif +#ifndef BIT_WRITE +# define BIT_WRITE(value, bit, bitvalue) (bitvalue ? BIT_SET(value, bit) : BIT_CLEAR(value, bit)) +#endif + +#ifndef SWAP +#define SWAP(x,y) do{ (x)=(x)^(y); (y)=(x)^(y); (x)=(x)^(y); }while(0) +#endif + +#ifndef DEC +# define DEC (10) +#endif +#ifndef HEX +# define HEX (16) +#endif +#ifndef OCT +# define OCT (8) +#endif +#ifndef BIN +# define BIN (2) +#endif + + +// workarounds for ARM microcontrollers +#if (!defined(__AVR__) || \ + defined(ARDUINO_ARCH_SAM) || \ + defined(ARDUINO_ARCH_SAMD)) + +#ifndef PROGMEM +# define PROGMEM +#endif +#ifndef PGM_P +# define PGM_P const char * +#endif +#ifndef PSTR +# define PSTR(str) (str) +#endif + +#ifndef memcpy_P +# define memcpy_P(dest, src, num) memcpy((dest), (src), (num)) +#endif +#ifndef strcpy_P +# define strcpy_P(dst, src) strcpy((dst), (src)) +#endif +#ifndef strcat_P +# define strcat_P(dst, src) strcat((dst), (src)) +#endif +#ifndef strcmp_P +# define strcmp_P(a, b) strcmp((a), (b)) +#endif +#ifndef strcasecmp_P +# define strcasecmp_P(a, b) strcasecmp((a), (b)) +#endif +#ifndef strncmp_P +# define strncmp_P(a, b, n) strncmp((a), (b), (n)) +#endif +#ifndef strncasecmp_P +# define strncasecmp_P(a, b, n) strncasecmp((a), (b), (n)) +#endif +#ifndef strstr_P +# define strstr_P(a, b) strstr((a), (b)) +#endif +#ifndef strlen_P +# define strlen_P(a) strlen((a)) +#endif +#ifndef sprintf_P +# define sprintf_P(s, f, ...) sprintf((s), (f), __VA_ARGS__) +#endif + +#ifndef pgm_read_byte +# define pgm_read_byte(addr) (*(const unsigned char *)(addr)) +#endif +#ifndef pgm_read_word +# define pgm_read_word(addr) (*(const unsigned short *)(addr)) +#endif +#ifndef pgm_read_dword +# define pgm_read_dword(addr) (*(const unsigned long *)(addr)) +#endif + +#endif + + +// digital functions +//#ifndef digitalPinToPortReg +#define SPI_SW_SS_PIN (10) //SS on Uno (for software SPI) +#define SPI_SW_MOSI_PIN (11) //MOSI on Uno (for software SPI) +#define SPI_SW_MISO_PIN (12) //MISO on Uno (for software SPI) +#define SPI_SW_SCK_PIN (13) //SCK on Uno (for software SPI) + + +// --- Arduino Due and SAM3X8E based boards --- +#if (defined(ARDUINO_SAM_DUE) || \ + defined(__SAM3X8E__)) + +#define UART_RX_PIN (0) +#define UART_TX_PIN (1) + +#define I2C_SDA_PIN (20) +#define I2C_SCL_PIN (21) + +#define SPI_HW_SS_PIN (78) //SS0:77, SS1:87, SS2:86, SS3:78 +#define SPI_HW_MOSI_PIN (75) //75 +#define SPI_HW_MISO_PIN (74) //74 +#define SPI_HW_SCK_PIN (76) //76 + + +// --- Arduino Zero and SAMD21G18 based boards --- +#elif (defined(ARDUINO_SAMD_ZERO) || \ + defined(__SAMD21G18A__)) + +#define UART_RX_PIN (0) +#define UART_TX_PIN (1) + +#define I2C_SDA_PIN (16) +#define I2C_SCL_PIN (17) + +#define SPI_HW_SS_PIN (14) //14 +#define SPI_HW_MOSI_PIN (21) //21 +#define SPI_HW_MISO_PIN (18) //18 +#define SPI_HW_SCK_PIN (20) //20 + + +// --- Arduino Mega and ATmega128x/256x based boards --- +#elif (defined(ARDUINO_AVR_MEGA) || \ + defined(ARDUINO_AVR_MEGA1280) || \ + defined(ARDUINO_AVR_MEGA2560) || \ + defined(__AVR_ATmega1280__) || \ + defined(__AVR_ATmega1281__) || \ + defined(__AVR_ATmega2560__) || \ + defined(__AVR_ATmega2561__)) + +#define UART_RX_PIN (0) //PE0 +#define UART_TX_PIN (1) //PE1 + +#define I2C_SDA_PIN (20) +#define I2C_SCL_PIN (21) + +#define SPI_HW_SS_PIN (53) //PB0 +#define SPI_HW_MOSI_PIN (51) //PB2 +#define SPI_HW_MISO_PIN (50) //PB3 +#define SPI_HW_SCK_PIN (52) //PB1 + +#define __digitalPinToPortReg(P) \ +(((P) >= 22 && (P) <= 29) ? &PORTA : \ +((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PORTB : \ +(((P) >= 30 && (P) <= 37) ? &PORTC : \ +((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PORTD : \ +((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PORTE : \ +(((P) >= 54 && (P) <= 61) ? &PORTF : \ +((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PORTG : \ +((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PORTH : \ +(((P) == 14 || (P) == 15) ? &PORTJ : \ +(((P) >= 62 && (P) <= 69) ? &PORTK : &PORTL)))))))))) + +#define __digitalPinToDDRReg(P) \ +(((P) >= 22 && (P) <= 29) ? &DDRA : \ +((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &DDRB : \ +(((P) >= 30 && (P) <= 37) ? &DDRC : \ +((((P) >= 18 && (P) <= 21) || (P) == 38) ? &DDRD : \ +((((P) >= 0 && (P) <= 3) || (P) == 5) ? &DDRE : \ +(((P) >= 54 && (P) <= 61) ? &DDRF : \ +((((P) >= 39 && (P) <= 41) || (P) == 4) ? &DDRG : \ +((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &DDRH : \ +(((P) == 14 || (P) == 15) ? &DDRJ : \ +(((P) >= 62 && (P) <= 69) ? &DDRK : &DDRL)))))))))) + +#define __digitalPinToPINReg(P) \ +(((P) >= 22 && (P) <= 29) ? &PINA : \ +((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PINB : \ +(((P) >= 30 && (P) <= 37) ? &PINC : \ +((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PIND : \ +((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PINE : \ +(((P) >= 54 && (P) <= 61) ? &PINF : \ +((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PING : \ +((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PINH : \ +(((P) == 14 || (P) == 15) ? &PINJ : \ +(((P) >= 62 && (P) <= 69) ? &PINK : &PINL)))))))))) + +#define __digitalPinToBit(P) \ +(((P) >= 7 && (P) <= 9) ? (P) - 3 : \ +(((P) >= 10 && (P) <= 13) ? (P) - 6 : \ +(((P) >= 22 && (P) <= 29) ? (P) - 22 : \ +(((P) >= 30 && (P) <= 37) ? 37 - (P) : \ +(((P) >= 39 && (P) <= 41) ? 41 - (P) : \ +(((P) >= 42 && (P) <= 49) ? 49 - (P) : \ +(((P) >= 50 && (P) <= 53) ? 53 - (P) : \ +(((P) >= 54 && (P) <= 61) ? (P) - 54 : \ +(((P) >= 62 && (P) <= 69) ? (P) - 62 : \ +(((P) == 0 || (P) == 15 || (P) == 17 || (P) == 21) ? 0 : \ +(((P) == 1 || (P) == 14 || (P) == 16 || (P) == 20) ? 1 : \ +(((P) == 19) ? 2 : \ +(((P) == 5 || (P) == 6 || (P) == 18) ? 3 : \ +(((P) == 2) ? 4 : \ +(((P) == 3 || (P) == 4) ? 5 : 7))))))))))))))) + + +// --- Arduino 644 --- +#elif (defined(__AVR_ATmega644__) || \ + defined(__AVR_ATmega644P__)) + +#define UART_RX_PIN (8) //PD0 +#define UART_TX_PIN (9) //PD1 + +#define I2C_SDA_PIN (17) //PC1 +#define I2C_SCL_PIN (16) //PC0 + +#define SPI_HW_SS_PIN (4) //PB4 +#define SPI_HW_MOSI_PIN (5) //PB5 +#define SPI_HW_MISO_PIN (6) //PB6 +#define SPI_HW_SCK_PIN (7) //PB7 + +#define __digitalPinToPortReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PORTB : (((P) >= 8 && (P) <= 15) ? &PORTD : (((P) >= 16 && (P) <= 23) ? &PORTC : &PORTA))) +#define __digitalPinToDDRReg(P) \ +(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 8 && (P) <= 15) ? &DDRC : &DDRA))) +#define __digitalPinToPINReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 8 && (P) <= 15) ? &PINC : &PINA))) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (P) - 24))) + + +// --- Arduino Leonardo and ATmega16U4/32U4 based boards --- +#elif (defined(ARDUINO_AVR_LEONARDO) || \ + defined(__AVR_ATmega16U4__) || \ + defined(__AVR_ATmega32U4__)) + +#define UART_RX_PIN (0) //PD2 +#define UART_TX_PIN (1) //PD3 + +#define I2C_SDA_PIN (2) //PD1 +#define I2C_SCL_PIN (3) //PD0 + +#define SPI_HW_SS_PIN (17) //PB0 +#define SPI_HW_MOSI_PIN (16) //PB2 +#define SPI_HW_MISO_PIN (14) //PB3 +#define SPI_HW_SCK_PIN (15) //PB1 + +#define __digitalPinToPortReg(P) \ +((((P) >= 0 && (P) <= 4) || (P) == 6 || (P) == 12 || (P) == 24 || (P) == 25 || (P) == 29) ? &PORTD : (((P) == 5 || (P) == 13) ? &PORTC : (((P) >= 18 && (P) <= 23)) ? &PORTF : (((P) == 7) ? &PORTE : &PORTB))) +#define __digitalPinToDDRReg(P) \ +((((P) >= 0 && (P) <= 4) || (P) == 6 || (P) == 12 || (P) == 24 || (P) == 25 || (P) == 29) ? &DDRD : (((P) == 5 || (P) == 13) ? &DDRC : (((P) >= 18 && (P) <= 23)) ? &DDRF : (((P) == 7) ? &DDRE : &DDRB))) +#define __digitalPinToPINReg(P) \ +((((P) >= 0 && (P) <= 4) || (P) == 6 || (P) == 12 || (P) == 24 || (P) == 25 || (P) == 29) ? &PIND : (((P) == 5 || (P) == 13) ? &PINC : (((P) >= 18 && (P) <= 23)) ? &PINF : (((P) == 7) ? &PINE : &PINB))) +#define __digitalPinToBit(P) \ +(((P) >= 8 && (P) <= 11) ? (P) - 4 : (((P) >= 18 && (P) <= 21) ? 25 - (P) : (((P) == 0) ? 2 : (((P) == 1) ? 3 : (((P) == 2) ? 1 : (((P) == 3) ? 0 : (((P) == 4) ? 4 : (((P) == 6) ? 7 : (((P) == 13) ? 7 : (((P) == 14) ? 3 : (((P) == 15) ? 1 : (((P) == 16) ? 2 : (((P) == 17) ? 0 : (((P) == 22) ? 1 : (((P) == 23) ? 0 : (((P) == 24) ? 4 : (((P) == 25) ? 7 : (((P) == 26) ? 4 : (((P) == 27) ? 5 : 6 ))))))))))))))))))) + + +// --- Arduino Uno and ATmega168/328 based boards --- +#elif (defined(ARDUINO_AVR_UNO) || \ + defined(ARDUINO_AVR_DUEMILANOVE) || \ + defined(__AVR_ATmega168__) || \ + defined(__AVR_ATmega168A__) || \ + defined(__AVR_ATmega168PA__) || \ + defined(__AVR_ATmega328__) || \ + defined(__AVR_ATmega328P__) || \ + defined(__AVR_ATmega328PB__)) + +#define UART_RX_PIN (0) //PD0 +#define UART_TX_PIN (1) //PD1 + +#define I2C_SDA_PIN (18) //A4 +#define I2C_SCL_PIN (19) //A5 + +#define SPI_HW_SS_PIN (10) //PB0 +#define SPI_HW_MOSI_PIN (11) //PB2 +#define SPI_HW_MISO_PIN (12) //PB3 +#define SPI_HW_SCK_PIN (13) //PB1 + +#if defined(__AVR_ATmega328PB__) +#define __digitalPinToPortReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : (((P) >= 14 && (P) <= 19) ? &PORTC : &PORTE))) +#define __digitalPinToDDRReg(P) \ +(((P) >= 0 && (P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : (((P) >= 14 && (P) <= 19) ? &DDRC : &DDRE))) +#define __digitalPinToPINReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : (((P) >= 14 && (P) <= 19) ? &PINC : &PINE))) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (((P) >= 14 && (P) <= 19) ? (P) - 14 : (((P) >= 20 && (P) <= 21) ? (P) - 18 : (P) - 22)))) +#else +#define __digitalPinToPortReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : &PORTC)) +#define __digitalPinToDDRReg(P) \ +(((P) >= 0 && (P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : &DDRC)) +#define __digitalPinToPINReg(P) \ +(((P) >= 0 && (P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : &PINC)) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) +#endif + +// --- ATtinyX5 --- +#elif defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) +// we have only PORTB +#define __digitalPinToPortReg(P) (&PORTB) +#define __digitalPinToDDRReg(P) (&DDRB) +#define __digitalPinToPINReg(P) (&PINB) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) + +// --- ATtinyX4 + ATtinyX7 --- +// ATtinyX4: PORTA for 0 to 7, PORTB for 8 to 11 +// ATtinyX7: PORTA for 0 to 7, PORTB for 8 to 15 +#elif defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) || defined(__AVR_ATtiny87__) || defined(__AVR_ATtiny167__) +#define __digitalPinToPortReg(P) (((P) >= 0 && (P) <= 7) ? &PORTA : &PORTB) +#define __digitalPinToDDRReg(P) (((P) >= 0 && (P) <= 7) ? &DDRA : &DDRB) +#define __digitalPinToPINReg(P) (((P) >= 0 && (P) <= 7) ? &PINA : &PINB) +#define __digitalPinToBit(P) (((P) >= 0 && (P) <= 7) ? (P) : (P) - 8 ) + +// --- Other --- +#else + +#define I2C_SDA_PIN SDA +#define I2C_SCL_PIN SCL + +#define SPI_HW_SS_PIN SS +#define SPI_HW_MOSI_PIN MOSI +#define SPI_HW_MISO_PIN MISO +#define SPI_HW_SCK_PIN SCK + + +#endif +//#endif //#ifndef digitalPinToPortReg + + +#ifndef digitalWriteFast +#if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) +#define digitalWriteFast(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + BIT_WRITE(*__digitalPinToPortReg(P), __digitalPinToBit(P), (V)); \ +} else { \ + digitalWrite((P), (V)); \ +} +#else +#define digitalWriteFast digitalWrite +#endif +#endif + + +#ifndef pinModeFast +#if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) +#define pinModeFast(P, V) \ +if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ + if (V == INPUT_PULLUP) {\ + BIT_WRITE(*__digitalPinToDDRReg(P), __digitalPinToBit(P), (INPUT)); \ + BIT_WRITE(*__digitalPinToPortReg(P), __digitalPinToBit(P), (HIGH)); \ + } else { \ + BIT_WRITE(*__digitalPinToDDRReg(P), __digitalPinToBit(P), (V)); \ + } \ +} else { \ + pinMode((P), (V)); \ +} +#else +#define pinModeFast pinMode +#endif +#endif + + +#ifndef digitalReadFast +#if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) +#define digitalReadFast(P) ( (int) __digitalReadFast((P)) ) +#define __digitalReadFast(P ) \ + (__builtin_constant_p(P) ) ? ( \ + ( BIT_READ(*__digitalPinToPINReg(P), __digitalPinToBit(P))) ? HIGH:LOW ) : \ + digitalRead((P)) +#else +#define digitalReadFast digitalRead +#endif +#endif + + +#ifndef digitalToggleFast +#if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) +#define digitalToggleFast(P) BIT_SET(*__digitalPinToPINReg(P), __digitalPinToBit(P)) +#endif +#endif + + +#endif //__digitalWriteFast_h_ \ No newline at end of file diff --git a/examples/YourFirstFFBController/YourFirstFFBController.ino b/examples/YourFirstFFBController/YourFirstFFBController.ino new file mode 100644 index 0000000..3674e8a --- /dev/null +++ b/examples/YourFirstFFBController/YourFirstFFBController.ino @@ -0,0 +1,136 @@ + +#include +#include "DigitalWriteFast.h" + +#define encoderPinA 2 +#define encoderPinB 3 + +#define motorPinA 7 +#define motorPinB 8 +#define motorPinPWM 9 + +#define ENCODER_MAX_VALUE 1200 +#define ENCODER_MIN_VALUE -1200 + +#define MAX_PWM 200 + +bool isOutOfRange = false; +int32_t forces[2]={0}; +Gains gains[2]; +EffectParams effectparams[2]; + +Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID,JOYSTICK_TYPE_JOYSTICK, + 8, 0, // Button Count, Hat Switch Count + true, true, false, // X and Y, but no Z Axis + true, true, false, // Rx, Ry, no Rz + false, false, // No rudder or throttle + false, false, false); // No accelerator, brake, or steering + +volatile long value = 0; +int32_t g_force = 0; + +int32_t currentPosition = 0; +volatile int8_t oldState = 0; +const int8_t KNOBDIR[] = { + 0, 1, -1, 0, + -1, 0, 0, 1, + 1, 0, 0, -1, + 0, -1, 1, 0 +}; + +void tick(void) +{ + int sig1 = digitalReadFast(encoderPinA); + int sig2 = digitalReadFast(encoderPinB); + int8_t thisState = sig1 | (sig2 << 1); + + if (oldState != thisState) { + currentPosition += KNOBDIR[thisState | (oldState<<2)]; + oldState = thisState; + } +} + +void setup() { + Serial.begin(115200); + attachInterrupt(digitalPinToInterrupt(encoderPinA),tick,CHANGE); + attachInterrupt(digitalPinToInterrupt(encoderPinB),tick,CHANGE); + Joystick.setRyAxisRange(0, 500); + Joystick.setRxAxisRange(0, 500); + Joystick.setYAxisRange(0, 500); + Joystick.setXAxisRange(ENCODER_MIN_VALUE, ENCODER_MAX_VALUE); + Joystick.setGains(gains); + Joystick.begin(true); + + pinMode(motorPinA, OUTPUT); + pinMode(motorPinB, OUTPUT); + pinMode(motorPinPWM, OUTPUT); + + pinMode(A0, INPUT_PULLUP); + + cli(); + TCCR3A = 0; //set TCCR1A 0 + TCCR3B = 0; //set TCCR1B 0 + TCNT3 = 0; //counter init + OCR3A = 399; + TCCR3B |= (1 << WGM32); //open CTC mode + TCCR3B |= (1 << CS31); //set CS11 1(8-fold Prescaler) + TIMSK3 |= (1 << OCIE3A); + sei(); + +} + +ISR(TIMER3_COMPA_vect){ + Joystick.getUSBPID(); +} + +unsigned int interval = 0; +void loop() { + value = currentPosition; + + if(value > ENCODER_MAX_VALUE) + { + isOutOfRange = true; + value = ENCODER_MAX_VALUE; + }else if(value < ENCODER_MIN_VALUE) + { + isOutOfRange = true; + value = ENCODER_MIN_VALUE; + }else{ + isOutOfRange = false; + } + + Joystick.setXAxis(value); + Joystick.setRxAxis(analogRead(A1)); + Joystick.setRyAxis(analogRead(A2)); + Joystick.setYAxis(analogRead(A3)); + + effectparams[0].springMaxPosition = ENCODER_MAX_VALUE; + effectparams[0].springPosition = value; + effectparams[1].springMaxPosition = 255; + effectparams[1].springPosition = 0; + Joystick.setEffectParams(effectparams); + Joystick.getForce(forces); + + + if(!isOutOfRange){ + if(forces[0] > 0) + { + digitalWrite(motorPinA, HIGH); + digitalWrite(motorPinB, LOW); + analogWrite(motorPinPWM, abs(forces[0])); + }else{ + digitalWrite(motorPinA, LOW); + digitalWrite(motorPinB, HIGH); + analogWrite(motorPinPWM, abs(forces[0])); + } + }else{ + if(value < 0){ + digitalWrite(motorPinA, LOW); + digitalWrite(motorPinB, HIGH); + }else{ + digitalWrite(motorPinA, HIGH); + digitalWrite(motorPinB, LOW); + } + analogWrite(motorPinPWM, MAX_PWM); + } +} diff --git a/examples/YourFirstFFBController/ffbcontroller.png b/examples/YourFirstFFBController/ffbcontroller.png new file mode 100644 index 0000000..8753f85 Binary files /dev/null and b/examples/YourFirstFFBController/ffbcontroller.png differ diff --git a/src/DynamicHID/DynamicHID.cpp b/src/DynamicHID/DynamicHID.cpp index e052da0..503ad6a 100644 --- a/src/DynamicHID/DynamicHID.cpp +++ b/src/DynamicHID/DynamicHID.cpp @@ -119,7 +119,6 @@ int DynamicHID_::RecvData(byte* data) void DynamicHID_::RecvfromUsb() { if (usb_Available() > 0) { - uint8_t out_ffbdata[64]; uint16_t len = USB_Recv(PID_ENDPOINT_OUT, &out_ffbdata, 64); if (len >= 0) { pidReportHandler.UppackUsbData(out_ffbdata, len); diff --git a/src/DynamicHID/DynamicHID.h b/src/DynamicHID/DynamicHID.h index 4f373f8..34e7ba2 100644 --- a/src/DynamicHID/DynamicHID.h +++ b/src/DynamicHID/DynamicHID.h @@ -136,7 +136,7 @@ class DynamicHID_ : public PluggableUSBModule private: uint8_t epType[2]; - + uint8_t out_ffbdata[64]; DynamicHIDSubDescriptor* rootNode; uint16_t descriptorSize; diff --git a/src/DynamicHID/PIDReportHandler.cpp b/src/DynamicHID/PIDReportHandler.cpp index 7221d77..8bfcc62 100644 --- a/src/DynamicHID/PIDReportHandler.cpp +++ b/src/DynamicHID/PIDReportHandler.cpp @@ -169,17 +169,6 @@ void PIDReportHandler::SetEffect(USB_FFBReport_SetEffect_Output_Data_t* data) effect->effectType = data->effectType; effect->gain = data->gain; effect->enableAxis = data->enableAxis; - //Serial.print("dX: "); - //Serial.print(effect->directionX); - //Serial.print(" dX: "); - //Serial.print(effect->directionY); - //Serial.print(" eT: "); - //Serial.print(effect->effectType); - //Serial.print(" eA: "); - //Serial.println(effect->enableAxis); - // effect->triggerRepeatInterval; - // effect->samplePeriod; // 0..32767 ms - // effect->triggerButton; } void PIDReportHandler::SetEnvelope(USB_FFBReport_SetEnvelope_Output_Data_t* data, volatile TEffectState* effect) @@ -192,14 +181,14 @@ void PIDReportHandler::SetEnvelope(USB_FFBReport_SetEnvelope_Output_Data_t* data void PIDReportHandler::SetCondition(USB_FFBReport_SetCondition_Output_Data_t* data, volatile TEffectState* effect) { - uint8_t axis = data->parameterBlockOffset; - effect->conditions[axis].cpOffset = data->cpOffset; - effect->conditions[axis].positiveCoefficient = data->positiveCoefficient; - effect->conditions[axis].negativeCoefficient = data->negativeCoefficient; - effect->conditions[axis].positiveSaturation = data->positiveSaturation; - effect->conditions[axis].negativeSaturation = data->negativeSaturation; - effect->conditions[axis].deadBand = data->deadBand; - effect->conditionBlocksCount++; + uint8_t axis = data->parameterBlockOffset; + effect->conditions[axis].cpOffset = data->cpOffset; + effect->conditions[axis].positiveCoefficient = data->positiveCoefficient; + effect->conditions[axis].negativeCoefficient = data->negativeCoefficient; + effect->conditions[axis].positiveSaturation = data->positiveSaturation; + effect->conditions[axis].negativeSaturation = data->negativeSaturation; + effect->conditions[axis].deadBand = data->deadBand; + effect->conditionBlocksCount++; } void PIDReportHandler::SetPeriodic(USB_FFBReport_SetPeriodic_Output_Data_t* data, volatile TEffectState* effect) @@ -248,6 +237,8 @@ void PIDReportHandler::UppackUsbData(uint8_t* data, uint16_t len) //Serial.print("len:"); //Serial.println(len); uint8_t effectId = data[1]; // effectBlockIndex is always the second byte. + //Serial.println("eid:"); + //Serial.println(effectId); switch (data[0]) // reportID { case 1: @@ -328,4 +319,4 @@ uint8_t* PIDReportHandler::getPIDBlockLoad() uint8_t* PIDReportHandler::getPIDStatus() { return (uint8_t*)& pidState; -} +} \ No newline at end of file diff --git a/src/DynamicHID/PIDReportType.h b/src/DynamicHID/PIDReportType.h index dadd5f8..20db921 100644 --- a/src/DynamicHID/PIDReportType.h +++ b/src/DynamicHID/PIDReportType.h @@ -28,7 +28,7 @@ #define _PIDREPORTTYPE_H #define MAX_EFFECTS 14 -#define FFB_AXIS_COUNT 2 +#define MAX_FFB_AXIS_COUNT 0x02 #define SIZE_EFFECT sizeof(TEffectState) #define MEMORY_SIZE (uint16_t)(MAX_EFFECTS*SIZE_EFFECT) #define TO_LT_END_16(x) ((x<<8)&0xFF00)|((x>>8)&0x00FF) @@ -69,8 +69,8 @@ typedef struct//FFB: Set Envelope Output Report uint8_t effectBlockIndex; // 1..40 uint16_t attackLevel; uint16_t fadeLevel; - uint16_t attackTime; // ms - uint16_t fadeTime; // ms + uint32_t attackTime; // ms + uint32_t fadeTime; // ms } USB_FFBReport_SetEnvelope_Output_Data_t; typedef struct// FFB: Set Condition Output Report @@ -93,7 +93,7 @@ typedef struct//FFB: Set Periodic Output Report uint16_t magnitude; int16_t offset; uint16_t phase; // 0..255 (=0..359, exp-2) - uint16_t period; // 0..32767 ms + uint32_t period; // 0..32767 ms } USB_FFBReport_SetPeriodic_Output_Data_t; typedef struct//FFB: Set ConstantForce Output Report @@ -184,6 +184,16 @@ typedef struct// FFB: PID Pool Feature Report uint8_t memoryManagement; // Bits: 0=DeviceManagedPool, 1=SharedParameterBlocks } USB_FFBReport_PIDPool_Feature_Data_t; +typedef struct { + int16_t cpOffset; // -128..127 + int16_t positiveCoefficient; // -128..127 + int16_t negativeCoefficient; // -128..127 + uint16_t positiveSaturation; // -128..127 + uint16_t negativeSaturation; // -128..127 + uint16_t deadBand; // 0..255 + +} TEffectCondition; + ///effect #define USB_DURATION_INFINITE 0x7FFF @@ -213,34 +223,29 @@ typedef struct// FFB: PID Pool Feature Report #define INERTIA_DEADBAND 0x30 #define FRICTION_DEADBAND 0x30 -typedef struct { - int16_t cpOffset; // -128..127 - int16_t positiveCoefficient; // -128..127 - int16_t negativeCoefficient; // -128..127 - uint16_t positiveSaturation; // -128..127 - uint16_t negativeSaturation; // -128..127 - uint16_t deadBand; // 0..255 -} TEffectCondition; - typedef struct { volatile uint8_t state; // see constants uint8_t effectType; // int16_t offset; uint8_t gain; + //envelop int16_t attackLevel, fadeLevel; + uint16_t fadeTime, attackTime; + int16_t magnitude; + //direction uint8_t enableAxis; // bits: 0=X, 1=Y, 2=DirectionEnable uint8_t directionX; // angle (0=0 .. 255=360deg) uint8_t directionY; // angle (0=0 .. 255=360deg) uint8_t conditionBlocksCount; - - TEffectCondition conditions[FFB_AXIS_COUNT]; - + //condition + TEffectCondition conditions[MAX_FFB_AXIS_COUNT]; + //periodic uint16_t phase; // 0..255 (=0..359, exp-2) int16_t startMagnitude; int16_t endMagnitude; uint16_t period; // 0..32767 ms - uint16_t duration, fadeTime, attackTime, elapsedTime; + uint16_t duration, elapsedTime; uint64_t startTime; } TEffectState; #endif diff --git a/src/FFBDescriptor.h b/src/FFBDescriptor.h index b3d30dc..ac0e44a 100644 --- a/src/FFBDescriptor.h +++ b/src/FFBDescriptor.h @@ -183,7 +183,8 @@ static const uint8_t pidReportDescriptor[] PROGMEM= { 0x55, 0xFD, // Unit Exponent (FDh) (X10^-3 ==> Milisecond) 0x27, 0xFF, 0x7F, 0, 0, // Logical Maximum (4294967295) 0x47, 0xFF, 0x7F, 0, 0, // Physical Maximum (4294967295) - 0x75, 0x20, // Report Size (32) + 0x75, 0x20, // Report Size (16) + 0x95, 0x02, // Report Count (2) 0x91, 0x02, // Output (Data,Var,Abs) 0x45, 0x00, // Physical Maximum (0) 0x66, 0x00, 0x00, // Unit (0) @@ -292,7 +293,7 @@ static const uint8_t pidReportDescriptor[] PROGMEM= { 0x47, 0xFF, 0x7F, 0, 0, // Physical Maximum (32K) 0x66, 0x03, 0x10, // Unit (1003h) (English Linear, Seconds) 0x55, 0xFD, // Unit Exponent (FDh) (X10^-3 ==> Milisecond) - 0x75, 0x20, // Report Size (32) + 0x75, 0x20, // Report Size (16) 0x95, 0x01, // Report Count (1) 0x91, 0x02, // Output (Data,Var,Abs) 0x66, 0x00, 0x00, // Unit (0) @@ -386,7 +387,7 @@ static const uint8_t pidReportDescriptor[] PROGMEM= { 0x35, 0x00, // Physical Minimum (0) 0x46, 0xFF, 0x00, // Physical Maximum (255) 0x75, 0x08, // Report Size (8) - 0x95, 0x02, // Report Count (2) + 0x95, 0x01, // Report Count (2) 0x91, 0x02, // Output (Data,Var,Abs) 0xC0, //End Collection Datalink (Logical) (OK) diff --git a/src/Joystick.cpp b/src/Joystick.cpp index ab6bb60..12866ea 100644 --- a/src/Joystick.cpp +++ b/src/Joystick.cpp @@ -41,6 +41,8 @@ #define JOYSTICK_INCLUDE_BRAKE B00001000 #define JOYSTICK_INCLUDE_STEERING B00010000 +unsigned int timecnt = 0; + Joystick_::Joystick_( uint8_t hidReportId, uint8_t joystickType, @@ -105,7 +107,7 @@ Joystick_::Joystick_( static uint8_t tempHidReportDescriptor[150]; int hidReportDescriptorSize = 0; - // // USAGE_PAGE (Generic Desktop) + // USAGE_PAGE (Generic Desktop) tempHidReportDescriptor[hidReportDescriptorSize++] = 0x05; tempHidReportDescriptor[hidReportDescriptorSize++] = 0x01; @@ -485,22 +487,25 @@ void Joystick_::begin(bool initAutoSendState) sendState(); } -void Joystick_::getForce(int32_t* forces) { +void Joystick_::getUSBPID() +{ DynamicHID().RecvfromUsb(); - forceCalculator(forces); } -int32_t Joystick_::getEffectForce(volatile TEffectState& effect,Gains _gains,EffectParams _effect_params, uint8_t axis){ +void Joystick_::getForce(int32_t* forces) +{ + forceCalculator(forces); +} - uint8_t direction; +int32_t Joystick_::getEffectForce(volatile TEffectState& effect, Gains _gains, EffectParams _effect_params, uint8_t axis){ + uint8_t direction; uint8_t condition; - - bool useForceDirectionForConditionEffect = (effect.enableAxis == DIRECTION_ENABLE && effect.conditionsCount == 1); + bool useForceDirectionForConditionEffect = (effect.enableAxis == DIRECTION_ENABLE && effect.conditionBlocksCount == 1); if (effect.enableAxis == DIRECTION_ENABLE) { direction = effect.directionX; - if (effect.conditionsCount > 1) { + if (effect.conditionBlocksCount > 1) { condition = axis; } else { condition = 0; // only one Condition Parameter Block is defined @@ -514,7 +519,6 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect,Gains _gains,Eff float angle = (direction * 360.0 / 255.0) * DEG_TO_RAD; float angle_ratio = axis == 0 ? sin(angle) : -1 * cos(angle); - int32_t force = 0; switch (effect.effectType) { @@ -541,16 +545,16 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect,Gains _gains,Eff break; case USB_EFFECT_SPRING://8 force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.springPosition, _effect_params.springMaxPosition), condition) * _gains.springGain; - if (useForceDirectionForConditionEffect) { + if (useForceDirectionForConditionEffect) { force *= angle_ratio; } - break; + break; case USB_EFFECT_DAMPER://9 force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.damperVelocity, _effect_params.damperMaxVelocity), condition) * _gains.damperGain; - if (useForceDirectionForConditionEffect) { + if (useForceDirectionForConditionEffect) { force *= angle_ratio; } - break; + break; case USB_EFFECT_INERTIA://10 if (_effect_params.inertiaAcceleration < 0 && _effect_params.frictionPositionChange < 0) { force = ConditionForceCalculator(effect, abs(NormalizeRange(_effect_params.inertiaAcceleration, _effect_params.inertiaMaxAcceleration)), condition) * _gains.inertiaGain; @@ -563,11 +567,11 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect,Gains _gains,Eff } break; case USB_EFFECT_FRICTION://11 - force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.frictionPositionChange, _effect_params.frictionMaxPositionChange), condition) * _gains.frictionGain; - if (useForceDirectionForConditionEffect) { - force *= angle_ratio; - } - break; + force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.frictionPositionChange, _effect_params.frictionMaxPositionChange), condition) * _gains.frictionGain; + if (useForceDirectionForConditionEffect) { + force *= angle_ratio; + } + break; case USB_EFFECT_CUSTOM://12 break; } @@ -577,55 +581,42 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect,Gains _gains,Eff void Joystick_::forceCalculator(int32_t* forces) { - forces[0] = 0; + forces[0] = 0; forces[1] = 0; - int32_t force = 0; for (int id = 0; id < MAX_EFFECTS; id++) { volatile TEffectState& effect = DynamicHID().pidReportHandler.g_EffectStates[id]; if ((effect.state == MEFFECTSTATE_PLAYING) && ((effect.elapsedTime <= effect.duration) || - (effect.duration == USB_DURATION_INFINITE)) && - !DynamicHID().pidReportHandler.devicePaused) + (effect.duration == USB_DURATION_INFINITE)) && !DynamicHID().pidReportHandler.devicePaused) { - if (effect.enableAxis == DIRECTION_ENABLE - || effect.enableAxis & X_AXIS_ENABLE) - { - forces[0] += (int32_t)(getEffectForce(effect,m_gains[0], m_effect_params[0], 0)); - } - if (effect.enableAxis == DIRECTION_ENABLE - || effect.enableAxis & Y_AXIS_ENABLE) - { - forces[1] += (int32_t)(getEffectForce(effect,m_gains[1], m_effect_params[1], 1)); - } - + forces[0] += (int32_t)(getEffectForce(effect, m_gains[0], m_effect_params[0], 0)); + forces[1] += (int32_t)(getEffectForce(effect, m_gains[1], m_effect_params[1], 1)); } } - forces[0] = (int32_t)((float)1.00 * forces[0] * m_gains[0].totalGain / 10000); // each effect gain * total effect gain = 10000 - forces[1] = (int32_t)((float)1.00 * forces[1] * m_gains[1].totalGain / 10000); // each effect gain * total effect gain = 10000 - forces[0] = constrain(forces[0], -255, 255); - forces[1] = constrain(forces[1], -255, 255); + forces[0] = (int32_t)((float)1.0 * forces[0] * m_gains[0].totalGain / 10000); // each effect gain * total effect gain = 10000 + forces[1] = (int32_t)((float)1.0 * forces[1] * m_gains[1].totalGain / 10000); // each effect gain * total effect gain = 10000 + forces[0] = map(forces[0], -10000, 10000, -250, 250); + forces[1] = map(forces[1], -10000, 10000, -250, 250); } int32_t Joystick_::ConstantForceCalculator(volatile TEffectState& effect) { - float tempforce = (float)effect.magnitude * effect.gain / 255; - tempforce = map(tempforce, -10000, 10000, -255, 255); - return (int32_t)tempforce; + return ApplyEnvelope(effect, (int32_t)effect.magnitude); } int32_t Joystick_::RampForceCalculator(volatile TEffectState& effect) { - int32_t rampForce = effect.startMagnitude + effect.elapsedTime * (effect.endMagnitude - effect.startMagnitude) / effect.duration; - return rampForce; + int32_t tempforce = (int32_t)(effect.startMagnitude + effect.elapsedTime * 1.0 * (effect.endMagnitude - effect.startMagnitude) / effect.duration); + return ApplyEnvelope(effect, tempforce); } int32_t Joystick_::SquareForceCalculator(volatile TEffectState& effect) { - int32_t offset = effect.offset * 2; - uint32_t magnitude = effect.magnitude; - uint32_t elapsedTime = effect.elapsedTime; - uint32_t phase = effect.phase; - uint32_t period = effect.period; + int16_t offset = effect.offset * 2; + int16_t magnitude = effect.magnitude; + uint16_t phase = effect.phase; + uint16_t elapsedTime = effect.elapsedTime; + uint16_t period = effect.period; int32_t maxMagnitude = offset + magnitude; int32_t minMagnitude = offset - magnitude; @@ -640,35 +631,36 @@ int32_t Joystick_::SquareForceCalculator(volatile TEffectState& effect) int32_t Joystick_::SinForceCalculator(volatile TEffectState& effect) { - float offset = effect.offset * 2; - float magnitude = effect.magnitude; - float phase = effect.phase; - float timeTemp = effect.elapsedTime; - float period = effect.period; - // float angle = ((timeTemp / period) + (phase / 255) * period) * 2 * PI; - float angle = ((timeTemp / period) * 2 * PI + (float)(phase / 36000)); + int16_t offset = effect.offset * 2; + int16_t magnitude = effect.magnitude; + uint16_t phase = effect.phase; + uint16_t timeTemp = effect.elapsedTime; + uint16_t period = effect.period; + float angle = 0.0; + if(period != 0) + angle = ((timeTemp * 1.0 / period) * 2 * PI + (phase / 36000.0)); float sine = sin(angle); - float tempforce = sine * magnitude; + int32_t tempforce = (int32_t)(sine * magnitude); tempforce += offset; return ApplyEnvelope(effect, tempforce); } int32_t Joystick_::TriangleForceCalculator(volatile TEffectState& effect) { - float offset = effect.offset * 2; - float magnitude = effect.magnitude; - float elapsedTime = effect.elapsedTime; - uint32_t phase = effect.phase; - uint32_t period = effect.period; - float periodF = effect.period; + int16_t offset = effect.offset * 2; + int16_t magnitude = effect.magnitude; + uint16_t elapsedTime = effect.elapsedTime; + uint16_t phase = effect.phase; + uint16_t period = effect.period; + uint16_t periodF = effect.period; - float maxMagnitude = offset + magnitude; - float minMagnitude = offset - magnitude; - uint32_t phasetime = (phase * period) / 255; + int16_t maxMagnitude = offset + magnitude; + int16_t minMagnitude = offset - magnitude; + int32_t phasetime = (phase * period) / 255; uint32_t timeTemp = elapsedTime + phasetime; - float reminder = timeTemp % period; - float slope = ((maxMagnitude - minMagnitude) * 2) / periodF; - float tempforce = 0; + int32_t reminder = timeTemp % period; + int32_t slope = ((maxMagnitude - minMagnitude) * 2) / periodF; + int32_t tempforce = 0; if (reminder > (periodF / 2)) tempforce = slope * (periodF - reminder); else tempforce = slope * reminder; tempforce += minMagnitude; @@ -677,20 +669,20 @@ int32_t Joystick_::TriangleForceCalculator(volatile TEffectState& effect) int32_t Joystick_::SawtoothDownForceCalculator(volatile TEffectState& effect) { - float offset = effect.offset * 2; - float magnitude = effect.magnitude; - float elapsedTime = effect.elapsedTime; - float phase = effect.phase; - uint32_t period = effect.period; - float periodF = effect.period; + int16_t offset = effect.offset * 2; + int16_t magnitude = effect.magnitude; + uint16_t elapsedTime = effect.elapsedTime; + uint16_t phase = effect.phase; + uint16_t period = effect.period; + uint16_t periodF = effect.period; - float maxMagnitude = offset + magnitude; - float minMagnitude = offset - magnitude; + int16_t maxMagnitude = offset + magnitude; + int16_t minMagnitude = offset - magnitude; int32_t phasetime = (phase * period) / 255; uint32_t timeTemp = elapsedTime + phasetime; - float reminder = timeTemp % period; - float slope = (maxMagnitude - minMagnitude) / periodF; - float tempforce = 0; + int32_t reminder = timeTemp % period; + int32_t slope = (maxMagnitude - minMagnitude) / periodF; + int32_t tempforce = 0; tempforce = slope * (period - reminder); tempforce += minMagnitude; return ApplyEnvelope(effect, tempforce); @@ -698,20 +690,20 @@ int32_t Joystick_::SawtoothDownForceCalculator(volatile TEffectState& effect) int32_t Joystick_::SawtoothUpForceCalculator(volatile TEffectState& effect) { - float offset = effect.offset * 2; - float magnitude = effect.magnitude; - float elapsedTime = effect.elapsedTime; - uint32_t phase = effect.phase; - uint32_t period = effect.period; - float periodF = effect.period; + int16_t offset = effect.offset * 2; + int16_t magnitude = effect.magnitude; + uint16_t elapsedTime = effect.elapsedTime; + uint16_t phase = effect.phase; + uint16_t period = effect.period; + uint16_t periodF = effect.period; - float maxMagnitude = offset + magnitude; - float minMagnitude = offset - magnitude; + int16_t maxMagnitude = offset + magnitude; + int16_t minMagnitude = offset - magnitude; int32_t phasetime = (phase * period) / 255; uint32_t timeTemp = elapsedTime + phasetime; - float reminder = timeTemp % period; - float slope = (maxMagnitude - minMagnitude) / periodF; - float tempforce = 0; + int32_t reminder = timeTemp % period; + int32_t slope = (maxMagnitude - minMagnitude) / periodF; + int32_t tempforce = 0; tempforce = slope * reminder; tempforce += minMagnitude; return ApplyEnvelope(effect, tempforce); @@ -719,12 +711,12 @@ int32_t Joystick_::SawtoothUpForceCalculator(volatile TEffectState& effect) int32_t Joystick_::ConditionForceCalculator(volatile TEffectState& effect, float metric, uint8_t axis) { - float deadBand; - float cpOffset; - float negativeCoefficient; - float negativeSaturation; - float positiveSaturation; - float positiveCoefficient; + float deadBand; + float cpOffset; + float positiveCoefficient; + float negativeCoefficient; + float positiveSaturation; + float negativeSaturation; deadBand = effect.conditions[axis].deadBand; cpOffset = effect.conditions[axis].cpOffset; @@ -733,15 +725,14 @@ int32_t Joystick_::ConditionForceCalculator(volatile TEffectState& effect, float positiveSaturation = effect.conditions[axis].positiveSaturation; positiveCoefficient = effect.conditions[axis].positiveCoefficient; - float tempForce = 0; - - if (metric < (cpOffset - deadBand)) { + float tempForce = 0; + if (metric < (cpOffset - deadBand)) + { tempForce = (metric - (float)1.00*(cpOffset - deadBand)/10000) * negativeCoefficient; - // tempForce = ((float)1.00 * (cpOffset - deadBand) / 10000 - metric) * negativeCoefficient; - tempForce = (tempForce < -negativeSaturation ? -negativeSaturation : tempForce); // I dont know why negativeSaturation = 55536.00 after negativeSaturation = -effect.negativeSaturation; - // tempForce = (tempForce < (-effect.negativeCoefficient) ? (-effect.negativeCoefficient) : tempForce); + tempForce = (tempForce < -negativeSaturation ? -negativeSaturation : tempForce); } - else if (metric > (cpOffset + deadBand)) { + else if (metric > (cpOffset + deadBand)) + { tempForce = (metric - (float)1.00 * (cpOffset + deadBand) / 10000) * positiveCoefficient; tempForce = (tempForce > positiveSaturation ? positiveSaturation : tempForce); } @@ -760,21 +751,20 @@ int32_t Joystick_::ConditionForceCalculator(volatile TEffectState& effect, float default: break; } - tempForce = map(tempForce, -10000, 10000, -255, 255); return (int32_t)tempForce; } -float Joystick_::NormalizeRange(int32_t x, int32_t maxValue) { +inline float Joystick_::NormalizeRange(int32_t x, int32_t maxValue) { return (float)x * 1.00 / maxValue; } -int32_t Joystick_::ApplyGain(uint8_t value, uint8_t gain) +inline int32_t Joystick_::ApplyGain(int16_t value, uint8_t gain) { - int32_t value_32 = (int16_t)value; + int32_t value_32 = value; return ((value_32 * gain) / 255); } -int32_t Joystick_::ApplyEnvelope(volatile TEffectState& effect, int32_t value) +inline int32_t Joystick_::ApplyEnvelope(volatile TEffectState& effect, int32_t value) { int32_t magnitude = ApplyGain(effect.magnitude, effect.gain); int32_t attackLevel = ApplyGain(effect.attackLevel, effect.gain); @@ -787,8 +777,7 @@ int32_t Joystick_::ApplyEnvelope(volatile TEffectState& effect, int32_t value) if (elapsedTime < attackTime) { - newValue = (magnitude - attackLevel) * elapsedTime; - newValue /= attackTime; + newValue = (magnitude - attackLevel) * elapsedTime / attackTime; newValue += attackLevel; } if (elapsedTime > (duration - fadeTime)) @@ -797,9 +786,7 @@ int32_t Joystick_::ApplyEnvelope(volatile TEffectState& effect, int32_t value) newValue /= fadeTime; newValue += fadeLevel; } - - newValue *= value; - newValue /= 255; + newValue = newValue * value / magnitude; return newValue; } diff --git a/src/Joystick.h b/src/Joystick.h index 9aced73..ac083d4 100644 --- a/src/Joystick.h +++ b/src/Joystick.h @@ -58,7 +58,6 @@ #define DIRECTION_ENABLE 0x04 #define X_AXIS_ENABLE 0x01 #define Y_AXIS_ENABLE 0x02 -#define FFB_AXIS_COUNT 0x02 #define FORCE_FEEDBACK_MAXGAIN 100 #define DEG_TO_RAD ((float)((float)3.14159265359 / 180.0)) @@ -150,10 +149,13 @@ class Joystick_ //force feedback effect params EffectParams* m_effect_params; + //lock data + bool is_calculating_force = true; + ///force calculate funtion float NormalizeRange(int32_t x, int32_t maxValue); int32_t ApplyEnvelope(volatile TEffectState& effect, int32_t value); - int32_t ApplyGain(uint8_t value, uint8_t gain); + int32_t ApplyGain(int16_t value, uint8_t gain); int32_t ConstantForceCalculator(volatile TEffectState& effect); int32_t RampForceCalculator(volatile TEffectState& effect); int32_t SquareForceCalculator(volatile TEffectState& effect); @@ -268,7 +270,8 @@ class Joystick_ void setHatSwitch(int8_t hatSwitch, int16_t value); void sendState(); - + // get USB PID data + void getUSBPID(); //force feedback Interfaces void getForce(int32_t* forces); //set gain functions diff --git a/version.info b/version.info new file mode 100644 index 0000000..60453e6 --- /dev/null +++ b/version.info @@ -0,0 +1 @@ +v1.0.0 \ No newline at end of file