import time 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 = -100 # 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 for the positioning and stabilization system mtr_num = 1 # flywheel number hyr_num = 1 # angular velocity sensor number mag_num = 1 # magnetometer number # Maximum allowed flywheel speed, rev/min mtr_max_speed = 3000 sun_max = 25000 # Maximum value of the solar sensor (higher values are deemed as errors) sun_min = 50 # Minimum value of the solar sensor (lower values are deemed as errors) # To define the steering command for the flywheel it is critical to know not the angular orientation of the satellite as related to magnetic field but the angular error – the difference between the current angle and target angle alpha_goal # By default the angle will be defined in range from -180 to 180 degree # angle_transformation function will change the angle measurement range. For the task of positioning the angle must be measured in the range from (alpha_goal-180) to (alpha_goal+180), where alpha_goal is the target angle between magnetometer’s x-axis and projection of magnetic field vector to horizontal plane. def angle_transformation(alpha, alpha_goal): if alpha<=(alpha_goal - 180): alpha = alpha + 360 elif alpha>(alpha_goal +180): alpha = alpha - 360 return alpha # Specifying the flywheel 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)) if mtr_new_speed > mtr_max_speed: mtr_new_speed = mtr_max_speed elif mtr_new_speed < -mtr_max_speed: mtr_new_speed = -mtr_max_speed return mtr_new_speed def initialize_all(): 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 on the angular velocity sensor sleep(1) print "Enable Sun sensors 1-4" sun_sensor_turn_on(1) sun_sensor_turn_on(2) sun_sensor_turn_on(3) sun_sensor_turn_on(4) sleep(1) def switch_off_all(): print "Finishing..." hyro_turn_off(hyr_num) # Switch off the angular velocity sensor sun_sensor_turn_off(1) sun_sensor_turn_off(2) sun_sensor_turn_off(3) sun_sensor_turn_off(4) motor_set_speed(mtr_num, 0) sleep (1) motor_turn_off(mtr_num) print "Finish program" def check_sun(sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42): delta1 = 0 delta2 = 0 delta3 = 0 delta4 = 0 delta5 = 0 delta6 = 0 delta7 = 0 delta8 = 0 delta_sum = 36000 delta_min = 35000 delta_i = 0 data_sun = [] for i in range(0, len(data_sun), 9): # len(data_sun) - array length delta1 = abs(sun11 - data_sun[i+0]) delta2 = abs(sun12 - data_sun[i+1]) delta3 = abs(sun21 - data_sun[i+2]) delta4 = abs(sun22 - data_sun[i+3]) delta5 = abs(sun31 - data_sun[i+4]) delta6 = abs(sun32 - data_sun[i+5]) delta7 = abs(sun41 - data_sun[i+6]) delta8 = abs(sun42 - data_sun[i+7]) delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8 #print i, data_sun[i+7] #print "Sun_sens", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42 #print "deltas= ", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum if delta_min > delta_sum : delta_min = delta_sum delta_i = data_sun[i+8] #print "delta_min= ", delta_min, "delta_i= ", delta_i return delta_i def sun_range(sun, sun_old): if sun[1] > sun_max: sun[1] = sun_old[1] if sun[1] < sun_min: sun[1] = sun_old[1] if sun[2] > sun_max: sun[2] = sun_old[2] if sun[2] < sun_min: sun[2] = sun_old[2] return sun def control(): # Program’s main function initialize_all() # Initialize flywheel status mtr_state = 0 # Initialize angular velocity sensor status hyro_state = 0 sun_sensor_num = 0 # Initialize variable for solar sensor number sun_result_1 = [0,0,0] # Initialize sun_result_1 sun_result_2 = [0,0,0] # Initialize sun_result_2 sun_result_3 = [0,0,0] # Initialize sun_result_3 sun_result_4 = [0,0,0] # Initialize sun_result_4 sun_result_1_Old = [0,0,0] # Initialize sun_result_1_Old sun_result_2_Old = [0,0,0] # Initialize sun_result_2_Old sun_result_3_Old = [0,0,0] # Initialize sun_result_3_Old sun_result_4_Old = [0,0,0] # Initialize sun_result_4_Old omega_goal = 0 # Target angular velocity alpha_goal = 30 # Target turning angle for i in range(300): # Put into memory the old values sun_result_1_Old = sun_result_1 sun_result_2_Old = sun_result_2 sun_result_3_Old = sun_result_3 sun_result_4_Old = sun_result_4 # sensors and flywheel data request sun_result_1 = sun_sensor_request_raw(1) sun_result_2 = sun_sensor_request_raw(2) sun_result_3 = sun_sensor_request_raw(3) sun_result_4 = sun_sensor_request_raw(4) # print sun_result_1, sun_result_2, sun_result_3, sun_result_4 # Checking the new measured value for going out of range sun_result_1 = sun_range(sun_result_1, sun_result_1_Old) sun_result_2 = sun_range(sun_result_2, sun_result_2_Old) sun_result_3 = sun_range(sun_result_3, sun_result_3_Old) sun_result_4 = sun_range(sun_result_4, sun_result_4_Old) alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2]) alpha_sun = angle_transformation(alpha_sun, alpha_goal) print alpha_sun hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) if not hyro_state: # if angular velocity 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,alpha_sun,alpha_goal,gz_degs,omega_goal) # set the flywheel’s new speed motor_set_speed(mtr_num, mtr_new_speed) sleep(time_step) switch_off_all()