Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

main.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
1:25a2b47ca291 
Publish Commit 24/8/21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 #include "mbed.h"
altb2 0:d2e117716219 2 #include "math.h"
altb2 0:d2e117716219 3 //------------------------------------------
altb2 0:d2e117716219 4 #define PI 3.1415927f
altb2 0:d2e117716219 5 //------------------------------------------
altb2 0:d2e117716219 6 #include "EncoderCounter.h"
altb2 0:d2e117716219 7 #include "EncoderCounterIndex.h"
altb2 0:d2e117716219 8 #include "DiffCounter.h"
altb2 0:d2e117716219 9 #include "IIR_filter.h"
altb2 0:d2e117716219 10 #include "LinearCharacteristics.h"
altb2 0:d2e117716219 11 #include "PID_Cntrl.h"
altb2 0:d2e117716219 12 #include "Unwrapper_2pi.h"
altb2 0:d2e117716219 13 #include "path_1d.h"
altb2 0:d2e117716219 14 #include "GPA.h"
altb2 0:d2e117716219 15 #include "ControllerLoop.h"
altb2 0:d2e117716219 16 #include "Mirror_Kinematic.h"
altb2 0:d2e117716219 17 #include "data_structs.h"
altb2 0:d2e117716219 18 #include "uart_comm_thread.h"
altb2 0:d2e117716219 19 #include "FastPWM.h"
altb2 0:d2e117716219 20
altb2 0:d2e117716219 21 static BufferedSerial serial_port(USBTX, USBRX);
altb2 0:d2e117716219 22 DigitalIn big_button(PC_3); // Enable button an backside
altb2 0:d2e117716219 23 bool key_was_pressed = false; //
altb2 0:d2e117716219 24 float Ts=.0002f; // sampling time
altb2 0:d2e117716219 25 void pressed(void);
altb2 0:d2e117716219 26 void released(void);
ernstpre 2:92c25cb669f4 27
ernstpre 2:92c25cb669f4 28
altb2 0:d2e117716219 29 //------------- DEFINE FILTERS ----------------
altb2 0:d2e117716219 30 // missing
altb2 0:d2e117716219 31 //------------- Define In/Out -----------------
altb2 0:d2e117716219 32 AnalogOut i_des1(PA_5);
altb2 0:d2e117716219 33 AnalogOut i_des2(PA_4);
altb2 0:d2e117716219 34 //AnalogIn i_act1(PA_3);
altb2 0:d2e117716219 35 //AnalogIn i_act2(PC_0);
altb2 0:d2e117716219 36 //FastPWM i_des1(PB_10);
altb2 0:d2e117716219 37 //FastPWM i_des2(PA_15);
altb2 0:d2e117716219 38 DigitalOut i_enable(PC_4);
altb2 0:d2e117716219 39 DigitalOut laser_on(PB_0);
altb2 0:d2e117716219 40 ///------------- Encoder -----------------------
altb2 0:d2e117716219 41 EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7
altb2 0:d2e117716219 42 InterruptIn indexpulse1(PA_8);
altb2 0:d2e117716219 43 EncoderCounterIndex index1(counter1,indexpulse1);
altb2 0:d2e117716219 44 // ------------------------------------
altb2 0:d2e117716219 45 EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7
altb2 0:d2e117716219 46 InterruptIn indexpulse2(PB_4);
altb2 0:d2e117716219 47 EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7
altb2 0:d2e117716219 48 // ------------------------------------
altb2 0:d2e117716219 49 DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data
altb2 0:d2e117716219 50 DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data
altb2 0:d2e117716219 51 //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98);
altb2 0:d2e117716219 52 LinearCharacteristics i2u(-.80,.80,0.0f,1.0f);
altb2 0:d2e117716219 53 LinearCharacteristics u2i(0.0,1.0,-1.0,1.0);
altb2 0:d2e117716219 54
altb2 0:d2e117716219 55 Unwrapper_2pi uw2pi1;
altb2 0:d2e117716219 56 Unwrapper_2pi uw2pi2;
altb2 0:d2e117716219 57 //------------------------------------------
altb2 0:d2e117716219 58 // ----- User defined functions -----------
altb2 0:d2e117716219 59 ControllerLoop loop(Ts); // this is forthe main controller loop
altb2 0:d2e117716219 60 uart_comm_thread uart_com(&serial_port,.05f); // this is the communication thread
altb2 0:d2e117716219 61 Timer glob_ti; // the global timer
altb2 0:d2e117716219 62 path_1d p1; // pathplanner (under constr.)
altb2 0:d2e117716219 63 path_1d p2; // pathplanner (under constr.)
altb2 0:d2e117716219 64 path_1d *current_path;
altb2 0:d2e117716219 65 // --------- GPA -----------------------------
altb2 0:d2e117716219 66 //init values: f0, f1, nbPts, A0, A1, Ts
ernstpre 1:25a2b47ca291 67 GPA myGPA(5 , 2400, 40, 60, 50, Ts);
altb2 0:d2e117716219 68 float exc=0.0; // excitation GPA
altb2 0:d2e117716219 69 //------------------------------------------------------------------------------
altb2 0:d2e117716219 70 // --------- Mirror kinematik, define values, trafos etc there
altb2 0:d2e117716219 71 Mirror_Kinematic mk;
altb2 0:d2e117716219 72 //------------------------------------------------------------------------------
altb2 0:d2e117716219 73 // --------- data: overall data structure for x-change
altb2 0:d2e117716219 74 DATA_Xchange data;
altb2 0:d2e117716219 75
altb2 0:d2e117716219 76 //******************************************************************************
altb2 0:d2e117716219 77 //---------- main loop -------------
altb2 0:d2e117716219 78 //******************************************************************************
altb2 0:d2e117716219 79 int main()
altb2 0:d2e117716219 80 {
altb2 0:d2e117716219 81 serial_port.set_baud(115200);
altb2 0:d2e117716219 82 serial_port.set_format(
altb2 0:d2e117716219 83 /* bits */ 8,
altb2 0:d2e117716219 84 /* parity */ BufferedSerial::None,
altb2 0:d2e117716219 85 /* stop bit */ 1
altb2 0:d2e117716219 86 );
altb2 0:d2e117716219 87 serial_port.set_blocking(false); // force to send whenever possible and data is there
altb2 0:d2e117716219 88 i_enable = 0; // disable current first
altb2 0:d2e117716219 89 counter1.reset(); // encoder reset
altb2 0:d2e117716219 90 counter2.reset(); // encoder reset
ernstpre 1:25a2b47ca291 91 mk.set_offsets(1473,3257); // individal set values for global position
altb2 0:d2e117716219 92 glob_ti.start();
altb2 0:d2e117716219 93 glob_ti.reset();
altb2 0:d2e117716219 94 loop.init_controllers();
ernstpre 2:92c25cb669f4 95 //uart_com.start_uart(); // communication with MATLAB
ernstpre 2:92c25cb669f4 96 printf("hello world \n");
altb2 0:d2e117716219 97 loop.start_loop();
altb2 0:d2e117716219 98 i_des1.write(i2u(0));
altb2 0:d2e117716219 99 i_des2.write(i2u(0));
altb2 0:d2e117716219 100 ThisThread::sleep_for(200);
ernstpre 2:92c25cb669f4 101 //uart_com.send_text((char *)"Start Mirroractuator 1.1"); // communciation with MATLAB
altb2 0:d2e117716219 102 /* p1.initialize(300,10,A,0,0,0);
altb2 0:d2e117716219 103 p2.initialize(300,10,-A,0,0,A);*/
altb2 0:d2e117716219 104 laser_on = 0;
altb2 0:d2e117716219 105 //for(int wk =0;wk<5;wk++)
altb2 0:d2e117716219 106 while(0)
altb2 0:d2e117716219 107 {
altb2 0:d2e117716219 108 short c1 = counter1; // get counts from Encoder
altb2 0:d2e117716219 109 short c2 = counter2; // get counts from Encoder
altb2 0:d2e117716219 110 current_path = &p1;
altb2 0:d2e117716219 111 current_path->start(glob_ti.read());
altb2 0:d2e117716219 112 while(!current_path->finished)
altb2 0:d2e117716219 113 ThisThread::sleep_for(100);
altb2 0:d2e117716219 114 current_path = &p2;
altb2 0:d2e117716219 115 current_path->start(glob_ti.read());
altb2 0:d2e117716219 116 while(!current_path->finished)
altb2 0:d2e117716219 117 ThisThread::sleep_for(100);
altb2 0:d2e117716219 118 ThisThread::sleep_for(100);
altb2 0:d2e117716219 119 laser_on = !laser_on;
altb2 0:d2e117716219 120 } // end of while(..)
altb2 0:d2e117716219 121 i_enable = 0;
altb2 0:d2e117716219 122 while(1)
altb2 0:d2e117716219 123 {
altb2 0:d2e117716219 124 ThisThread::sleep_for(200);
altb2 0:d2e117716219 125 }
ernstpre 2:92c25cb669f4 126 // *** create CSV file of data
ernstpre 2:92c25cb669f4 127
ernstpre 2:92c25cb669f4 128 /*FILE *f = fopen ("test.csv", "a");
ernstpre 2:92c25cb669f4 129
ernstpre 2:92c25cb669f4 130 // inform user
ernstpre 2:92c25cb669f4 131 if (!f) {
ernstpre 2:92c25cb669f4 132 printf ("failed\n");
ernstpre 2:92c25cb669f4 133 } else {
ernstpre 2:92c25cb669f4 134 printf ("success\n");
ernstpre 2:92c25cb669f4 135 }
ernstpre 2:92c25cb669f4 136
ernstpre 2:92c25cb669f4 137 fprintf (f, " test\n");
ernstpre 2:92c25cb669f4 138 fclose (f);
ernstpre 2:92c25cb669f4 139 return 0;*/
ernstpre 2:92c25cb669f4 140
altb2 0:d2e117716219 141 } // END OF main