From c07c44dc2a27671d9c82b874fd27f1a3576049de Mon Sep 17 00:00:00 2001 From: Thomas Pietrzak Date: Mon, 22 May 2017 21:35:22 +0200 Subject: [PATCH] split in modules --- BowModel.cpp | 22 ++++++++ BowModel.h | 22 ++++++++ ForceModel.cpp | 11 ++++ ForceModel.h | 14 ++++++ Makefile | 7 ++- PluckModel.cpp | 21 ++++++++ PluckModel.h | 24 +++++++++ SpringModel.cpp | 15 ++++++ SpringModel.h | 21 ++++++++ capacitivetouch.c | 66 ++++++++++++++++++++++++ capacitivetouch.h | 11 ++++ forcefader.c | 124 ---------------------------------------------- forcefader.cpp | 60 ++++++++++++++++++++++ forcemotor.c | 49 ++++++++++++++++++ forcemotor.h | 12 +++++ 15 files changed, 354 insertions(+), 125 deletions(-) create mode 100644 BowModel.cpp create mode 100644 BowModel.h create mode 100644 ForceModel.cpp create mode 100644 ForceModel.h create mode 100644 PluckModel.cpp create mode 100644 PluckModel.h create mode 100644 SpringModel.cpp create mode 100644 SpringModel.h create mode 100644 capacitivetouch.c create mode 100644 capacitivetouch.h delete mode 100644 forcefader.c create mode 100644 forcefader.cpp create mode 100644 forcemotor.c create mode 100644 forcemotor.h 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 -- 2.30.2