import math

kd = 200			# Differential feedback coefficient. If the satellite’s angular velocity is positive, than satellite must be spun clock-wise (i.e. the flywheel must rotated clock-wise).
kp = -50			# Proportional feedback coefficient. If the current angle is more than target angle, than satellite must be spun counter-clock-wise (i.e. the flywheel must rotated counter-clock-wise).
time_step = 0.05		# working time-step

mtr_num = 1			# flywheel number
hyr_num = 1			# angular velocity sensor number
mag_num = 1			# magnetometer number

# mag_calibrated function will adjust the magnetometer’s readings considering the calibration factors
def mag_calibrated(magx,magy,magz):
	magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24)
	magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24)
	magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24)
	return magx_cal, magy_cal, magz_cal
	
# motor_new_speed_PD function defines flywheel’s new speed 
# flywheel’s new speed equals to flywheel’s current speed + speed increment
# Speed increment is proportioned by angle error and angular velocity error 

def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
	mtr_new_speed = int(mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal))
	return mtr_new_speed
	# return 0
	
def initialize_all(): # All systems’ initialization function
	print "Enable motor №", mtr_num 
	motor_turn_on(mtr_num)
	sleep(1)
	
	print "Enable angular velocity sensor №", hyr_num 
	hyro_turn_on(hyr_num) # Switch the sensor on
	sleep(1)
	
	print "Enable magnetometer", mag_num
	magnetometer_turn_on(mag_num)
	sleep(1)
	
def switch_off_all(): # All systems’ switching off function
	print "Finishing..."
	print "Disable angular velocity sensor №", hyr_num
	hyro_turn_off(hyr_num)   # Switch the sensor off
	print "Disable magnetometer", mag_num
	magnetometer_turn_off(mag_num)
	motor_set_speed(mtr_num, 0)
	sleep (1)
	motor_turn_off(mtr_num)
	print "Finish program"
	
def control(): # Program’s main function
	initialize_all()
	mtr_state = 0		# Initialize flywheel status
	hyro_state = 0 		# Initialize angular velocity sensor status
	mag_state = 0 		# Initialize magnetometer status
	alpha_goal = 0		# Target angle
	omega_goal = 0 		# Target angular velocity
	mag_alpha = 0
	
	for i in range(500):
		# sensors and flywheel data request
		mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)
		hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
		mtr_state, mtr_speed = motor_request_speed(mtr_num)
		
		if not mag_state: # if magnetometer returned error code 0 (no error)
			magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw)
			magy_cal = - magy_cal	# transition from the left coordinate system which is plotted on the magnetometer, to the right coordinate system to position the positive angle direction counter clock-wise 
			mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180
			print "magx_cal =", magx_cal, "magy_cal =", magy_cal, "magz_cal =", magz_cal # output of the magnetometer calibrated values 
			print "mag_alpha atan2= ", mag_alpha
		elif mag_state == 1:
			print "Fail because of access error, check the connection"
		elif mag_state == 2:
			print "Fail because of interface error, check your code"
		
		if not hyro_state: # if the sensor returned error code 0 (no error)
			gx_degs = gx_raw * 0.00875
			gy_degs = gy_raw * 0.00875
			gz_degs = gz_raw * 0.00875
			omega = gz_degs	# if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis
			print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные
		elif hyro_state == 1: # if the sensor returned error code 1
			print "Fail because of access error, check the connection"
		elif hyro_state == 2: # if the sensor returned error code 2
			print "Fail because of interface error, check your code"
		
		if not mtr_state:	# if the flywheel returned error code 0 (no error)
			print "Motor_speed: ", mtr_speed
			mtr_new_speed = motor_new_speed_PD(mtr_speed,mag_alpha,alpha_goal,gz_degs,omega_goal)	# setting the flywheel’s new speed 
			motor_set_speed(mtr_num, mtr_new_speed)
				
		sleep(time_step)
	switch_off_all()