From: Thomas Pietrzak Date: Mon, 22 May 2017 19:35:22 +0000 (+0200) Subject: split in modules X-Git-Url: https://git.thomaspietrzak.com/?a=commitdiff_plain;h=c07c44dc2a27671d9c82b874fd27f1a3576049de;p=forcefader.git split in modules --- diff --git a/BowModel.cpp b/BowModel.cpp new file mode 100644 index 0000000..8670a45 --- /dev/null +++ b/BowModel.cpp @@ -0,0 +1,22 @@ +#include "BowModel.h" + +#include + +BowModel::BowModel(float threshold, float k) +: ForceModel(), _threshold(threshold), _k(k), _stickpos(0.0) +{ +} + +float BowModel::computeForce(float pos) +{ + float f = _k * (pos - _stickpos); + if (fabs(f) > _threshold) + { + f = 0.0; + _stickpos = pos; + } + _lastforce = f; + //_lastspeed = (pos - lastpos) / time; + _lastpos = pos; + return f; +} diff --git a/BowModel.h b/BowModel.h new file mode 100644 index 0000000..fa78a67 --- /dev/null +++ b/BowModel.h @@ -0,0 +1,22 @@ +#pragma once + +#include "ForceModel.h" + +class BowModel: public ForceModel +{ + public: + BowModel(float threshold, float k); + + float computeForce(float pos); + + float getThreshold() { return _threshold; } + float getK() { return _k; } + + void setThreshold(float threshold) { _threshold = threshold; } + void setK(float k) { _k = k; } + + private: + float _threshold; + float _k; + float _stickpos; +}; diff --git a/ForceModel.cpp b/ForceModel.cpp new file mode 100644 index 0000000..4a96f5e --- /dev/null +++ b/ForceModel.cpp @@ -0,0 +1,11 @@ +#include "ForceModel.h" + +ForceModel::ForceModel() +: _lastpos(0.0), _lastforce(0.0)//, _lastspeed(0.0) +{ +} + +float ForceModel::computeForce(float pos) +{ + return 0.0; +} \ No newline at end of file diff --git a/ForceModel.h b/ForceModel.h new file mode 100644 index 0000000..86f1e9d --- /dev/null +++ b/ForceModel.h @@ -0,0 +1,14 @@ +#pragma once + +class ForceModel { + public: + virtual ~ForceModel() {} + + virtual float computeForce(float pos); + + protected: + ForceModel(); + float _lastpos; + float _lastforce; + //float _lastspeed; +}; diff --git a/Makefile b/Makefile index 58f0789..87287d5 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,8 @@ export BOARD = stm32f429discovery ROOTDIR = $(CURDIR)/uC-sdk +export VERBOSE = true + ifeq ($(wildcard $(ROOTDIR)/uC-sdk.root),) ifneq ($(wildcard ../../../uC-sdk.root),) ROOTDIR = ../../.. @@ -8,7 +10,7 @@ endif endif TARGET = forcefader.bin -TARGET_SRCS = forcefader.c +TARGET_SRCS = forcefader.cpp capacitivetouch.c forcemotor.c ForceModel.cpp SpringModel.cpp BowModel.cpp PluckModel.cpp LIBDEPS = \ $(ROOTDIR)/FreeRTOS/libFreeRTOS.a \ @@ -24,6 +26,9 @@ TARGET_INCLUDES = include include $(ROOTDIR)/common.mk +TARGET_CXX += -std=c++11 +#TARGET_LD = $(TOOLCHAIN)-g++ + all: uC-sdk $(TARGET) clean: clean-generic diff --git a/PluckModel.cpp b/PluckModel.cpp new file mode 100644 index 0000000..1fe6593 --- /dev/null +++ b/PluckModel.cpp @@ -0,0 +1,21 @@ +#include "PluckModel.h" + +#include + +PluckModel::PluckModel(float pos, float delta, float k) +: ForceModel(), _pos(pos), _delta(delta), _k(k) +{ +} + +float PluckModel::computeForce(float pos) +{ + float f; + if (fabs(pos - _pos) < _delta) + f = _k * (pos - _pos); + else + f = 0.0; + _lastforce = f; + //_lastspeed = (pos - lastpos) / time; + _lastpos = pos; + return f; +} diff --git a/PluckModel.h b/PluckModel.h new file mode 100644 index 0000000..0633f6d --- /dev/null +++ b/PluckModel.h @@ -0,0 +1,24 @@ +#pragma once + +#include "ForceModel.h" + +class PluckModel: public ForceModel +{ + public: + PluckModel(float pos, float delta, float k); + + float computeForce(float pos); + + float getPos() { return _pos; } + float getDelta() { return _delta; } + float getK() { return _k; } + + void setPos(float pos) { _pos = pos; } + void setDelta(float delta) { _delta = delta; } + void setK(float k) { _k = k; } + + private: + float _pos; + float _delta; + float _k; +}; diff --git a/SpringModel.cpp b/SpringModel.cpp new file mode 100644 index 0000000..08078aa --- /dev/null +++ b/SpringModel.cpp @@ -0,0 +1,15 @@ +#include "SpringModel.h" + +SpringModel::SpringModel(float pos, float k) +: ForceModel(), _pos(pos), _k(k) +{ +} + +float SpringModel::computeForce(float pos) +{ + float f = _k * (pos - _pos); + _lastforce = f; + //_lastspeed = (pos - lastpos) / time; + _lastpos = pos; + return f; +} diff --git a/SpringModel.h b/SpringModel.h new file mode 100644 index 0000000..c2a751a --- /dev/null +++ b/SpringModel.h @@ -0,0 +1,21 @@ +#pragma once + +#include "ForceModel.h" + +class SpringModel: public ForceModel +{ + public: + SpringModel(float pos, float k); + + float computeForce(float pos); + + float getPos() { return _pos; } + float getK() { return _k; } + + void setPos(float pos) { _pos = pos; } + void setK(float k) { _k = k; } + + private: + float _pos; + float _k; +}; diff --git a/capacitivetouch.c b/capacitivetouch.c new file mode 100644 index 0000000..236b45a --- /dev/null +++ b/capacitivetouch.c @@ -0,0 +1,66 @@ +#include "capacitivetouch.h" + +#include +#include + +static timer_channel_t capacitiveloop_timer = { .timer = timer_2, .channel = 1 }; +static timer_channel_t capacitivemeasure_timer = { .timer = timer_4, .channel = 1 }; + +uint16_t capacitance = 0; + +static uint8_t measuring = 0; + +static pin_t touch_TX_pin, touch_RX_pin; + +static void capacitiveRX() +{ + if (measuring) + { + capacitance = timer_get_count(timer_4); +// printf("capa : %8d\n", capacitance); + gpio_set(touch_TX_pin, 0); + measuring = 0; + } +} + +static void capacitiveTimeout() +{ + if (measuring) + { + capacitance = 0xffff; +// printf("capa : timeout\n"); + gpio_set(touch_TX_pin, 0); + measuring = 0; + } +} + +static void capacitiveTX() +{ + if (!measuring) + { + timer_set_count(timer_4, 0); + gpio_set(touch_TX_pin, 1); + measuring = 1; + } +} + +void init_capacitive_touch() +{ + touch_TX_pin = make_pin(gpio_port_c, 12); + touch_RX_pin = make_pin(gpio_port_g, 11); + + gpio_config(touch_TX_pin, pin_dir_write, pull_down); + gpio_config(touch_RX_pin, pin_dir_read, pull_none); + gpio_set(touch_TX_pin, 0); + + //start capacitive measure + timer_config(timer_2, 1050, 1000); + timer_irq_init(capacitiveloop_timer, event_update, capacitiveTX); + + //capacitive counter + timer_config(timer_4, 84, 4000); + //timer_disable(timer_4); + timer_irq_init(capacitivemeasure_timer, event_update, capacitiveTimeout); + + gpio_irq_init(touch_RX_pin, capacitiveRX, rising); +} \ No newline at end of file diff --git a/capacitivetouch.h b/capacitivetouch.h new file mode 100644 index 0000000..6cd456d --- /dev/null +++ b/capacitivetouch.h @@ -0,0 +1,11 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +void init_capacitive_touch(); + +#ifdef __cplusplus +} +#endif diff --git a/forcefader.c b/forcefader.c deleted file mode 100644 index ec2da76..0000000 --- a/forcefader.c +++ /dev/null @@ -1,124 +0,0 @@ -#include -#include -#include -#include - -#define POSMIN 0 -#define POSMAX 4095 - -#define THRESHOLD 90 - -uint16_t faderpos; -pin_t faderpos_pin, motorsignal_pin, motordir1_pin, motordir2_pin, fadertouch_pin, touch_TX_pin, touch_RX_pin; - -timer_channel_t forceloop_timer = { .timer = 1, .channel = 1 }; -timer_channel_t capacitiveloop_timer = { .timer = 2, .channel = 1 }; -timer_channel_t motorsignal_timer = { .timer = 3, .channel = 3 }; -timer_channel_t capacitivemeasure_timer = { .timer = 4, .channel = 1 }; - -uint16_t capacitance = 0; - -void setSpringForce(uint16_t pos, uint8_t k) -{ - uint32_t force; - if (faderpos > pos) - { - gpio_set(motordir1_pin, 0); - gpio_set(motordir2_pin, 1); - force = ((uint32_t)(faderpos - pos) * k) / 4096; - } - else - { - gpio_set(motordir1_pin, 1); - gpio_set(motordir2_pin, 0); - force = ((uint32_t)(pos - faderpos) * k) / 4096; - } - //printf("force 1: %8d\n", force); - timer_pwmchannel_init(motorsignal_timer, motorsignal_pin, force); -} - -void updateForce() -{ - if (capacitance > THRESHOLD) - setSpringForce(2048, 255); -} - -uint8_t measuring = 0; - -void capacitiveRX() -{ - if (measuring) - { - capacitance = timer_get_count(timer_4); - //printf("capa : %8d\n", c); - gpio_set(touch_TX_pin, 0); - measuring = 0; - } -} - -void capacitiveTimeout() -{ - if (measuring) - { - capacitance = 0xffff; - //printf("capa : timeout\n"); - gpio_set(touch_TX_pin, 0); - measuring = 0; - } -} - -void capacitiveTX() -{ - if (!measuring) - { - timer_set_count(timer_4, 0); - gpio_set(touch_TX_pin, 1); - measuring = 1; - } -} - -int main() -{ - faderpos_pin = make_pin(gpio_port_a, 0); //ADC1 CH0 - motorsignal_pin = make_pin(gpio_port_b, 0); //TIMER3 CH3 - motordir1_pin = make_pin(gpio_port_e, 6); - motordir2_pin = make_pin(gpio_port_f, 4); - touch_TX_pin = make_pin(gpio_port_c, 12); - touch_RX_pin = make_pin(gpio_port_g, 11); - - faderpos = 0; - uint8_t channel = 0; - adc_config_continuous(adc_1, &channel, &faderpos_pin, &faderpos, 1); - - gpio_config(motordir1_pin, pin_dir_write, pull_none); - gpio_config(motordir2_pin, pin_dir_write, pull_none); - - gpio_config(touch_TX_pin, pin_dir_write, pull_down); - gpio_config(touch_RX_pin, pin_dir_read, pull_none); - gpio_set(touch_TX_pin, 0); - - //compute new force - timer_config(timer_1, 42, 1000); - timer_irq_init(forceloop_timer, event_update, updateForce); - - //start capacitive measure - timer_config(timer_2, 1050, 1000); - timer_irq_init(capacitiveloop_timer, event_update, capacitiveTX); - - //motor amplitude - timer_config(timer_3, 8, 255); - timer_pwmchannel_init(motorsignal_timer, motorsignal_pin, 0); - - //capacitive counter - timer_config(timer_4, 84, 4000); - //timer_disable(timer_4); - timer_irq_init(capacitivemeasure_timer, event_update, capacitiveTimeout); - - gpio_irq_init(touch_RX_pin, capacitiveRX, rising); - - //Loop - while (1); - //printf("pos : %4d\n", faderpos); - - return 0; -} diff --git a/forcefader.cpp b/forcefader.cpp new file mode 100644 index 0000000..5fdf57f --- /dev/null +++ b/forcefader.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include + +#include "ForceModel.h" +#include "SpringModel.h" +#include "BowModel.h" +#include "PluckModel.h" + +#include "forcemotor.h" +#include "capacitivetouch.h" + +#define POSMAX 4095 //12 bits ADC + +#define TRACKLENGTH 0.1 //10cm + +#define CAPACITIVETHRESHOLD 90 + +uint16_t faderpos; +pin_t faderpos_pin; + +timer_channel_t forceloop_timer = { .timer = timer_1, .channel = 1 }; + +ForceModel *fm = NULL; + +void updateForce() +{ + if (!fm)// || capacitance < CAPACITIVETHRESHOLD) + return; + + float fp = (faderpos * TRACKLENGTH) / POSMAX; + set_force(fm->computeForce(fp)); +} + +int main() +{ + //fm = new SpringModel(0.05, 5); + //fm = new BowModel(0.01, 5); + fm = new PluckModel(0.05, 0.02, 5); + + faderpos_pin = make_pin(gpio_port_a, 0); //ADC1 CH0 + + faderpos = 0; + uint8_t channel = 0; + adc_config_continuous(adc_1, &channel, &faderpos_pin, &faderpos, 1); + + //compute new forces + timer_config(timer_1, 42, 1000); + timer_irq_init(forceloop_timer, event_update, updateForce); + + init_capacitive_touch(); + init_force_motor(); + + //Loop + while (1); + //printf("pos : %4d\n", faderpos); + + return 0; +} diff --git a/forcemotor.c b/forcemotor.c new file mode 100644 index 0000000..83d1e65 --- /dev/null +++ b/forcemotor.c @@ -0,0 +1,49 @@ +#include "forcemotor.h" + +#include +//#include + +#include +#include + +#define FORCEMAX 0.1 //100gF +#define COMMANDMAX 255 //255 levels of force + +pin_t motorsignal_pin, motordir1_pin, motordir2_pin; + +timer_channel_t motorsignal_timer = { .timer = timer_3, .channel = 3 }; + +void init_force_motor() +{ + motorsignal_pin = make_pin(gpio_port_b, 0); //TIMER3 CH3 + motordir1_pin = make_pin(gpio_port_e, 6); + motordir2_pin = make_pin(gpio_port_f, 4); + + gpio_config(motordir1_pin, pin_dir_write, pull_none); + gpio_config(motordir2_pin, pin_dir_write, pull_none); + + //motor amplitude + timer_config(timer_3, 8, COMMANDMAX); + timer_pwmchannel_init(motorsignal_timer, motorsignal_pin, 0); +} + +void set_force(float force) +{ + uint32_t f; + if (fabs(force) > FORCEMAX) + f = COMMANDMAX; + else + f = fabs(force) * COMMANDMAX / FORCEMAX; + if (force > 0) + { + gpio_set(motordir1_pin, 0); + gpio_set(motordir2_pin, 1); + } + else + { + gpio_set(motordir1_pin, 1); + gpio_set(motordir2_pin, 0); + } + //printf("force %lu %f\n", f, force); + timer_pwmchannel_init(motorsignal_timer, motorsignal_pin, f); +} diff --git a/forcemotor.h b/forcemotor.h new file mode 100644 index 0000000..7e03f65 --- /dev/null +++ b/forcemotor.h @@ -0,0 +1,12 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +void init_force_motor(); +void set_force(float force); + +#ifdef __cplusplus +} +#endif