Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

main.cpp@8:49ac75c42da0, 2021-04-27 (annotated)

Committer:
altb2
Date:
Tue Apr 27 07:50:03 2021 +0000
Revision:
8:49ac75c42da0
Parent:
7:942fd77d5e19
Child:
10:bfacffec199a
intermediate;

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 6:9ebeffe446e4 16 #include "Mirror_Kinematic.h"
altb2 6:9ebeffe446e4 17 #include "data_structs.h"
altb2 6:9ebeffe446e4 18 #include "uart_comm_thread.h"
altb2 8:49ac75c42da0 19 #include "FastPWM.h"
altb2 0:a56e9c932891 20
altb2 6:9ebeffe446e4 21 static BufferedSerial serial_port(USBTX, USBRX);
altb2 8:49ac75c42da0 22 DigitalIn big_button(PC_3); // User Button, short and long presses!
altb2 0:a56e9c932891 23 bool key_was_pressed = false;
altb2 8:49ac75c42da0 24 float Ts=.0002f; // sampling time
altb2 5:768e10f6d372 25 void pressed(void);
altb2 5:768e10f6d372 26 void released(void);
altb2 7:942fd77d5e19 27 //DigitalOut led1(LED1);
altb2 7:942fd77d5e19 28 //DigitalOut led2(LED2);
altb2 5:768e10f6d372 29 //------------- DEFINE FILTERS ----------------
altb2 5:768e10f6d372 30 // missing
altb2 6:9ebeffe446e4 31 //------------- Define In/Out -----------------
altb2 7:942fd77d5e19 32 AnalogOut i_des1(PA_5);
altb2 6:9ebeffe446e4 33 AnalogOut i_des2(PA_4);
altb2 8:49ac75c42da0 34 //AnalogIn i_act1(PA_3);
altb2 8:49ac75c42da0 35 //AnalogIn i_act2(PC_0);
altb2 8:49ac75c42da0 36 //FastPWM i_des1(PB_10);
altb2 8:49ac75c42da0 37 //FastPWM i_des2(PA_15);
altb2 8:49ac75c42da0 38
altb2 6:9ebeffe446e4 39 DigitalOut i_enable(PC_4);
altb2 6:9ebeffe446e4 40 DigitalOut laser_on(PB_0);
altb2 6:9ebeffe446e4 41 ///------------- Encoder -----------------------
altb2 5:768e10f6d372 42 EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7
altb2 5:768e10f6d372 43 InterruptIn indexpulse1(PA_8);
altb2 5:768e10f6d372 44 EncoderCounterIndex index1(counter1,indexpulse1);
altb2 6:9ebeffe446e4 45 // ------------------------------------
altb2 5:768e10f6d372 46 EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 5:768e10f6d372 47 InterruptIn indexpulse2(PB_4);
altb2 5:768e10f6d372 48 EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7
altb2 6:9ebeffe446e4 49 // ------------------------------------
altb2 6:9ebeffe446e4 50 DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data
altb2 6:9ebeffe446e4 51 DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data
altb2 6:9ebeffe446e4 52 //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98);
altb2 8:49ac75c42da0 53 LinearCharacteristics i2u(-.80,.80,0.02,0.98,0.02,.98);
altb2 8:49ac75c42da0 54 LinearCharacteristics u2i(0.0,1.0,-1.0,1.0);
altb2 6:9ebeffe446e4 55
altb2 6:9ebeffe446e4 56 Unwrapper_2pi uw2pi1;
altb2 6:9ebeffe446e4 57 Unwrapper_2pi uw2pi2;
altb2 0:a56e9c932891 58 //------------------------------------------
altb2 0:a56e9c932891 59 // ----- User defined functions -----------
altb2 5:768e10f6d372 60 ControllerLoop loop(Ts);
altb2 8:49ac75c42da0 61 uart_comm_thread uart_com(&serial_port,.05f);
altb2 6:9ebeffe446e4 62 Ticker do_referencing;
altb2 6:9ebeffe446e4 63 Timer glob_ti;
altb2 5:768e10f6d372 64 path_1d p1;
altb2 5:768e10f6d372 65 path_1d p2;
altb2 5:768e10f6d372 66 path_1d *current_path;
altb2 5:768e10f6d372 67 float A = 2.7;
altb2 5:768e10f6d372 68 float dc=0.0;
altb2 8:49ac75c42da0 69 GPA myGPA(5, 2400, 40, 25.0, 25.0, Ts);
altb2 7:942fd77d5e19 70 Mirror_Kinematic mk;
altb2 6:9ebeffe446e4 71 DATA_Xchange data;
altb2 6:9ebeffe446e4 72
altb2 5:768e10f6d372 73 //GPA myGPA(1, 2500, 100, 30, 20, Ts);
altb2 5:768e10f6d372 74 //GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts);
altb2 8:49ac75c42da0 75
altb2 8:49ac75c42da0 76
altb2 5:768e10f6d372 77 float exc=0.0; // excitation GPA
altb2 5:768e10f6d372 78 // f1 f2 N A1 A2 Ts
altb2 5:768e10f6d372 79
altb2 5:768e10f6d372 80 // *****************************************************************************
altb2 6:9ebeffe446e4 81
altb2 5:768e10f6d372 82
altb2 5:768e10f6d372 83
altb2 0:a56e9c932891 84 //******************************************************************************
altb2 0:a56e9c932891 85 //---------- main loop -------------
altb2 0:a56e9c932891 86 //******************************************************************************
altb2 0:a56e9c932891 87 int main()
altb2 0:a56e9c932891 88 {
altb2 6:9ebeffe446e4 89 serial_port.set_baud(115200);
altb2 6:9ebeffe446e4 90 serial_port.set_format(
altb2 6:9ebeffe446e4 91 /* bits */ 8,
altb2 6:9ebeffe446e4 92 /* parity */ BufferedSerial::None,
altb2 6:9ebeffe446e4 93 /* stop bit */ 1
altb2 6:9ebeffe446e4 94 );
altb2 6:9ebeffe446e4 95 serial_port.set_blocking(false); // force to send whenever possible and data is there
altb2 5:768e10f6d372 96 i_enable = 0; // disable current first
altb2 0:a56e9c932891 97 counter1.reset(); // encoder reset
altb2 5:768e10f6d372 98 counter2.reset(); // encoder reset
altb2 8:49ac75c42da0 99 //i_des1.period_ticks(1280);
altb2 8:49ac75c42da0 100 //i_des2.period_ticks(1280);
altb2 6:9ebeffe446e4 101 i_enable = 0;
altb2 8:49ac75c42da0 102 mk.set_offsets(1401,1510);
altb2 5:768e10f6d372 103 glob_ti.start();
altb2 5:768e10f6d372 104 glob_ti.reset();
altb2 6:9ebeffe446e4 105 printf("Start Mirroractuator 1.0\r\n");
altb2 6:9ebeffe446e4 106 loop.init_controllers();
altb2 8:49ac75c42da0 107 // uart_com.start_uart();
altb2 8:49ac75c42da0 108 // loop.start_loop();
altb2 7:942fd77d5e19 109 i_des1.write(i2u(0));
altb2 6:9ebeffe446e4 110 i_des2.write(i2u(0));
altb2 8:49ac75c42da0 111 ThisThread::sleep_for(200);
altb2 8:49ac75c42da0 112 uart_com.send_text((char *)"Start Mirroractuator 1.0");
altb2 6:9ebeffe446e4 113 /* p1.initialize(300,10,A,0,0,0);
altb2 5:768e10f6d372 114 p2.initialize(300,10,-A,0,0,A);
altb2 6:9ebeffe446e4 115 laser_on = 0;
altb2 6:9ebeffe446e4 116
altb2 6:9ebeffe446e4 117
altb2 6:9ebeffe446e4 118 //for(int wk =0;wk<5;wk++)
altb2 6:9ebeffe446e4 119 while(0)
altb2 5:768e10f6d372 120 {
altb2 6:9ebeffe446e4 121 short c1 = counter1; // get counts from Encoder
altb2 6:9ebeffe446e4 122 short c2 = counter2; // get counts from Encoder
altb2 6:9ebeffe446e4 123 /*current_path = &p1;
altb2 5:768e10f6d372 124 current_path->start(glob_ti.read());
altb2 5:768e10f6d372 125 while(!current_path->finished)
altb2 5:768e10f6d372 126 wait(.1);
altb2 5:768e10f6d372 127 current_path = &p2;
altb2 5:768e10f6d372 128 current_path->start(glob_ti.read());
altb2 5:768e10f6d372 129 while(!current_path->finished)
altb2 5:768e10f6d372 130 wait(.1);
altb2 5:768e10f6d372 131 ThisThread::sleep_for(100);
altb2 6:9ebeffe446e4 132
altb2 6:9ebeffe446e4 133 // printf("c1,c2: %d %d \r\n",c1,c2);
altb2 6:9ebeffe446e4 134
altb2 6:9ebeffe446e4 135 wait(.25);
altb2 6:9ebeffe446e4 136 laser_on = !laser_on;
altb2 5:768e10f6d372 137 }
altb2 6:9ebeffe446e4 138 i_enable = 0;*/
altb2 8:49ac75c42da0 139 while(1){
altb2 8:49ac75c42da0 140 ThisThread::sleep_for(200);
altb2 8:49ac75c42da0 141 printf("hello\r\n");
altb2 8:49ac75c42da0 142 }
altb2 0:a56e9c932891 143
altb2 0:a56e9c932891 144 } // END OF main