Mirror actuator for RT2 lab

Dependencies:   Library_Cntrl Library_Misc

Files at this revision

API Documentation at this revision

Comitter:
altb2
Date:
Thu Apr 15 05:36:55 2021 +0000
Parent:
6:9ebeffe446e4
Commit message:
intermediate

Changed in this revision

ControllerLoop.cpp Show annotated file Show diff for this revision Revisions of this file
ControllerLoop.h Show annotated file Show diff for this revision Revisions of this file
Library_Misc.lib Show annotated file Show diff for this revision Revisions of this file
Mirror_Kinematic.cpp Show annotated file Show diff for this revision Revisions of this file
Mirror_Kinematic.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
uart_comm_thread.cpp Show annotated file Show diff for this revision Revisions of this file
uart_comm_thread.h Show annotated file Show diff for this revision Revisions of this file
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
+
+