Revision 0:d784b08f51ff, committed 2018-09-28

Comitter:

altb

Date:

Fri Sep 28 12:42:32 2018 +0000

Child:

1:8725adf9efd0

Commit message:

Misc

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,40 @@
+/*  
+    DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
+    
+              b*(1 - z^-1)                      s
+      G(z) = -------------  <-- tustin --  ----------- = G(s)
+              1 - a*z^-1                      T*s + 1
+*/
+
+#include "DiffCounter.h"
+#define   pi 3.141592653589793
+using namespace std;
+
+DiffCounter::DiffCounter(float T, float Ts)
+{   
+    b = 2.0/(2.0*(double)T + (double)Ts);
+    a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
+    incPast = 0;
+    vel = 0.0;
+    inc2rad = 2.0*pi/(4.0*6400.0);   // incr encoder with 6400inc/rev
+}
+
+DiffCounter::~DiffCounter() {}
+
+void DiffCounter::reset(float initValue, short inc)
+{
+    vel = (double)initValue;
+    incPast = inc;
+}
+
+float DiffCounter::doStep(short inc)
+{
+    long del = (long)(inc - incPast);
+    incPast = inc;
+    if(del < -16000)
+        del += 0xFFFF;
+    if(del > 16000)
+        del -= 0xFFFF;
+    vel = b*(double)del*inc2rad - a*vel;
+    return (float)vel;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.h	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef DIFFCOUNTER_H_
+#define DIFFCOUNTER_H_
+
+class DiffCounter
+{
+public:
+
+    DiffCounter(float T, float Ts);
+    
+    float operator()(short inc) {
+        return doStep(inc);
+    }
+    
+    virtual     ~DiffCounter();
+    
+    void        reset(float initValue, short inc);
+    float       doStep(short inc);
+
+private:
+
+    double b;
+    double a;
+    short incPast;
+    double vel;
+    double inc2rad;
+
+};
+
+#endif /* DIFFCOUNTER_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,139 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+    
+    // check pins
+    
+    if ((a == PA_6) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOC->MODER &= ~GPIO_MODER_MODER7;     // reset port C7
+        GPIOC->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port C7
+        GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port C7
+        GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOC->AFR[0] &= ~0xF0000000;           // reset alternate function of port C7
+        GPIOC->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port C7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+    } else if ((a == PB_6) && (b == PB_7)) {
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER6;     // reset port B6
+        GPIOB->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port B6
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port B6
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port B6
+        GPIOB->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port B6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER7;     // reset port B7
+        GPIOB->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port B7
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port B7
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B7
+        GPIOB->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port B7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST;  //reset TIM4 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;     // TIM4 clock enable
+        
+    } else {
+        
+        printf("pinmap not found for peripheral\n");
+    }
+    
+    // configure general purpose timer 3 or 4
+    
+    TIM->CR1 = 0x0000;          // counter disable
+    TIM->CR2 = 0x0000;          // reset master mode selection
+    TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+    TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+    TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+    TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+    TIM->CNT = 0x0000;          // reset counter value
+    TIM->ARR = 0xFFFF;          // auto reload register
+    TIM->CR1 = TIM_CR1_CEN;     // counter enable
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(short offset) {
+    
+    TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounter::read() {
+    
+    return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+    
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+    
+    public:
+        
+                    EncoderCounter(PinName a, PinName b);
+        virtual     ~EncoderCounter();
+        void        reset();
+        void        reset(short offset);
+        short       read();
+                    operator short();
+        
+    private:
+        
+        TIM_TypeDef*    TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.cpp	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,36 @@
+#include "LinearCharacteristics.h"
+
+using namespace std;
+
+LinearCharacteristics::LinearCharacteristics(float gain,float offset){    // standard lin characteristics
+    this->gain = gain;
+    this->offset = offset;
+}
+
+LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->ulim = 999999.0;
+    this->llim = -999999.0;
+    
+}
+LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax,float ll, float ul){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->llim = ll;
+    this->ulim = ul;
+    
+}
+
+LinearCharacteristics::~LinearCharacteristics() {}
+
+
+float LinearCharacteristics::evaluate(float x)
+{   
+float dum = this->gain*(x - this->offset);
+if(dum > this->ulim)
+    dum = this->ulim;
+if(dum < this->llim)
+    dum = this->llim;
+return dum;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.h	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,30 @@
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+
+
+#ifndef LINEAR_CHARACTERISTICS_H_
+#define LINEAR_CHARACTERISTICS_H_   
+
+
+class LinearCharacteristics{
+     public:
+            LinearCharacteristics(float, float);
+            LinearCharacteristics(float, float, float, float);
+            LinearCharacteristics(float, float, float, float, float, float);
+            float evaluate(float);
+            float operator()(float x){
+                return evaluate(x);
+                } 
+                //...
+                virtual     ~LinearCharacteristics();
+                // here: the calculation function
+    
+    private:
+        // here: private functions and values...
+        float gain;
+        float offset;
+        float ulim;
+        float llim;
+};
+
+
+#endif      // LINEAR_CHARACTERISTICS_H_
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper.cpp	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,30 @@
+/*  
+*/
+
+#include "Unwrapper.h"
+#define   pi 3.141592653589793
+using namespace std;
+
+Unwrapper::Unwrapper(double i2r)
+{   
+    inc2rad = i2r;
+    last_value = 0;
+}
+
+Unwrapper::~Unwrapper() {}
+
+void Unwrapper::reset(void)
+{
+    last_value = 0;
+}
+
+double Unwrapper::doStep(short inc)
+{
+    long temp = inc;
+    if((temp - last_value) > 32000)
+        temp -= 0xFFFF;
+    else if((temp - last_value) < -32000)
+        temp += 0xFFFF;
+    last_value = temp;
+    return (temp*inc2rad);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper.h	Fri Sep 28 12:42:32 2018 +0000
@@ -0,0 +1,26 @@
+/*  
+*/
+
+using namespace std;
+
+class Unwrapper
+{
+public:
+
+    Unwrapper(double);
+    
+    double operator()(short inc) {
+        return doStep(inc);
+    }
+    
+    virtual     ~Unwrapper();
+    
+    void        reset(void);
+    double       doStep(short inc);
+
+private:
+
+    long last_value;
+    double inc2rad;
+
+};
\ No newline at end of file