Revision 0:05dd1de8cc3f, committed 2019-05-03

Comitter:

altb2

Date:

Fri May 03 09:37:27 2019 +0000

Child:

1:92f175969d90

Commit message:

Final Template Version for Lab #7

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Fri May 03 09:37:27 2019 +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();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,34 @@
+/*
+ * 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 May 03 09:37:27 2019 +0000
@@ -0,0 +1,52 @@
+#include "LinearCharacteristics.h"
+
+using namespace std;
+
+LinearCharacteristics::LinearCharacteristics(float gain,float offset){    // standard lin characteristics
+    this->gain = gain;
+    this->offset = offset;
+    this->ulim = 999999.0;
+    this->llim = -999999.0;
+}
+
+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;
+    }
+
+void LinearCharacteristics::setup(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;   
+}
+void LinearCharacteristics::setup(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; 
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.h	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,33 @@
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+
+
+#ifndef LINEAR_CHARACTERISTICS_H_
+#define LINEAR_CHARACTERISTICS_H_   
+
+
+class LinearCharacteristics{
+     public:
+            LinearCharacteristics(){};
+            LinearCharacteristics(float, float);
+            LinearCharacteristics(float, float, float, float);
+            LinearCharacteristics(float, float, float, float, float, float);
+            float evaluate(float);
+            void setup(float, float, float, float);
+            void setup(float, float, float, float, float, 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_

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Cntrl.cpp	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,33 @@
+/*  
+    PI Controller class 
+    
+                    1          s             
+      G(s) = P + I --- + D --------- 
+                    s      T_f*s + p              
+*/
+
+#include "PID_Cntrl.h"
+using namespace std;
+
+PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax)
+{
+// ....
+
+    reset(0.0f);
+}
+
+PID_Cntrl::~PID_Cntrl() {}
+
+void PID_Cntrl::reset(float initValue)
+{
+    // here code for resetting variables
+
+}
+
+float PID_Cntrl::update(double e)
+{
+    // here the main code!!!
+    
+    return 0.0f;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_Cntrl.h	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,25 @@
+#ifndef PID_CNTRL_H_
+#define PID_CNTRL_H_
+
+// PID Controller Class (Template)
+class PID_Cntrl
+{
+public:
+
+    PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMin, float uMax);
+
+    float operator()(float error) {
+        return update((double)error);
+    }
+
+    virtual     ~PID_Cntrl();
+
+    void        reset(float initValue);
+    float       update(double error);
+    
+private:
+
+    // here some local variables are defined
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "EncoderCounter.h"
+#include "LinearCharacteristics.h"
+//------------------------------------------
+#define PI 3.1415927f
+//------------------------------------------
+ 
+/* GRT: Control of voice-coil with PID-T1 - Controller
+ */
+Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
+InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
+AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 4A (see ESCON)
+
+bool key_was_pressed = false;
+bool controller_active = false;
+void pressed(void);                     // user Button pressed
+void released(void);                    // user Button released
+  
+//------------------------------------------
+// ... here define variables like gains etc.
+//------------------------------------------
+LinearCharacteristics i2u(-4.0f,4.0f,0.0f,3.2f / 3.3f);         // output is normalized output
+//------------------------------------------
+Ticker  ControllerLoopTimer;            // interrupt for control loop
+EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
+Timer t_but;                            // define timer for button
+// ----- User defined functions -----------
+void updateLoop(void);   // loop for State machine (via interrupt)
+float Ts = 0.0005f;                     // sample time of main loop
+uint16_t k = 0;
+//******************************************************************************
+//---------- main loop -------------
+//******************************************************************************
+int main()
+{   
+    pc.baud(115200);   // for serial comm.
+    counter1.reset();   // encoder reset
+    out.write(i2u(0.0));
+    button.fall(&pressed);          // attach key pressed function
+    button.rise(&released);         // attach key pressed function
+    pc.printf("Start controller now...\r\n");
+    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
+    while(1);
+}   // END OF main
+//******************************************************************************
+//---------- main loop (called via interrupt) -------------
+//******************************************************************************
+void updateLoop(void){
+    float x = (float)(counter1)/1000.0f;  // get counts from Encoder
+    float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
+    if(controller_active)
+        {   
+        // here calculate controller's output
+            }
+    out.write(i2u(i_des));
+    if(++k>1000)
+    {
+        pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
+        k=0;
+        }    
+} // END OF updateLoop(void)
+ 
+//******************************************************************************
+ 
+ 
+// start timer as soon as Button is pressed
+void pressed()
+{
+    t_but.start();
+}
+// Falling edge of button: enable/disable controller 
+void released()
+{
+    // readout, stop and reset timer
+    float ButtonTime = t_but.read();
+    t_but.stop();
+    t_but.reset();
+    if(ButtonTime > 0.05f) 
+    {   
+        controller_active = !controller_active;
+        if(controller_active)
+            {
+            pc.printf("Controller active\r\n");
+            // reset controller here!!!        
+            }
+        else
+            pc.printf("Controller disabled\r\n");
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 03 09:37:27 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file