Revision 7:942fd77d5e19, committed 2021-04-15

Comitter:

altb2

Date:

Thu Apr 15 05:36:55 2021 +0000

Parent:

6:9ebeffe446e4

Commit message:

intermediate

Changed in this revision

diff -r 9ebeffe446e4 -r 942fd77d5e19 ControllerLoop.cpp
--- a/ControllerLoop.cpp	Thu Apr 01 13:38:54 2021 +0000
+++ b/ControllerLoop.cpp	Thu Apr 15 05:36:55 2021 +0000
@@ -3,14 +3,16 @@
 
 
 
-ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
+ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096), dout1(PB_9)
 {
     this->Ts = Ts;
     diff1.reset(0.0f,0);
     diff2.reset(0.0f,0);
+    Kv = 150;
     is_initialized = false;
     ti.reset();
     ti.start();
+    data.laser_on = false;
     }
 
 
@@ -20,7 +22,6 @@
 void ControllerLoop::loop(void){
     float A=.6;
     uint32_t k=0;
-    float Kv = 150;
     while(1)
         {
         ThisThread::flags_wait_any(threadFlag);
@@ -33,7 +34,7 @@
   //          printf("1: %d %d, 2: %d %d\r\n",index1.positionAtIndexPulse,c1-index1.positionAtIndexPulse-mc.inc_offset[0],index2.positionAtIndexPulse,c2-index2.positionAtIndexPulse-mc.inc_offset[1]);
             //led1=!led1;
             }
-        if(0)//!is_initialized)
+        if(!is_initialized)
             {
             find_index();
             if(index1.positionAtIndexPulse != 0 && index2.positionAtIndexPulse != 0) 
@@ -41,31 +42,56 @@
             }
         else
             {
-            short c1 = counter1 - index1.positionAtIndexPulse - mc.inc_offset[0]- mc.inc_additional_offset[0];            // get counts from Encoder
-            short c2 = counter2 - index2.positionAtIndexPulse - mc.inc_offset[1]- mc.inc_additional_offset[1];            // get counts from Encoder
+            short c1 = counter1 - index1.positionAtIndexPulse - mk.inc_offset[0]- mk.inc_additional_offset[0];            // get counts from Encoder
+            short c2 = counter2 - index2.positionAtIndexPulse - mk.inc_offset[1]- mk.inc_additional_offset[1];            // get counts from Encoder
             data.sens_phi[0] = uw2pi1(2*3.1415927/4000.0*(float)c1);
             data.sens_Vphi[0] = diff1(c1);                 // motor velocity 
             data.sens_phi[1] = uw2pi2(2*3.1415927/4000.0*(float)c2);
-            data.sens_Vphi[1] = diff2(c2);                 // motor velocity 
-            float w01=2*3.1415927 * 3;
+            data.sens_Vphi[1] = diff2(c2);                 // motor velocity
+            // ------------------------ do the control first
+            float v_des1 = pos_cntrl(data.cntrl_phi_des[0]-data.sens_phi[0]);
+            float v_des2 = pos_cntrl(data.cntrl_phi_des[1]-data.sens_phi[1]);
+            data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ;
+            data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ;
+            // ------------------------ write outputs
+            i_des1.write(i2u(data.i_des[0]));
+            i_des2.write(i2u(data.i_des[1]));
+            // now do trafos etc
+
+            float w01=2*3.1415927 * .5;
             float w02=2*3.1415927 * 1.5;
-            
+            float xy[2];
+            if(mk.external_control) // get desired values from external source (GUI)
+                {
+                if(mk.trafo_is_on)  // use desired xy values from xternal source and transform
+                                    // otherwise external source delivers phi1, phi2 values directly
+                    {
+                    bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
+                    }
+                }
+            else
+                {
+                if(mk.trafo_is_on)
+                    {
+                    data.cntrl_xy_des[0] = 50.0f*cosf(w01*glob_ti.read());
+                    data.cntrl_xy_des[1] = 50.0f*sinf(w01*glob_ti.read());
+                    bool dum = mk.X2P(data.cntrl_xy_des,data.cntrl_phi_des);
+                    }
+                else
+                    {
+                    data.cntrl_phi_des[0] = .50f*cosf(w01*glob_ti.read());
+                    data.cntrl_phi_des[1] = .50f*sinf(w01*glob_ti.read());
+                    }
+                }
+            //bool dum = mk.P2X(data.cntrl_phi_des,data.cntrl_xy_des)
             //current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
             
-            data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read());
-            data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read());
-            data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
-            data.est_xy[1]=data.cntrl_phi_des[1];
+            //data.cntrl_phi_des[0] = 0.0f*A*sinf(w01*ti.read());
+            //data.cntrl_phi_des[1] = 0.0f*A*sinf(w02*ti.read());
+            //data.est_xy[0]=data.cntrl_phi_des[0]; // temporary
+            //data.est_xy[1]=data.cntrl_phi_des[1];
             //laser_on = 1;
-    
-            float v_des1 = Kv*(data.cntrl_phi_des[0]-data.sens_phi[0]);
-            float v_des2 = Kv*(data.cntrl_phi_des[1]-data.sens_phi[1]);
-            data.i_des[0] = v_cntrl[0](v_des1 - data.sens_Vphi[0] ) ;
-            data.i_des[1] = v_cntrl[1](v_des2 - data.sens_Vphi[1] ) ;
-    //float dum = (float)(++u_test%16)/16.0f;
-    //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
-    //i_des1.write(i2u(data.i_des[0]));
-            i_des2.write(i2u(data.i_des[1]));
+            laser_on = data.laser_on;
             }
         }
 }
@@ -78,6 +104,12 @@
     thread.start(callback(this, &ControllerLoop::loop));
     ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
 }
+
+float ControllerLoop::pos_cntrl(float d_phi)
+{
+    return Kv*mk.mot_inc_to_rad*round(mk.mot_rad_to_inc*(d_phi));
+    }
+
 void ControllerLoop::init_controllers(void)
 {
     float Kp = 2000 * 4.89e-7/0.094f;   // XX * J/km
@@ -97,6 +129,6 @@
     float i2 = Kp*(25.0f - vel2 ) ;
     //float dum = (float)(++u_test%16)/16.0f;
     //i_des1.write(i2u(.25f*sin(2*3.14f*1.0f)));
-    //i_des1.write(i2u(i1));
+    i_des1.write(i2u(i1));
     i_des2.write(i2u(i2));
     }
\ No newline at end of file

diff -r 9ebeffe446e4 -r 942fd77d5e19 ControllerLoop.h
--- a/ControllerLoop.h	Thu Apr 01 13:38:54 2021 +0000
+++ b/ControllerLoop.h	Thu Apr 15 05:36:55 2021 +0000
@@ -15,14 +15,12 @@
 extern DiffCounter diff1,diff2;
 extern path_1d *current_path;
 extern LinearCharacteristics i2u;
-//extern AnalogOut i_des1, i_des2;
-extern AnalogOut i_des2;
+extern AnalogOut i_des1, i_des2;
 extern DigitalOut i_enable;
 extern Timer glob_ti;
-extern Mirror_Kinematic mc;
+extern Mirror_Kinematic mk;
 extern DigitalOut laser_on;
 extern DATA_Xchange data;
-extern DigitalOut led1;
 
 // This is the loop class, it is not a controller at first hand, it guarantees a cyclic call
 class ControllerLoop
@@ -48,4 +46,7 @@
     PIDT2_Cntrl v_cntrl[2];
     Unwrapper_2pi uw2pi1;
     Unwrapper_2pi uw2pi2;
+    float pos_cntrl(float);
+    float Kv;
+    DigitalOut dout1;
 };

diff -r 9ebeffe446e4 -r 942fd77d5e19 Library_Misc.lib
--- a/Library_Misc.lib	Thu Apr 01 13:38:54 2021 +0000
+++ b/Library_Misc.lib	Thu Apr 15 05:36:55 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/altb2/code/Library_Misc/#9e3b67631e9f
+https://os.mbed.com/teams/RT2_P3_students/code/Library_Misc/#9c87abf3235a

diff -r 9ebeffe446e4 -r 942fd77d5e19 Mirror_Kinematic.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mirror_Kinematic.cpp	Thu Apr 15 05:36:55 2021 +0000
@@ -0,0 +1,101 @@
+#include "Mirror_Kinematic.h"
+
+Mirror_Kinematic::Mirror_Kinematic(void) 
+{
+    screen_h = 97;
+    screen_d = 4;
+    dist_L = 16;
+    inc_offset[0] = inc_offset[1] = 0;
+    inc_additional_offset[0] = inc_additional_offset[1] = 0;
+    mot_inc_to_rad = (3.141592653589793 * 2.0) / 4000.0;
+    mot_rad_to_inc = 4000.0/(3.141592653589793 * 2.0);
+    n = 1.585;  // Brechungsindex
+    old_phi[0]=0.0;
+    old_phi[1]=0.0;
+    trafo_is_on = false;
+    external_control = false;
+    controller_is_on = true;
+    }
+    
+void Mirror_Kinematic::set_offsets(int16_t o1,int16_t o2)
+{
+    inc_offset[0] = o1;
+    inc_offset[1] = o2;
+    }
+void Mirror_Kinematic::set_additional_offsets(int16_t o1,int16_t o2)
+{
+    inc_additional_offset[0] = o1;
+    inc_additional_offset[1] = o2;
+    }
+void Mirror_Kinematic::add_additional_offsets(int16_t o1,int16_t o2)
+{
+    inc_additional_offset[0] += o1;
+    inc_additional_offset[1] += o2;
+    }
+int16_t Mirror_Kinematic::get_additional_offsets(uint8_t axis)
+{
+    if(axis>1)
+        return 0;
+    else
+        return inc_additional_offset[axis];
+    }
+bool Mirror_Kinematic::P2X(float *P, float *X)
+{
+    // calculation time 5.7usec on F446RE   
+    float c1 = cosf(P[0]);
+    float c2 = cosf(P[1]);
+    float s1 = sinf(P[0]);
+    float s2 = sinf(2.0f*P[1]);
+    float n1x=c1;
+    float n1y=c1*s1;
+    float n1z=s1*s1;
+    float sq2=sqrt(.5);
+    // i.e.: cos(2*x) = 2*cos(x)^2-1
+    //float n2x = n1z*cosf(2.0f*P[1]) + n1x*sinf(2.0f*P[1]);
+    float n2x = n1z*(2.0f*c2*c2-1.0f) + n1x*s2;
+    float n2y = n1y;
+    float a1 = sq2*c2 - sq2*s1;
+    float a2 = sq2*c2 + sq2*s1;
+    float dum1 = n1x * a1 - n1z*a2;
+    float n2z = n1x*(2.0f*c2*c2-1.0f) - n1z*s2;
+    if(dum1*n2z == 0)
+        return false;
+    float dad = dist_L * a1/dum1;
+    float Q2x = dad * n1x - dist_L;
+    float Q2y = dad * n1y;
+    float Q2z = dad * n1z;
+    float x = atanf(n2x/n2z)/n;
+    float y = atanf(n2y/n2z)/n;
+    float dx = screen_d * x/sqrt(1-x*x);
+    float dy = screen_d * y/sqrt(1-y*y);
+    X[0] =  Q2x + (n2x*(screen_h - Q2z))/n2z - dx;
+    X[1] =  Q2y + (n2y*(screen_h - Q2z))/n2z - dy;
+    
+    return true;
+    }
+
+bool Mirror_Kinematic::X2P(float *X, float *P)
+{
+    float J12 = 0.0090517133f;
+    float J21 = 0.0052923231f;
+    float Xn[2];
+    float dx = 1e4;
+    float dy = 1e4;
+    P[0] = old_phi[0];
+    P[1] = old_phi[1];
+    uint8_t k = 0;
+    do
+        {
+            if( !P2X(P,Xn))
+                return false;
+            dx = Xn[0]-X[0];
+            dy = Xn[1]-X[1];
+            P[0] -= J12 * dy;
+            P[1] -= J21 * dx;
+        }
+        while((dx*dx+dy*dy) > 1e-3 && ++k<20);
+    data.num_it = k;
+    old_phi[0] = P[0];
+    old_phi[1] = P[1];
+    return true;
+}
\ No newline at end of file

diff -r 9ebeffe446e4 -r 942fd77d5e19 Mirror_Kinematic.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mirror_Kinematic.h	Thu Apr 15 05:36:55 2021 +0000
@@ -0,0 +1,39 @@
+#ifndef MIRROR_KINEMATIC_H_
+#define MIRROR_KINEMATIC_H_
+
+#include "mbed.h"
+#include "data_structs.h"
+
+extern DATA_Xchange data;
+
+
+class Mirror_Kinematic
+{
+public:
+    Mirror_Kinematic(void);
+    float screen_h;
+    float  n ;          //        Brechungsindex
+    float dist_L;       // distance laser0 to 2nd axis
+    float screen_d;     // thickness of screen / mmm
+    void set_offsets(int16_t,int16_t);
+    void set_additional_offsets(int16_t,int16_t);
+    void add_additional_offsets(int16_t,int16_t);
+    int16_t get_additional_offsets(uint8_t axis);
+    int16_t add_additional_offsets(uint8_t axis);
+    int16_t inc_offset[2];
+    int16_t inc_additional_offset[2];
+    float mot_inc_to_rad;
+    float mot_rad_to_inc;
+    bool P2X(float *,float *);
+    bool X2P(float *,float *);
+    bool trafo_is_on;
+    bool external_control;
+    bool controller_is_on;
+private:
+    float old_phi[2];
+    
+    };
+    
+#endif
+    
+

diff -r 9ebeffe446e4 -r 942fd77d5e19 main.cpp
--- a/main.cpp	Thu Apr 01 13:38:54 2021 +0000
+++ b/main.cpp	Thu Apr 15 05:36:55 2021 +0000
@@ -20,15 +20,15 @@
 static BufferedSerial serial_port(USBTX, USBRX);
 InterruptIn button(USER_BUTTON);        // User Button, short and long presses!
 bool key_was_pressed = false;
-float Ts=.0002f;                         // sampling time
+float Ts=.0005f;                         // sampling time
 void pressed(void);
 void released(void); 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
+//DigitalOut led1(LED1);
+//DigitalOut led2(LED2);
 //------------- DEFINE FILTERS ----------------
 // missing
 //------------- Define In/Out -----------------
-//AnalogOut i_des1(PA_5);
+AnalogOut i_des1(PA_5);
 AnalogOut i_des2(PA_4);
 DigitalOut i_enable(PC_4);
 DigitalOut laser_on(PB_0);
@@ -51,7 +51,7 @@
 //------------------------------------------
 // ----- User defined functions -----------
 ControllerLoop loop(Ts);
-uart_comm_thread uart_com(&serial_port,.5f);
+uart_comm_thread uart_com(&serial_port,.1f);
 Ticker do_referencing;
 Timer glob_ti;
 path_1d p1;
@@ -59,7 +59,7 @@
 path_1d *current_path;
 float A = 2.7;
 float dc=0.0;
-Mirror_Kinematic mc;
+Mirror_Kinematic mk;
 DATA_Xchange data;
 
 //GPA myGPA(1, 2500, 100, 30, 20, Ts);
@@ -87,17 +87,15 @@
     counter1.reset();   // encoder reset
     counter2.reset();   // encoder reset
     i_enable = 0;
-    mc.set_offsets(2956,2343);
+    mk.set_offsets(0,0);
     glob_ti.start();
     glob_ti.reset();
     printf("Start Mirroractuator 1.0\r\n");
     loop.init_controllers();
     uart_com.start_uart();
     loop.start_loop();
-    //i_des1.write(i2u(0));
+    i_des1.write(i2u(0));
     i_des2.write(i2u(0));
-    led1 = 1;
-    led2 = 1;
    /* p1.initialize(300,10,A,0,0,0);
     p2.initialize(300,10,-A,0,0,A);
     laser_on = 0;

diff -r 9ebeffe446e4 -r 942fd77d5e19 uart_comm_thread.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart_comm_thread.cpp	Thu Apr 15 05:36:55 2021 +0000
@@ -0,0 +1,320 @@
+// includes
+#include "uart_comm_thread.h"
+/*
+-------- DATA PROTOCOL----------------------------
+   254	 1	255	201	1	  4	     0		...
+    n1	n2	rec id1	id2	#Byte1 #Byte2 thedata
+--------------------------------------------------
+1-20 sensor values,
+		id1		id2	
+		10			 	Counter values
+				1		c1
+				2		c2	(increments)
+		11		actual current
+				1		i1 / A
+				2		i2 / A
+--------------------------------------------------
+// NOT USED: 21-40 cntrl values,		21		desired values			1 		Phi1 / rad				2 		Phi2 / rad				3 		x / mm				4 		y / mm
+--------------------------------------------------
+101-120 estimates/actual values,
+		101		angles and calculated x,y
+				1 		Phi1 / rad
+				2 		Phi2 / rad
+				3		x / mm
+				4		y / mm
+
+--------------------------------------------------
+121-140 send techn. values, like offsets
+		id1		id2	
+		121
+				1 		inc_offset phi1 / increments	int16_t
+				2		inc_offset phi2	/ 		"			"
+				3 		inc_additional_offset phi1 / increments	int16_t
+				4		inc_additional_offset phi2	/ 		"			"
+		125
+				1		num_it of X2P trafo
+--------------------------------------------------
+
+2xx: set-value and commands (like disable controller...)
+		id1		id2	
+	 	201: 			Set values/parameters
+				1 		inc_additional_offset phi1 / increments	int16_t
+				2		inc_additional_offset phi2	/ 		"			"
+		
+		202:			set desired absolute values	
+				1		phi1		rad		float
+				2		phi2		rad		float
+				3		x			mm		float
+				4		y			mm		float
+		203				Increment values
+				1		dphi1		rad		float
+				2		dphi2		rad		float
+				3		dx			mm		float
+				4		dy			mm		float
+		220			Laser on/off
+				1	0 = off, 1 = on
+		221			Trafo on/off
+				1	0 = off, 1 = on
+		230			external control on/off
+				1	0 = off, 1 = on
+
+*/
+
+// #### constructor
+uart_comm_thread::uart_comm_thread(BufferedSerial *com, float Ts): thread(osPriorityBelowNormal, 4096)
+ {  
+    // init serial
+    uart = com;
+    //uart->attach(callback(this, &uart_comm_thread::callBack), RawSerial::RxIrq);
+    this->Ts = Ts;
+    init();
+}
+
+// #### destructor
+uart_comm_thread::~uart_comm_thread() {
+    
+    
+}
+
+// #### request data from device
+void uart_comm_thread::init(){
+       // init statemachine
+}
+
+
+// #### run the statemachine
+void uart_comm_thread::run(void)
+{
+    // returnvalue
+    bool retVal = false;
+	uint8_t checksum,k;
+	uint8_t send_state =101;
+	while(true)
+    	{
+        ThisThread::flags_wait_any(threadFlag);
+        //---  The LOOP --------------------------------------------------------
+        uint32_t num = uart->read(buffer, sizeof(buffer));
+        if (num >0) 
+    		{
+        	if(buffer[0] == 254 && buffer[1] == 1)
+            	{
+            	if(analyse_received_data())
+            			;//		led1 = !led1;
+            	}
+            }
+		switch(send_state)
+			{
+			case 101:
+				send_data(101,12,2*4,data.sens_phi);		// done in every 2nd cycle!
+				send_data(101,34,2*4,data.est_xy);		// done in every 2nd cycle!
+				send_state = 121;
+				break;	
+			case 121:
+				send_data(121,34,2*2,mk.inc_additional_offset);		// done in every 2nd cycle!
+				send_state = 202;
+				break;
+			case 202:
+				send_data(202,12,2*4,data.cntrl_phi_des);		// done in every 2nd cycle!
+				send_state = 125;
+				break;
+			case 125:
+				send_data(125,1,data.num_it);		// done in every 2nd cycle!
+				send_state = 101;
+				break;
+			default:
+				break;
+			}
+        }// loop
+}
+
+
+
+// #### receive data from hardware buffer, ensuring no dropped bytes
+
+// ------------------- start uart ----------------
+void uart_comm_thread::start_uart(void){
+		
+		thread.start(callback(this, &uart_comm_thread::run));
+		ticker.attach(callback(this, &uart_comm_thread::sendThreadFlag), Ts);
+	
+    //    thread.start(callback(this, &uart_comm_thread::run));
+    //    ticker.attach(callback(this, &uart_comm_thread::sendSignal), Ts);
+        printf("UART Thread started\r\n");
+
+}
+// this is for realtime OS
+void uart_comm_thread::sendThreadFlag() {
+    
+    thread.flags_set(threadFlag);
+}
+
+void uart_comm_thread::send_data(char id1,char id2,uint16_t N,float *dat)
+{	
+	buffer[0]=254;buffer[1]=1;buffer[2]=255;	// standard pattern
+	buffer[3] = id1;
+	buffer[4] = id2;
+	buffer[5] = *(char *)N;
+	uart->write(buffer, 7);
+	char *float_data = (char *)dat;
+	uart->write(float_data,N);
+	uart->write("\r",2);		// line end
+	//mutex.unlock();
+}
+void uart_comm_thread::send_data(char id1,char id2,uint16_t N,int16_t *dat)
+{	
+	buffer[0]=254;buffer[1]=1;buffer[2]=255;	// standard pattern
+	buffer[3] = id1;
+	buffer[4] = id2;
+	buffer[5] = *(char *)N;
+	uart->write(buffer, 7);
+	char *int_data = (char *)dat;
+	uart->write(int_data,N);
+	uart->write("\r",2);		// line end
+	//mutex.unlock();
+}
+void uart_comm_thread::send_data(char id1,char id2,int16_t dat)
+{	
+	buffer[0]=254;buffer[1]=1;buffer[2]=255;	// standard pattern
+	buffer[3] = id1;
+	buffer[4] = id2;
+	buffer[5] = 2;
+	buffer[6] = 0;
+	uart->write(buffer, 7);
+	char *int_data = (char *)dat;
+	uart->write(int_data,2);
+	uart->write("\r",2);		// line end
+	//mutex.unlock();
+}
+void uart_comm_thread::send_data(char id1,char id2,uint8_t dat)
+{	
+	buffer[0]=254;buffer[1]=1;buffer[2]=255;	// standard pattern
+	buffer[3] = id1;
+	buffer[4] = id2;
+	buffer[5] = 1;
+	buffer[6] = 0;
+	uart->write(buffer, 7);
+	char int8_data = (char)dat;
+	uart->write(&int8_data,1);
+	uart->write("\r",2);		// line end
+	//mutex.unlock();
+}
+
+bool uart_comm_thread::analyse_received_data(void){
+	char msg_id1 = buffer[3];
+	char msg_id2 = buffer[4];
+	uint16_t N = 256 * buffer[6] + buffer[5];
+	switch(msg_id1)
+		{
+		case 201:
+			if(N != 2)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					mk.add_additional_offsets(256 * buffer[8] + buffer[7],0);
+					return true;
+					break;
+				case 2:
+					mk.add_additional_offsets(0,256 * buffer[8] + buffer[7]);
+					return true;
+					break;
+				default:
+					break;
+				}
+			break;		// case 201
+		case 202:
+			if(N != 4)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					data.cntrl_phi_des[0] = *(float *)&buffer[7];
+					return true;
+					break;
+				case 2:
+					data.cntrl_phi_des[1] = *(float *)&buffer[7];
+					return true;
+					break;
+				case 3:
+					data.cntrl_xy_des[1] = 	*(float *)&buffer[7];
+					return true;
+					break;
+				case 4:
+					data.cntrl_xy_des[1] = *(float *)&buffer[7];
+					return true;
+					break;
+				default:
+					break;
+				}
+			break;		// case 202
+		case 203:
+			if(N != 4)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					data.cntrl_phi_des[0] += *(float *)&buffer[7];
+					return true;
+					break;
+				case 2:
+					data.cntrl_phi_des[1] += *(float *)&buffer[7];
+					return true;
+					break;
+				case 3:
+					data.cntrl_xy_des[1] += 	*(float *)&buffer[7];
+					return true;
+					break;
+				case 4:
+					data.cntrl_xy_des[1] += *(float *)&buffer[7];
+					return true;
+					break;
+				default:
+					break;
+				}
+			break;		// case 203
+		case 220:
+			if(N != 1)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					if(buffer[7] == 1)
+						data.laser_on = true;
+					else 
+						data.laser_on = false;
+					return true;
+					break;
+				}
+			break;
+		case 221:
+			if(N != 1)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					if(buffer[7] == 1)
+						mk.trafo_is_on = true;
+					else 
+						mk.trafo_is_on = false;
+					return true;
+					break;
+				}
+			break;
+		case 230:
+			if(N != 1)
+				return false;
+			switch(msg_id2)
+				{
+				case 1:
+					if(buffer[7] == 1)
+						mk.external_control = true;
+					else 
+						mk.external_control = false;
+					return true;
+					break;
+				}
+			break;
+		}
+	return false;	
+}
+

diff -r 9ebeffe446e4 -r 942fd77d5e19 uart_comm_thread.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uart_comm_thread.h	Thu Apr 15 05:36:55 2021 +0000
@@ -0,0 +1,85 @@
+#ifndef UART_COMM_THREAD_H_
+#define UART_COMM_THREAD_H_
+
+#include "mbed.h"
+#include "ThreadFlag.h"
+#include "data_structs.h"
+#include "data_structs.h"
+#include "Mirror_Kinematic.h"
+
+using namespace std;
+
+extern DATA_Xchange data;
+extern Mirror_Kinematic mk;
+
+// "protocol" specifics
+
+#define BUF_LEN      	20  // max 256
+#define DATA_LEN      	20  // max 256
+
+// states
+#define IDLE    0
+#define WAIT    1
+#define RECEIVE 2
+#define DONE    3
+
+#define LEN_OF_EXP_TYPE2 1   // length in bytes of expected Type
+#define NUM_OF_VALUE    7   // number of expected values
+#define EXPECTED2        LEN_OF_EXP_TYPE2 * NUM_OF_VALUE  // byte per Value * expected values = total expected bytes
+
+extern Mirror_Kinematic mc;
+
+// predefiniton for callback (couldn't implement as member)
+
+class uart_comm_thread{
+public:
+// public members
+    uart_comm_thread(BufferedSerial*,float);
+    virtual ~uart_comm_thread();
+    void run(void);             // runs the statemachine, call this function periodicly, returns true when new data ready (only true for 1 cycle)
+   // void request();         // request new set of data
+    void start_uart(void);
+
+    // public vars
+    // public vars
+	const uint8_t N = DATA_LEN;
+
+	uint16_t 	head[6];
+	float 		f_values[20];
+	uint8_t 	checksum;
+	uint8_t 	buffer[80];     // RX buffer
+	uint8_t buffCnt;            // max 255
+	uint8_t expected;
+	uint8_t num_floats;
+	uint8_t k_write;
+
+private:
+
+    // private members 
+    void sendCmd(char);     // sends comand to device
+    void callBack();        // ISR for storing serial bytes
+    void callBack_2();        // ISR for storing serial bytes
+    void init();            // re initializes the buffers and the statemachine
+    float Ts;
+    EventQueue printfQueue;
+    bool analyse_received_data(void);
+    
+// -------------------
+//	uint8_t buffer[BUF_LEN];     // RX buffer
+//	uint8_t buffCnt;            // max 255
+	uint8_t state;              // statemachine state variable
+	BufferedSerial* uart;   // pointer to uart for communication with device
+    ThreadFlag              threadFlag;
+    Thread                  thread;
+    Ticker                  ticker;
+    Mutex mutex;
+	void sendThreadFlag();
+	void send_data(char,char,uint16_t,float*);
+	void send_data(char,char,uint16_t,int16_t*);
+	void send_data(char,char,int16_t);
+	void send_data(char,char,uint8_t);
+};
+
+#endif
+
+