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()