#include <stdio.h>
	 #include <stdint.h>
	 #include "libschsat.h"
         void control(void){

	 float gyro_x = 0, gyro_y = 0, gyro_z = 0;
	 uint16_t tmp=0;
	 
	 gyro_set_offset(0, 4.6, -2.0, 0.7); //эти значения будут другими
	 
	 float gyro_set = 0.0;
	 float p_koeff = 1.0;
	 float motor_set = 0;
	 	 
	 for (int i = 0; i < 300; i++)
	 {
		gyro_request_raw(tmp, &gyro_x, &gyro_y, &gyro_z); // gyro returns degrees per sec!
		printf("Gyro: %6f\t  Wheel_rpm: %6f\n", gyro_z, motor_set);

		// simple P-controller
		float delta_gyro = (gyro_set - gyro_z );
		if (fabs(delta_gyro) > 0.001)
		{
			motor_set += delta_gyro * p_koeff;
		}
		motor_set_speed(0, motor_set);
		mSleep(100);
	 }
	 motor_set_speed(0, 0);
	 // set telemetry period 1 sec
	 puts("Job done!");
    }