diff --git a/3rd/mt6701.c b/3rd/mt6701.c
index 93f796c..27c008b 100644
--- a/3rd/mt6701.c
+++ b/3rd/mt6701.c
@@ -3,6 +3,7 @@
 
 volatile int16_t angle;
 volatile float angle_f;
+volatile float angle_f_rad;
 
 /* Data sent to the Target */
 uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
@@ -103,8 +104,56 @@ void MT6701_iic_read_angel(void)
         gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
     }
 }
+volatile float Last_ts = 0.0;
+volatile float last_angle = 0.0;
+float GetAngle_NoTrack(void)
+{
+	  MT6701_iic_read_angel();
+    angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
+		angle_f_rad = (float)angle * _2PI / 16384;
+	return angle_f_rad;
+}
+volatile float full_rotations = 0.0;
+volatile float Last_Angle = 0.0;
+float GetAngle(void)
+{
+	volatile  float D_Angle = 0.0;
+	volatile float Angle = GetAngle_NoTrack();
+	D_Angle = Angle - Last_Angle;
+	
+	if( fabs(D_Angle) > (0.8f*2*PI) )
+	{
+		full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1);
+	}
+	
+	Last_Angle = Angle;
+	
+	return (full_rotations * 2 * PI + Last_Angle);
+}
 
+volatile float Last_Vel_ts = 0.0;
+volatile float Vel_Last_Angle = 0.0;
+float GetVelocity(void)
+{
+	volatile float dt = 0.0;
+	volatile float Vel_ts = SysTick -> VAL;
+	if(Vel_ts < Last_Vel_ts) dt = (Last_Vel_ts - Vel_ts)/9*1e-6f;
+	else dt = (0xFFFFFF - Vel_ts + Last_Vel_ts)/9*1e-6f;
+	
+	if(dt < 0.0001) dt = 10000;
+	
+	float Vel_Angle = GetAngle();
+	
+	float dv = Vel_Angle - Vel_Last_Angle;
 
+	float velocity = (Vel_Angle - Vel_Last_Angle)/dt;
+	
+	Last_Vel_ts = Vel_ts;
+  Vel_Last_Angle = Vel_Angle;
+  
+	
+  return velocity;
+}
 
 void MT6701_get_angle_degree(void)
 {
diff --git a/3rd/mt6701.h b/3rd/mt6701.h
index 05620bc..468a809 100644
--- a/3rd/mt6701.h
+++ b/3rd/mt6701.h
@@ -7,6 +7,10 @@
 extern "C"
 {
 #endif
+
+#define PI       3.14159265359f
+#define _2PI       6.28318530718f
+
 #define I2C_TX_PACKET_SIZE (1)
 
 /*
@@ -17,6 +21,9 @@ extern "C"
 #define I2C_RX_PACKET_SIZE (2)
 void MT6701_iic_read_angel(void);
 void MT6701_get_angle_degree(void);
+float GetAngle(void);
+float GetAngle_NoTrack(void);
+float GetVelocity(void);
 
 #ifdef __cplusplus
 }
diff --git a/empty.c b/empty.c
index 0640f58..5f4c9ad 100644
--- a/empty.c
+++ b/empty.c
@@ -42,16 +42,18 @@ extern float angle_f;
 
 const float num_f = 0.123456f;
 
+
 int main(void)
 {
     SYSCFG_DL_init();
     NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
     while (1) {
 			DL_GPIO_togglePins(LED_PORT,LED_PA0_PIN);
-			MT6701_get_angle_degree();
+			//MT6701_get_angle_degree();
 			delay_ms(20);
+			volatile float angle_rad = GetAngle();
 			//printf("test \n");
-			printf("angle %.5f \n" , angle_f);
+			printf("angle %.5f \n" , angle_rad);
     }
 }