Skip to content

Commit

Permalink
Default spring (jmriego#18)
Browse files Browse the repository at this point in the history 
* 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]>
  • Loading branch information
@jmriego
3 people authored Nov 6, 2021
1 parent 1e7ba75 commit c326572
Show file tree
Hide file tree
Showing 9 changed files with 141 additions and 69 deletions.
36 changes: 36 additions & 0 deletions .github/workflows/arduino.yml
View file
Original file line number Diff line number Diff line change
@@ -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


2 changes: 1 addition & 1 deletion Fino.ino
View file
Original file line number Diff line number Diff line change
@@ -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"
Expand Down
16 changes: 13 additions & 3 deletions com.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 0 additions & 12 deletions config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion order.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,8 @@ enum Order {
POSITION = 4,
ERROR = 5,
LOG = 6,
VERSION = 7
VERSION = 7,
CONFIG = 8
};

typedef enum Order Order;
Expand Down
43 changes: 23 additions & 20 deletions src/DynamicHID/PIDReportHandler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
PIDReportHandler::PIDReportHandler()
{
nextEID = 1;
devicePaused = 0;
deviceState = MDEVICESTATE_SPRING;
}

PIDReportHandler::~PIDReportHandler()
Expand Down Expand Up @@ -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))
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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(" ");
Expand Down
8 changes: 7 additions & 1 deletion src/DynamicHID/PIDReportHandler.h
View file
Original file line number Diff line number Diff line change
@@ -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"

Expand All @@ -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;
Expand Down
43 changes: 27 additions & 16 deletions src/Joystick.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Expand All @@ -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];

Expand All @@ -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
Expand Down Expand Up @@ -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);
}
}
Expand Down
47 changes: 32 additions & 15 deletions src/Joystick.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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){
Expand Down

0 comments on commit c326572

Please sign in to comment.