Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

main.cpp@5:768e10f6d372, 2021-02-25 (annotated)

Committer:
altb2
Date:
Thu Feb 25 20:28:45 2021 +0000
Revision:
5:768e10f6d372
Parent:
4:3d8dd3d17564
Child:
6:9ebeffe446e4
first commit of Mirror actuator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a56e9c932891 1 #include "mbed.h"
altb2 0:a56e9c932891 2 #include "math.h"
altb2 0:a56e9c932891 3 //------------------------------------------
altb2 0:a56e9c932891 4 #define PI 3.1415927f
altb2 0:a56e9c932891 5 //------------------------------------------
altb2 0:a56e9c932891 6 #include "EncoderCounter.h"
altb2 5:768e10f6d372 7 #include "EncoderCounterIndex.h"
altb2 0:a56e9c932891 8 #include "DiffCounter.h"
altb2 0:a56e9c932891 9 #include "IIR_filter.h"
altb2 0:a56e9c932891 10 #include "LinearCharacteristics.h"
altb2 5:768e10f6d372 11 #include "PID_Cntrl.h"
altb2 5:768e10f6d372 12 #include "Unwrapper_2pi.h"
altb2 5:768e10f6d372 13 #include "path_1d.h"
altb2 5:768e10f6d372 14 #include "GPA.h"
altb2 5:768e10f6d372 15 #include "ControllerLoop.h"
altb2 5:768e10f6d372 16
altb2 5:768e10f6d372 17
altb2 0:a56e9c932891 18
altb2 5:768e10f6d372 19 Serial pc(USBTX, USBRX,115200);
altb2 5:768e10f6d372 20 InterruptIn button(USER_BUTTON); // User Button, short and long presses!
altb2 0:a56e9c932891 21 bool key_was_pressed = false;
altb2 5:768e10f6d372 22 float Ts=.001; // sampling time
altb2 5:768e10f6d372 23 void pressed(void);
altb2 5:768e10f6d372 24 void released(void);
altb2 5:768e10f6d372 25 float vel;
altb2 5:768e10f6d372 26 //------------- DEFINE FILTERS ----------------
altb2 5:768e10f6d372 27 // missing
altb2 5:768e10f6d372 28 //-----------------------------------
altb2 5:768e10f6d372 29 Timer glob_ti;
altb2 5:768e10f6d372 30 EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7
altb2 5:768e10f6d372 31 InterruptIn indexpulse1(PA_8);
altb2 5:768e10f6d372 32 EncoderCounterIndex index1(counter1,indexpulse1);
altb2 5:768e10f6d372 33 //
altb2 5:768e10f6d372 34 EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 5:768e10f6d372 35 InterruptIn indexpulse2(PB_4);
altb2 5:768e10f6d372 36 EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7
altb2 5:768e10f6d372 37 //
altb2 5:768e10f6d372 38 DiffCounter diff1(0.0008,Ts); // discrete differentiate, based on encoder1 data
altb2 5:768e10f6d372 39 DiffCounter diff2(0.0008,Ts); // discrete differentiate, based on encoder2 data
altb2 5:768e10f6d372 40 LinearCharacteristics i2pwm(-1.0,1.0,0.01,0.99,.01,.99);
altb2 5:768e10f6d372 41 //
altb2 5:768e10f6d372 42 PwmOut i_des1(PB_10);
altb2 5:768e10f6d372 43 PwmOut i_des2(PA_15);
altb2 5:768e10f6d372 44 DigitalOut i_enable(PC_4);
altb2 5:768e10f6d372 45 float data[2000][4];
altb2 5:768e10f6d372 46 Unwrapper_2pi uw2pi;
altb2 0:a56e9c932891 47 //------------------------------------------
altb2 0:a56e9c932891 48 // ----- User defined functions -----------
altb2 5:768e10f6d372 49 ControllerLoop loop(Ts);
altb2 5:768e10f6d372 50 path_1d p1;
altb2 5:768e10f6d372 51 path_1d p2;
altb2 5:768e10f6d372 52 path_1d *current_path;
altb2 5:768e10f6d372 53 float A = 2.7;
altb2 5:768e10f6d372 54 float dc=0.0;
altb2 5:768e10f6d372 55 //GPA myGPA(1, 2500, 100, 30, 20, Ts);
altb2 5:768e10f6d372 56 //GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts);
altb2 5:768e10f6d372 57 float exc=0.0; // excitation GPA
altb2 5:768e10f6d372 58 // f1 f2 N A1 A2 Ts
altb2 5:768e10f6d372 59
altb2 5:768e10f6d372 60 // *****************************************************************************
altb2 5:768e10f6d372 61 void rise_edge(void)
altb2 5:768e10f6d372 62 {
altb2 5:768e10f6d372 63 //glob_ti.reset();
altb2 5:768e10f6d372 64 }
altb2 5:768e10f6d372 65 // *****************************************************************************
altb2 5:768e10f6d372 66 void fall_edge(void)
altb2 5:768e10f6d372 67 {
altb2 5:768e10f6d372 68 uint32_t time_us = glob_ti.read_us();
altb2 5:768e10f6d372 69 dc = (float)time_us/200.0f;
altb2 5:768e10f6d372 70 }
altb2 5:768e10f6d372 71 //InterruptIn i_des2(PA_6);
altb2 5:768e10f6d372 72
altb2 5:768e10f6d372 73
altb2 0:a56e9c932891 74 //******************************************************************************
altb2 0:a56e9c932891 75 //---------- main loop -------------
altb2 0:a56e9c932891 76 //******************************************************************************
altb2 0:a56e9c932891 77 int main()
altb2 0:a56e9c932891 78 {
altb2 5:768e10f6d372 79 i_des1.period_us(200);
altb2 5:768e10f6d372 80 i_des2.period_us(200); // PWM Frequency of i_desired
altb2 5:768e10f6d372 81 i_enable = 0; // disable current first
altb2 0:a56e9c932891 82 counter1.reset(); // encoder reset
altb2 5:768e10f6d372 83 counter2.reset(); // encoder reset
altb2 5:768e10f6d372 84 ThisThread::sleep_for(100);
altb2 5:768e10f6d372 85 i_enable = 1;
altb2 5:768e10f6d372 86
altb2 5:768e10f6d372 87 glob_ti.start();
altb2 5:768e10f6d372 88 glob_ti.reset();
altb2 5:768e10f6d372 89 printf("Start Mirror\r\n");
altb2 5:768e10f6d372 90 p1.initialize(300,10,A,0,0,0);
altb2 5:768e10f6d372 91 p2.initialize(300,10,-A,0,0,A);
altb2 5:768e10f6d372 92 for(int wk =0;wk<10;wk++)
altb2 5:768e10f6d372 93 {
altb2 5:768e10f6d372 94 current_path = &p1;
altb2 5:768e10f6d372 95 current_path->start(glob_ti.read());
altb2 5:768e10f6d372 96 while(!current_path->finished)
altb2 5:768e10f6d372 97 wait(.1);
altb2 5:768e10f6d372 98 current_path = &p2;
altb2 5:768e10f6d372 99 current_path->start(glob_ti.read());
altb2 5:768e10f6d372 100 while(!current_path->finished)
altb2 5:768e10f6d372 101 wait(.1);
altb2 5:768e10f6d372 102 ThisThread::sleep_for(100);
altb2 5:768e10f6d372 103 }
altb2 5:768e10f6d372 104 i_enable = 0;
altb2 5:768e10f6d372 105 while(1)
altb2 5:768e10f6d372 106 wait(1);
altb2 5:768e10f6d372 107
altb2 5:768e10f6d372 108
altb2 0:a56e9c932891 109
altb2 0:a56e9c932891 110 } // END OF main