added code handling IMU and started writing stabilization code

Core/Src/stabilize.c

@@ -0,0 +1,40 @@
+/*
+ * stabilize.c
+ *
+ *  Created on: Aug 9, 2025
+ *      Author: angoosh
+ */
+
+#include "stabilize.h"
+
+Stabilize_Typedef STAB;
+
+void Stabilize_init(){
+	STAB.pitch_gain = 1;
+	STAB.roll_gain = 1;
+	STAB.yaw_gain = 1;
+}
+
+int Stabilize_Roll(int servo){
+	int stab_servo = 0;
+
+	stab_servo = servo + (IMU.roll * STAB.roll_gain);
+
+	return stab_servo;
+}
+
+int Stabilize_Pitch(int servo){
+	int stab_servo = 0;
+
+	stab_servo = servo + (IMU.pitch * STAB.pitch_gain);
+
+	return stab_servo;
+}
+
+int Stabilize_Yaw(int servo){
+	int stab_servo = 0;
+
+	stab_servo = servo + (IMU.yaw * STAB.yaw_gain);
+
+	return stab_servo;
+}