import time import math #!/usr/bin/env python # -*- coding: utf-8 -*- # Differential feedback coefficient. kd = 200.0 # Algorithm working time step, s time_step = 0.05 # Satellite’s target angular velocity, deg/s. # For stabilization mode it is 0.0. omega_goal = 0.0 # Maximum allowed flywheel speed, rev/min mtr_max_speed = 5000 # Flywheel number mtr_num = 1 # Angular velocity sensors’ number hyr_num = 1 # Magnetometer number mag_num = 1 # Measuring result’s number i = 1 # Current turning angle alpha = 0.0 # The function will switch on all apparatuses # that will be used in the main program. def initialize_all(): print "Enable angular velocity sensor №", hyr_num hyro_turn_on(hyr_num) sleep(1) print "Enable magnetometer", mag_num magnetometer_turn_on(mag_num) sleep(1) # Wait 1 second for switching on 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) print "Enable motor №", mtr_num motor_turn_on(mtr_num) sleep(1) # The function will switch off all sensors, # that will be used in the main program. def switch_off_all(): print "Finishing..." hyro_turn_off(hyr_num) magnetometer_turn_off(mag_num) 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 mag_calibrated(magx,magy,magz): # instead of these 3 lines of code with calibration factors there must be the lines with calibration factors of your magnetometer #magx_cal = 1.04*magx - 0.26*magy + 0.05*magz - 68.76 # it is the 1st line that must be changed with results of your magnetometer calibration #magy_cal = 0.24*magx + 1.04*magy + 0.29*magz + 256.92 # it is the 2nd line that must be changed with results of your magnetometer calibration а #magz_cal = -0.09*magx - 0.19*magy + 0.77*magz + 159.41 # it is the 3rd line that must be changed with results of your magnetometer calibration 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 # Functions that defines flywheel’s new speed. # Flywheel’s new speed will be the sum of # flywheel’s current speed and speed increment. # Speed increment is proportioned by angle error # and angular velocity error. # mtr_speed - flywheel’s current angular speed, rev/min # omega – satellite’s current angular speed, deg/s # omega_goal - target satellite’s angular speed, deg/s # mtr_new_speed - target flywheel’s angular speed, rev/min def motor_new_speed_PD(mtr_speed, omega, omega_goal): mtr_new_speed = int(mtr_speed + 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 # Program’s main function that will call other functions. def control(): omega_goal = 0 # omega_goal - target satellite’s angular speed, deg/s initialize_all() # Initialize flywheel’s status mtr_state = 0 # Initialize angular velocity sensor’s status hyro_state = 0 sun_sensor_num = 0 # Initialize the variable for the solar sensor’s 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 mag_alpha = 0 output_data_all = [0, 0, 0, 0, 0, 0, 0, 0, 0] output_data = [0, 0, 0, 0, 0, 0, 0, 0, 0] # Measuring result’s number i = 0 # Put into memory rotation start time time_start = time.time() # Put into memory one-second interval start time time_interval = time.time() # solar sensors data output interval in seconds time_output = 0.1 while True: # angular velocity sensor and flywheel data request hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) # Processing the readings of angular velocity sensor, # calculation of satellite angular velocity upon angular velocity sensor’s readings. # If angular velocity sensor’s error code is 0 (no error) if not hyro_state: gx_degs = gx_raw * 0.00875 gy_degs = gy_raw * 0.00875 gz_degs = gz_raw * 0.00875 # if angular velocity sensor arranged via z-axis up, then satellite’s angular velocity # matches to sensors’ readings of z-axis, otherwise # it is required to reverse the sign: omega = - gz_degs omega = gz_degs elif hyro_state == 1: print "Fail because of access error, check the connection" elif hyro_state == 2: print "Fail because of interface error, check your code" # Processing the readings of flywheel and setting the target angular velocity. if not mtr_state: # if magnetometer returned error code 0 (no error) # set flywheel’s new speed mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal) motor_set_speed(mtr_num, mtr_new_speed) time.sleep(time_step) time_current = time.time() - time_start 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 if (time.time() - time_interval) > time_output: # Put into memory next one-second interval starting time time_interval = time.time() 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, mag_alpha, time_current output_data = [time_current, 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], mag_alpha] output_data_all += output_data if i > 100: # Start rotation upon 5 seconds delay from the moment of launching omega_goal = 6.0 # omega_goal – satellite’s target angular velocity, deg/s if time_current > 90: break i += 1 switch_off_all() print "time_end = " , time.time() - time_start for i in range(0, 5000, 10): print output_data_all[i-1], output_data_all[i], output_data_all[i+1], output_data_all[i+2], output_data_all[i+3], output_data_all[i+4], output_data_all[i+5], output_data_all[i+6], output_data_all[i+7], output_data_all[i+8]