Инструменты пользователя

Инструменты сайта


lesson7

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

Предыдущая версия справа и слева Предыдущая версия
Следующая версия
Предыдущая версия
lesson7 [2019/05/14 11:25]
golikov [Программа ориентации по магнитометру]
lesson7 [2020/02/17 15:14]
golikov
Строка 10: Строка 10:
 import math import math
  
-kd 200 К-т дифференциальной ​обратной связи. Если угловая скорость спутника положительна, то спутник надо раскручивать по часовой стрелки, т.е. маховик надо ​разгонять по часовой стрелке+time_step ​0.1 Временной ​шаг ​работы алгоритма, ​с 
-kp -50 К-т пропорциональной обратной связи. Если текущий угол ​больше целевогото спутник надо вращать ​против часовой стрелки, соответственно ​маховик ​надо разгонять против часовой стрелки. +omega_goal = 0.0 # Целевая ​угловая скорость спутника, ​град/с. Для ​режима стабилизации равна 0.0
-time_step ​0.05 Временной шаг ​работы+alpha_goal ​0 Целевой угол ​поворота 
 +mtr_max_speed = 5000 # Максимально допустимая скорость маховикаоб/мин 
 +mtr_num = 1 # Номер маховика 
 +hyr_num = 1 # Номер ДУС 
 +mag_num ​1 Номер магнитометра
  
-mtr_num ​1 Номер маховика + 
-hyr_num ​1 Номер ДУС +errora ​0            Ошибка по углу 
-mag_num ​1 Номер магнитометра+Pa = 0          # Воздействие пропорционального звена 
 +Da = 0          # Воздействие дифференциального звена 
 +Ia = 0          # Воздействие интегрального звена 
 +Kpa = 200 # Пропорциональный ​коэфициент ошибки по углу 
 +Kda = 0.02            # Дифференциальный коэффициент 
 +Kia 0.5            # Интегральный коэффициент 
 +lastErrora = 0        # Прошлая ошибка по углу 
 +Integratora = 0      # Интеграл (сумма всех ошибок по углу) 
 +PID_alpha = 0 Величина управляющего воздействия 
 +Integrator_maxa = 10 # Ограничение ​максимального значения интергатора 
 +Integrator_mina = -10 # Ограничение минимального значения интергатора 
 + 
 +error = 0          # Ошибка по угловой скорости 
 +P = 0          # Воздействие пропорционального звена 
 +D = 0          # Воздействие дифференциального звена 
 +I = 0          # Воздействие интегрального звена 
 +Kp = 200            # Пропорциональный коэффициент 
 +Kd = 0.02           ​ # ​Дифференциальный коэффициент 
 +Ki 0.5            Интегральный коэффициент 
 +lastError = 0        # Прошлая ошибка по угловой скорости 
 +Integrator = 0      # Интеграл (сумма всех ошибок по угловой скорости) 
 +PID_omega = 0        # Величина управляющего воздействия 
 +Integrator_max = 10  # Ограничение ​максимального значения интергатора 
 +Integrator_min = -10 # Ограничение ​минимального значения интергатора
  
 # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
 def mag_calibrated(magx,​magy,​magz):​ def mag_calibrated(magx,​magy,​magz):​
- magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24+ magx_cal = 1.451626*(magx + 47.687257) + 0.036309*(magy + -1468.312497) + 0.008968*(magz + 48.449050
- magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24+ magy_cal = 0.036309*(magx + 47.687257) + 0.408031*(magy + -1468.312497) + 0.074774*(magz + 48.449050
- magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24)+ magz_cal = 0.008968*(magx + 47.687257) + 0.074774*(magy + -1468.312497) + 1.429262*(magz + 48.449050)
  return magx_cal, magy_cal, magz_cal  return magx_cal, magy_cal, magz_cal
-  +  
-# Функция ​motor_new_speed_PD ​определяет новую скорость маховика +# Функции для определение ​новой скорости маховика. 
-# Новая скорость маховика ​равна текущей скорости маховика ​приращение скорости +# Новая скорость маховика ​складывается из 
-# Приращение скорости пропорционально ошибке по углу и ошибке ​по угловой скорости+текущей скорости маховика ​mtr_speed и приращения скорости ​PID 
 +# Приращение скорости ​получено путем сложения трех коэффициентов 
 +# P - пропорциональный коэффициент регулятора 
 +# I - интегральный ​коэффициент регулятора 
 +# D - дифференциальный коэффициент регулятора 
 +# Integrator накапливает суммарную ​ошибку 
 +# Сатуратор ограничивает максимальную величину суммарной ​ошибки 
 +# mtr_speed - текущая угловая скорость маховика,​ об/​мин 
 +# omega - текущая ​угловая скорость спутника,​ град/​с 
 +# omega_goal - целевая угловая ​скорость спутника, град/​с 
 +# mtr_new_speed - требуемая угловая скорость маховика,​ об/мин
 def motor_new_speed_PD(mtr_speed,​ alpha, alpha_goal, omega, omega_goal):​ 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)) + global Integrator 
- return mtr_new_speed + global lastError 
- # return 0+ global Integratora 
 + global lastErrora
   
 + error = omega - omega_goal ​           # Вычисление ошибки
 + P = Kp * error                        # Вычисление воздействия пропорционального звена
 + D = Kd * (error - lastError) / time_step # Вычисление воздействия дифференциального звена
 + lastError = error                    # Запоминаем ошибку
 + Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку
 + if Integrator > Integrator_max: ​         # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
 + Integrator = Integrator_max
 + elif Integrator < Integrator_min:​
 + Integrator = Integrator_min
 + I = Integrator * Ki             # Вычисление воздействия интегрального звена
 + PID_omega = P + I + D      # Вычисление суммарного управляющего воздействия
 +
 + errora = alpha - alpha_goal ​           # Вычисление ошибки
 + Pa = Kpa * errora ​                       # Вычисление воздействия пропорционального звена
 + Da = Kda * (errora - lastErrora) / time_step # Вычисление воздействия дифференциального звена
 + lastErrora = errora ​                   # Запоминаем ошибку
 + Integratora = Integratora + errora * time_step # Накапливаем суммарную ошибку
 + if Integratora > Integrator_maxa: ​         # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
 + Integratora = Integrator_maxa
 + elif Integratora < Integrator_mina:​
 + Integratora = Integrator_mina
 + Ia = Integratora * Kia             # Вычисление воздействия интегрального звена
 + PID_alpha = Pa + Ia + Da      # Вычисление суммарного управляющего воздействия
 +
 + mtr_new_speed = int(mtr_speed + PID_omega + PID_alpha)
 + 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
 + print "P= ", P, "I= ", I, "D= ", D, "​PID_omega= ", PID_omega, "​PID_alpha=",​ PID_alpha, "​mtr_new_speed= ", mtr_new_speed  ​
 + print "Pa= ", Pa, "Ia= ", Ia, "Da= ", Da, "​PID_alpha=",​ PID_alpha
 + return mtr_new_speed
 + 
 + 
 +# Функция включает все приборы,​
 +# которые будут использоваться в основной программе.
 def initialize_all():​ # Функция инициализации всех систем def initialize_all():​ # Функция инициализации всех систем
  print "​Enable motor №", mtr_num ​  print "​Enable motor №", mtr_num ​
  motor_turn_on(mtr_num)  motor_turn_on(mtr_num)
  sleep(1)  sleep(1)
-  
  print "​Enable angular velocity sensor №", hyr_num ​  print "​Enable angular velocity sensor №", hyr_num ​
  hyro_turn_on(hyr_num) # Включаем ДУС  hyro_turn_on(hyr_num) # Включаем ДУС
  sleep(1)  sleep(1)
-  
  print "​Enable magnetometer",​ mag_num  print "​Enable magnetometer",​ mag_num
  magnetometer_turn_on(mag_num)  magnetometer_turn_on(mag_num)
Строка 56: Строка 128:
  motor_turn_off(mtr_num)  motor_turn_off(mtr_num)
  print "​Finish program"​  print "​Finish program"​
-  +  
-def control(): ​основная функция программы+  
 +Основная функция программы, в которой вызываются остальные функции. 
 +def control():
  initialize_all()  initialize_all()
  mtr_state = 0 # Инициализируем статус маховика  mtr_state = 0 # Инициализируем статус маховика
  hyro_state = 0 # Инициализируем статус ДУС  hyro_state = 0 # Инициализируем статус ДУС
  mag_state = 0 # Инициализируем статус магнитометра  mag_state = 0 # Инициализируем статус магнитометра
- alpha_goal = 0 # Целевой угол+ alpha_goal = 90 # Целевой угол
  omega_goal = 0 # Целевая угловая скорость  omega_goal = 0 # Целевая угловая скорость
  mag_alpha = 0  mag_alpha = 0
-  +          
- for i in range(500): + for i in range(200): 
-опрос датчиков и маховика+ #print "i = ", i 
 +  
 + # Опрос датчика угловой скорости ​и маховика.
  mag_state,​ magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num)  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) ​  hyro_state,​ gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) ​
- mtr_state,​ mtr_speed = motor_request_speed(mtr_num)+ mtr_state,​ mtr_speed = motor_request_speed(mtr_num)
   
  if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет  if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет
  magx_cal,​ magy_cal, magz_cal = mag_calibrated(magx_raw,​magy_raw,​magz_raw)  magx_cal,​ magy_cal, magz_cal = mag_calibrated(magx_raw,​magy_raw,​magz_raw)
- magy_cal = - magy_cal # переходим из левой системы координат,​ которая изображена на магнитометре в правую, для того чтобы  + magy_cal = - magy_cal # переходим из левой системы координат,​ которая изображена на магнитометре в правую
-                                                  положительное направление угла было против часовой стрелки+
  mag_alpha = math.atan2(magy_cal,​ magx_cal)/​math.pi*180  mag_alpha = math.atan2(magy_cal,​ magx_cal)/​math.pi*180
- print "​magx_cal =", magx_cal, "​magy_cal =", magy_cal, "​magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра+ #mag_alpha = angle_transformation(mag_alpha,​ alpha_goal) 
 + #print "​magx_cal =", magx_cal, "​magy_cal =", magy_cal, "​magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра
  print "​mag_alpha atan2= ", mag_alpha  print "​mag_alpha atan2= ", mag_alpha
  elif mag_state == 1:  elif mag_state == 1:
Строка 83: Строка 159:
  elif mag_state == 2:  elif mag_state == 2:
  print "Fail because of interface error, check your code"  print "Fail because of interface error, check your code"
-  +  ​ 
- if not hyro_state: ​если ДУС ​вернул код ошибки 0, т.е. ошибки нет+Обработка показаний датчика угловой скорости
 +вычисление угловой скорости спутника по показаниям ДУС. 
 + # Если ​код ошибки ​ДУС равен ​0, т.е. ошибки нет 
 + if not hyro_state:
  gx_degs = gx_raw * 0.00875  gx_degs = gx_raw * 0.00875
  gy_degs = gy_raw * 0.00875  gy_degs = gy_raw * 0.00875
  gz_degs = gz_raw * 0.00875  gz_degs = gz_raw * 0.00875
- omega = gz_degs # если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z + # если ДУС установлен осью z вверх, то угловая скорость 
- print "​gx_degs =", gx_degs, "​gy_degs =", gy_degs, "​gz_degs =", gz_degs ​# Выводим данные +спутника совпадает с показаниями ДУС по оси z, иначе 
- elif hyro_state == 1: # если датчик вернул сообщение об ошибке 1+ # необходимо изменить знак: omega = - gz_degs 
 + omega = gz_degs 
 + #print "​gx_degs =", gx_degs, "​gy_degs =", gy_degs, "​gz_degs =", gz_degs 
 + elif hyro_state == 1:
  print "Fail because of access error, check the connection"​  print "Fail because of access error, check the connection"​
- elif hyro_state == 2: # если датчик вернул сообщение об ошибке 2+ elif hyro_state == 2:
  print "Fail because of interface error, check your code"  print "Fail because of interface error, check your code"
-  +  
- if not mtr_state: если маховик вернул код ошибки 0, т.е. ошибки нет+ #Обработка показаний маховика и установка требуемой угловой скорости. 
 + if not mtr_state:​ #​ если ​код ошибки 0, т.е. ошибки нет
  print "​Motor_speed:​ ", mtr_speed  print "​Motor_speed:​ ", mtr_speed
- mtr_new_speed = motor_new_speed_PD(mtr_speed,​mag_alpha,​alpha_goal,​gz_degs,​omega_goal) # установка новой скорости маховика+ # установка новой скорости маховика 
 + mtr_new_speed = motor_new_speed_PD(mtr_speed,​ mag_alpha, alpha_goal, omega, omega_goal)
  motor_set_speed(mtr_num,​ mtr_new_speed)  motor_set_speed(mtr_num,​ mtr_new_speed)
- + 
  sleep(time_step)  sleep(time_step)
 + 
  switch_off_all()  switch_off_all()
 </​file>​ </​file>​
Строка 123: Строка 208:
 const float time_step = 0.05; // Временной шаг работы const float time_step = 0.05; // Временной шаг работы
 const uint16_t mtr_num = 1; // Номер маховика const uint16_t mtr_num = 1; // Номер маховика
-const uint16_t hyr_num = 1;  // Номер ДУС +const uint16_t hyr_num = 1;          ​// Номер ДУС 
-const uint16_t mag_num = 1;​ //​Номер магнитометра+const uint16_t mag_num = 1; // Номер магнитометра
 // Максимально допустимая скорость маховика,​ об/мин // Максимально допустимая скорость маховика,​ об/мин
 const int16_t mtr_max_speed = 5000; const int16_t mtr_max_speed = 5000;
Строка 168: Строка 253:
 /* Функция инициализации всех систем,​ /* Функция инициализации всех систем,​
  включает все приборы,​которые будут использоваться в основной программе.*/​  включает все приборы,​которые будут использоваться в основной программе.*/​
- printf ("​Enable motor №%d\n",​ mtr_num); ​//а нужнали ссылка??​+ printf ("​Enable motor №%d\n",​ mtr_num);
  motor_turn_on(mtr_num);​  motor_turn_on(mtr_num);​
  Sleep(1);  Sleep(1);
lesson7.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

Инструменты страницы