Default spring (#18)

* default_spring: 1st try

* adding dump

* get default spring working

* add workflow and remove dump debug

* modify to use structs for joystick gains

* add new setup gains

Co-authored-by: jmriego <[email protected]>
Co-authored-by: Jose Riego Valenzuela <[email protected]>

Author: 3 people

.github/workflows/arduino.yml

@@ -0,0 +1,36 @@
+name: Arduino
+
+on:
+  push:
+    tags: ['v*']
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Setup Arduino CLI
+      uses: arduino/[email protected]
+
+    - name: Install Arduino Micro platform
+      run: |
+          arduino-cli core update-index
+          arduino-cli core install arduino:avr
+
+    - name: Compile Fino Arduino Sketch
+      run: |
+          mkdir -p build/arduino.avr.micro
+          arduino-cli compile --fqbn arduino:avr:micro --verbose --output-dir build/arduino.avr.micro
+
+    - name: Upload Arduino sketch artifact
+      uses: actions/upload-artifact@v2
+      with:
+        name: Artifact.Fino
+        path: build/arduino.avr.micro/Fino.ino.hex
+
+

Fino.ino

@@ -1,6 +1,6 @@
 #define DEBUGNO
 // the digits mean Mmmmrrr (M=Major,m=minor,r=revision)
-#define SKETCH_VERSION 2004000
+#define SKETCH_VERSION 3000000
 
 #include "src/Joystick.h"
 #include "config.h"

com.ino

@@ -53,11 +53,21 @@ void get_messages_from_serial()
           write_i32(SKETCH_VERSION);
           break;
         }
-  			// Unknown order
-  			default:
+        case CONFIG:
+        {
+          float defaultSpringGain = read_i8() / 100.0;
+
+          Gains gains[FFB_AXIS_COUNT];
+          gains[0].defaultSpringGain = defaultSpringGain;
+          gains[1].defaultSpringGain = defaultSpringGain;
+          Joystick.setGains(gains);
+          break;
+        }
+        // Unknown order
+        default:
           write_order(ERROR);
           write_i16(404);
-  				return;
+          return;
       }
     }
     write_order(RECEIVED); // Confirm the reception

config.h

@@ -5,18 +5,6 @@
 // --------------------------
 
 #define default_gain 1.0
-#define totalGain default_gain
-#define constantGain default_gain
-#define rampGain default_gain
-#define squareGain default_gain
-#define sineGain default_gain
-#define triangleGain default_gain
-#define sawtoothdownGain default_gain
-#define sawtoothupGain default_gain
-#define springGain default_gain
-#define inertiaGain default_gain
-#define frictionGain 0.25
-#define damperGain default_gain
 #define damperSplineNumPoints 6
 
 // comment out this line if you don't want to have a spline configuration for the damper

order.h

@@ -10,7 +10,8 @@ enum Order {
   POSITION = 4,
   ERROR = 5,
   LOG = 6,
-  VERSION = 7
+  VERSION = 7,
+  CONFIG = 8
 };
 
 typedef enum Order Order;

src/DynamicHID/PIDReportHandler.cpp

@@ -3,7 +3,7 @@
 PIDReportHandler::PIDReportHandler() 
 {
 	nextEID = 1;
-	devicePaused = 0;
+	deviceState = MDEVICESTATE_SPRING;
 }
 
 PIDReportHandler::~PIDReportHandler() 
@@ -137,18 +137,20 @@ void PIDReportHandler::DeviceControl(USB_FFBReport_DeviceControl_Output_Data_t*
 	else if (control == 0x03)
 	{ // 3=Stop All Effects
 		StopAllEffects();
+		deviceState &= ~(MDEVICESTATE_SPRING);
 	}
 	else if (control == 0x04)
 	{ //  4=Reset
 		FreeAllEffects();
+		deviceState |= MDEVICESTATE_SPRING;
 	}
 	else if (control == 0x05)
 	{ // 5=Pause
-		devicePaused = 1;
+		deviceState |= MDEVICESTATE_PAUSED;
 	}
 	else if (control == 0x06)
 	{ // 6=Continue
-		devicePaused = 0;
+		deviceState &= ~(MDEVICESTATE_PAUSED);
 	}
 	else if (control & (0xFF - 0x3F))
 	{
@@ -193,22 +195,22 @@ void PIDReportHandler::SetEffect(USB_FFBReport_SetEffect_Output_Data_t* data)
         uint8_t loopCount = effect->loopCount > 0 ? effect->loopCount : 1;
         effect->totalDuration = (data->duration + data->startDelay) * loopCount;
     }
-	//Serial.print("lC: ");
-	//Serial.println(effect->loopCount);
-	//Serial.print("d: ");
-	//Serial.print(effect->duration);
-	//Serial.print("tD: ");
-	//Serial.println(effect->totalDuration);
-	//Serial.print("sD: ");
-	//Serial.println(effect->startDelay);
-	//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);
+	// Serial.print("lC: ");
+	// Serial.print(effect->loopCount);
+	// Serial.print(" d: ");
+	// Serial.print(effect->duration);
+	// Serial.print(" tD: ");
+	// Serial.print(effect->totalDuration);
+	// Serial.print(" sD: ");
+	// Serial.print(effect->startDelay);
+	// Serial.print(" dX: ");
+	// Serial.print(effect->direction[0]);
+	// Serial.print(" dY: ");
+	// Serial.print(effect->direction[1]);
+	// Serial.print(" eT: ");
+	// Serial.print(effect->effectType);
+	// Serial.print(" eA: ");
+	// Serial.println(effect->enableAxis);
 	//  effect->triggerRepeatInterval;
 	//  effect->samplePeriod;   // 0..32767 ms
 	//  effect->triggerButton;
@@ -282,7 +284,8 @@ void PIDReportHandler::CreateNewEffect(USB_FFBReport_CreateNewEffect_Feature_Dat
 void PIDReportHandler::UppackUsbData(uint8_t* data, uint16_t len)
 {
 	//Serial.print("len:");
-	//Serial.println(len);
+	//Serial.print(len);
+	//Serial.print("=");
     //for (uint16_t i=0; i<len; ++i) {
     //  if (data[i] < 0xA0) {
     //    Serial.print(" ");

src/DynamicHID/PIDReportHandler.h

@@ -1,5 +1,11 @@
 #ifndef _PIDREPORTHANDLER_H
 #define _PIDREPORTHANDLER_H
+
+// Bit-masks for effect states
+#define MDEVICESTATE_FREE   0x00
+#define MDEVICESTATE_PAUSED 0x01
+#define MDEVICESTATE_SPRING 0x02
+
 #include <Arduino.h>
 #include "PIDReportType.h"
 
@@ -10,7 +16,7 @@ class PIDReportHandler {
 	// Effect management
 	volatile uint8_t nextEID;  // FFP effect indexes starts from 1
 	volatile TEffectState  g_EffectStates[MAX_EFFECTS + 1];
-	volatile uint8_t devicePaused;
+	volatile uint8_t deviceState;
 	//variables for storing previous values
 	volatile int32_t inertiaT = 0;
 	volatile int16_t oldSpeed = 0;

src/Joystick.cpp

@@ -513,28 +513,28 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect, EffectParams _e
 	switch (effect.effectType)
     {
 	    case USB_EFFECT_CONSTANT://1
-	        force = ConstantForceCalculator(effect) * constantGain * angle_ratio;
+	        force = ConstantForceCalculator(effect) * m_gains[axis].constantGain * angle_ratio;
 	        break;
 	    case USB_EFFECT_RAMP://2
-	    	force = RampForceCalculator(effect) * rampGain * angle_ratio;
+	    	force = RampForceCalculator(effect) * m_gains[axis].rampGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_SQUARE://3
-	    	force = SquareForceCalculator(effect) * squareGain * angle_ratio;
+	    	force = SquareForceCalculator(effect) * m_gains[axis].squareGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_SINE://4
-	    	force = SinForceCalculator(effect) * sineGain * angle_ratio;
+	    	force = SinForceCalculator(effect) * m_gains[axis].sineGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_TRIANGLE://5
-	    	force = TriangleForceCalculator(effect) * triangleGain * angle_ratio;
+	    	force = TriangleForceCalculator(effect) * m_gains[axis].triangleGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_SAWTOOTHDOWN://6
-	    	force = SawtoothDownForceCalculator(effect) * sawtoothdownGain * angle_ratio;
+	    	force = SawtoothDownForceCalculator(effect) * m_gains[axis].sawtoothdownGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_SAWTOOTHUP://7
-	    	force = SawtoothUpForceCalculator(effect) * sawtoothupGain * angle_ratio;
+	    	force = SawtoothUpForceCalculator(effect) * m_gains[axis].sawtoothupGain * angle_ratio;
 	    	break;
 	    case USB_EFFECT_SPRING://8
-	    	force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.springPosition, m_effect_params[axis].springMaxPosition), condition) * angle_ratio * springGain;
+	    	force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.springPosition, m_effect_params[axis].springMaxPosition), condition) * angle_ratio * m_gains[axis].springGain;
 	    	break;
 	    case USB_EFFECT_DAMPER://9
 	    	force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.damperVelocity, m_effect_params[axis].damperMaxVelocity), condition) * angle_ratio;
@@ -543,21 +543,21 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect, EffectParams _e
                 Interpolation::CatmullSpline(damperSplinePoints[0], damperSplinePoints[1], damperSplineNumPoints, abs(force))
                 /10000.0);
             #else
-	    	force = force * damperGain;
+	    	force = force * m_gains[axis].damperGain;
             #endif
             force = damperFilter[axis].update(force);
 	    	break;
 	    case USB_EFFECT_INERTIA://10
 	    	if (_effect_params.inertiaAcceleration < 0 && _effect_params.frictionPositionChange < 0) {
-	    		force = ConditionForceCalculator(effect, abs(NormalizeRange(_effect_params.inertiaAcceleration, m_effect_params[axis].inertiaMaxAcceleration)), condition) * angle_ratio * inertiaGain;
+	    		force = ConditionForceCalculator(effect, abs(NormalizeRange(_effect_params.inertiaAcceleration, m_effect_params[axis].inertiaMaxAcceleration)), condition) * angle_ratio * m_gains[axis].inertiaGain;
 	    	}
 	    	else if (_effect_params.inertiaAcceleration < 0 && _effect_params.frictionPositionChange > 0) {
-	    		force = -1 * ConditionForceCalculator(effect, abs(NormalizeRange(_effect_params.inertiaAcceleration, m_effect_params[axis].inertiaMaxAcceleration)), condition) * angle_ratio * inertiaGain;
+	    		force = -1 * ConditionForceCalculator(effect, abs(NormalizeRange(_effect_params.inertiaAcceleration, m_effect_params[axis].inertiaMaxAcceleration)), condition) * angle_ratio * m_gains[axis].inertiaGain;
 	    	}
             force = inertiaFilter[axis].update(force);
 	    	break;
 	    case USB_EFFECT_FRICTION://11
-	    	force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.frictionPositionChange, m_effect_params[axis].frictionMaxPositionChange), condition) * angle_ratio * frictionGain;
+	    	force = ConditionForceCalculator(effect, NormalizeRange(_effect_params.frictionPositionChange, m_effect_params[axis].frictionMaxPositionChange), condition) * angle_ratio * m_gains[axis].frictionGain;
             force = frictionFilter[axis].update(force);
 	    	break;
 	    case USB_EFFECT_CUSTOM://12
@@ -567,11 +567,21 @@ int32_t Joystick_::getEffectForce(volatile TEffectState& effect, EffectParams _e
 		return force;
 }
 
-
 void Joystick_::forceCalculator(int32_t* forces) {
     forces[0] = 0;
     forces[1] = 0;
-        int32_t force = 0;
+
+    // If the device is in default auto spring effect lets calculate it
+    if (DynamicHID().pidReportHandler.deviceState == MDEVICESTATE_SPRING)
+    {
+        for (int axis = 0; axis < FFB_AXIS_COUNT; ++axis)
+        {
+	    	forces[axis] = (int32_t)(NormalizeRange(m_effect_params[axis].springPosition, m_effect_params[axis].springMaxPosition) * -10000 * m_gains[axis].defaultSpringGain); // TODO
+        }
+    }
+    else
+    {
+
 	    for (int id = 0; id < MAX_EFFECTS; id++) {
 	    	volatile TEffectState& effect = DynamicHID().pidReportHandler.g_EffectStates[id];
 
@@ -591,7 +601,7 @@ void Joystick_::forceCalculator(int32_t* forces) {
 	    	    (effect.elapsedTime >= 0) &&
                 // dont calculate effects that have already finished
 	    	    (effect.elapsedTime <= effect.duration) &&
-	    		!DynamicHID().pidReportHandler.devicePaused)
+	    		!DynamicHID().pidReportHandler.deviceState)
             {
                 // if this is a directional conditional calculate the conditional parameters
                 // as the length in the direction of its angle. This is the same as the dot product of the vectors
@@ -629,10 +639,11 @@ void Joystick_::forceCalculator(int32_t* forces) {
             //    Serial.println(effect.totalDuration);
             //}
 	    }
+    }
 
     for (int axis = 0; axis < FFB_AXIS_COUNT; ++axis)
     {
-        forces[axis] = (int32_t)((float)totalGain * forces[axis]); // each effect gain * total effect gain = 10000
+        forces[axis] = (int32_t)((float)m_gains[axis].totalGain * forces[axis]); // each effect gain * total effect gain = 10000
         forces[axis] = constrain(forces[axis], -10000, 10000);
     }
 }

src/Joystick.h

@@ -55,23 +55,24 @@
 #define JOYSTICK_TYPE_GAMEPAD              0x05
 #define JOYSTICK_TYPE_MULTI_AXIS           0x08
 
-#define FORCE_FEEDBACK_MAXGAIN              100
+#define FORCE_FEEDBACK_MAXGAIN              1.0
 #define DEG_TO_RAD              ((float)((float)3.14159265359 / 180.0))
 
 struct Gains{
-    uint8_t totalGain         = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t constantGain      = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t rampGain          = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t squareGain        = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t sineGain          = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t triangleGain      = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t sawtoothdownGain  = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t sawtoothupGain    = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t springGain        = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t damperGain        = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t inertiaGain       = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t frictionGain      = FORCE_FEEDBACK_MAXGAIN;
-	uint8_t customGain        = FORCE_FEEDBACK_MAXGAIN;
+    float totalGain         = FORCE_FEEDBACK_MAXGAIN;
+	float constantGain      = FORCE_FEEDBACK_MAXGAIN;
+	float rampGain          = FORCE_FEEDBACK_MAXGAIN;
+	float squareGain        = FORCE_FEEDBACK_MAXGAIN;
+	float sineGain          = FORCE_FEEDBACK_MAXGAIN;
+	float triangleGain      = FORCE_FEEDBACK_MAXGAIN;
+	float sawtoothdownGain  = FORCE_FEEDBACK_MAXGAIN;
+	float sawtoothupGain    = FORCE_FEEDBACK_MAXGAIN;
+	float springGain        = FORCE_FEEDBACK_MAXGAIN;
+	float damperGain        = FORCE_FEEDBACK_MAXGAIN;
+	float inertiaGain       = FORCE_FEEDBACK_MAXGAIN;
+	float frictionGain      = FORCE_FEEDBACK_MAXGAIN;
+	float customGain        = FORCE_FEEDBACK_MAXGAIN;
+	float defaultSpringGain = 0.0;
 };
 
 struct EffectParams{
@@ -131,6 +132,9 @@ class Joystick_
 	uint8_t                  _hidReportId;
 	uint8_t                  _hidReportSize; 
 
+	//force feedback gain
+	Gains m_gains[FFB_AXIS_COUNT];
+
 	//force feedback effect params
 	EffectParams* m_effect_params;
 
@@ -171,7 +175,6 @@ class Joystick_
 
 	void begin(bool initAutoSendState = true);
 	void end();
-	
 	// Set Range Functions
 	inline void setXAxisRange(int16_t minimum, int16_t maximum)
 	{
@@ -235,6 +238,20 @@ class Joystick_
 
 	//force feedback Interfaces
 	void getForce(int32_t* forces);
+
+
+    Gains *getGains() {
+        return m_gains;
+    }
+
+	int8_t setGains(Gains* _gains){
+	    if(_gains != nullptr){
+            memcpy(m_gains, _gains, sizeof(m_gains));
+	        return 0;
+	    }
+	    return -1;
+	};
+
 	//set effect params funtions
 	int8_t setEffectParams(EffectParams* _effect_params){
 	    if(_effect_params != nullptr){