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

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


lesson7

Различия

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

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

lesson7 [2020/02/17 15:14]
golikov
lesson7 [2020/03/25 16:28]
Строка 1: Строка 1:
-====== 07 Урок. Ориентация Орбикрафт с помощью магнитометра ====== 
-===== Программа ориентации по магнитометру ===== 
-Создайте следующую программу. Коэффициенты для функции def mag_calibrated возьмите из программы Magneto. 
- 
-{{::​07image001.png?​nolink&​400|}} 
- 
-Код на Python. 
- 
-<file python orientation.py>​ 
-import math 
- 
-time_step = 0.1 # Временной шаг работы алгоритма,​ с 
-omega_goal = 0.0 # Целевая угловая скорость спутника,​ град/​с. Для режима стабилизации равна 0.0. 
-alpha_goal = 0 # Целевой угол поворота 
-mtr_max_speed = 5000 # Максимально допустимая скорость маховика,​ об/мин 
-mtr_num = 1 # Номер маховика 
-hyr_num = 1 # Номер ДУС 
-mag_num = 1 # Номер магнитометра 
- 
- 
-errora = 0            # Ошибка по углу 
-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 вносит поправки в показания магнитометра с учетом калибровочных коэффициентов 
-def mag_calibrated(magx,​magy,​magz):​ 
- magx_cal = 1.451626*(magx + 47.687257) + 0.036309*(magy + -1468.312497) + 0.008968*(magz + 48.449050) 
- magy_cal = 0.036309*(magx + 47.687257) + 0.408031*(magy + -1468.312497) + 0.074774*(magz + 48.449050) 
- magz_cal = 0.008968*(magx + 47.687257) + 0.074774*(magy + -1468.312497) + 1.429262*(magz + 48.449050) 
- return magx_cal, magy_cal, magz_cal 
-  
-# Функции для определение новой скорости маховика. 
-# Новая скорость маховика складывается из 
-# текущей скорости маховика 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):​ 
- global Integrator 
- global lastError 
- 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():​ # Функция инициализации всех систем 
- print "​Enable motor №", mtr_num ​ 
- motor_turn_on(mtr_num) 
- sleep(1) 
- 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) 
-  
-def switch_off_all():​ # Функция выключения всех систем 
- print "​Finishing..."​ 
- print "​Disable angular velocity sensor №", hyr_num 
- hyro_turn_off(hyr_num) ​  # Выключаем ДУС 
- 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(): 
- initialize_all() 
- mtr_state = 0 # Инициализируем статус маховика 
- hyro_state = 0 # Инициализируем статус ДУС 
- mag_state = 0 # Инициализируем статус магнитометра 
- alpha_goal = 90 # Целевой угол 
- omega_goal = 0 # Целевая угловая скорость 
- mag_alpha = 0 
-          
- for i in range(200): 
- #print "i = ", i 
-  
- # Опрос датчика угловой скорости и маховика. 
- 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: # если магнитометр вернул код ошибки 0, т.е. ошибки нет 
- magx_cal,​ magy_cal, magz_cal = mag_calibrated(magx_raw,​magy_raw,​magz_raw) 
- magy_cal = - magy_cal # переходим из левой системы координат,​ которая изображена на магнитометре в правую 
- mag_alpha = math.atan2(magy_cal,​ magx_cal)/​math.pi*180 
- #​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 
- 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" 
-  ​ 
- # Обработка показаний датчика угловой скорости,​ 
- # вычисление угловой скорости спутника по показаниям ДУС. 
- # Если код ошибки ДУС равен 0, т.е. ошибки нет 
- if not hyro_state: 
- gx_degs = gx_raw * 0.00875 
- gy_degs = gy_raw * 0.00875 
- gz_degs = gz_raw * 0.00875 
- # если ДУС установлен осью z вверх, то угловая скорость 
- # спутника совпадает с показаниями ДУС по оси z, иначе 
- # необходимо изменить знак: 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"​ 
- elif hyro_state == 2: 
- print "Fail because of interface error, check your code" 
-  
- #​Обработка показаний маховика и установка требуемой угловой скорости. 
- if not mtr_state:​ #​ если код ошибки 0, т.е. ошибки нет 
- print "​Motor_speed:​ ", mtr_speed 
- # установка новой скорости маховика 
- mtr_new_speed = motor_new_speed_PD(mtr_speed,​ mag_alpha, alpha_goal, omega, omega_goal) 
- motor_set_speed(mtr_num,​ mtr_new_speed) 
-  
- sleep(time_step) 
-  
- switch_off_all() 
-</​file>​ 
- 
-Код на С. 
-<file c orientation.c>​ 
-#include <​stdio.h>​ 
-#include <​stdint.h>​ 
-#include "​libschsat.h"​ 
-#define LSS_OK 0  
-#define LSS_ERROR 1  
-#define LSS_BREAK 2 
-#include <​math.h>​ 
- 
-const int kd = 200; /* К-т дифференциальной обратной связи. ​ 
-Если угловая скорость спутника положительна,​ то спутник надо раскручивать 
- по часовой стрелки,​ т.е. маховик надо разгонять по часовой стрелке.*/​ 
-  
-const int kp = -50; /* К-т пропорциональной обратной связи. ​ 
-Если текущий угол больше целевого,​ то спутник надо вращать против ​ 
-часовой стрелки,​ соответственно маховик надо разгонять против часовой стрелки.*/​ 
-const float time_step = 0.05; // Временной шаг работы 
-const uint16_t mtr_num = 1; // Номер маховика 
-const uint16_t hyr_num = 1;         // Номер ДУС 
-const uint16_t mag_num = 1; // Номер магнитометра 
-// Максимально допустимая скорость маховика,​ об/мин 
-const int16_t mtr_max_speed = 5000; 
-int16_t mtr_new_speed;​ 
- 
-int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ 
-//​Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов 
- float magx_cal; 
- magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24); 
- float magy_cal; 
- magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24); 
- float magz_cal; 
- magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24); 
- *magx = (int16_t) magx_cal; 
- *magy = (int16_t) magy_cal; 
- *magz = (int16_t) magz_cal;  
- return 0; 
-} 
- 
-int motor_new_speed_PD(int mtr_speed, int omega, int omega_goal){  
- /* Функция для определения новой скорости маховика. 
- ​Новая скорость маховика складывается из 
- ​текущей скорости маховика и приращения скорости. 
- ​Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. 
- ​mtr_speed - текущая угловая скорость маховика,​ об/мин 
- omega - текущая угловая скорость спутника,​ град/с 
- ​omega_goal - целевая угловая скорость спутника,​ град/с 
- ​mtr_new_speed - требуемая угловая скорость маховика,​ об/​мин*/​ 
-  
- mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal));​  
- if (mtr_new_speed > mtr_max_speed) 
- { 
- mtr_new_speed = mtr_max_speed;​ 
- } 
- else if (mtr_new_speed < -mtr_max_speed) 
- { 
- mtr_new_speed = -mtr_max_speed;​ 
- } 
- return mtr_new_speed;​ 
-} 
- 
-void initialize_all(void){ 
-/* Функция инициализации всех систем,​ 
- включает все приборы,​которые будут использоваться в основной программе.*/​ 
- printf ("​Enable motor №%d\n",​ mtr_num); 
- motor_turn_on(mtr_num);​ 
- Sleep(1); 
- printf("​Enable angular velocity sensor №%d\n",​ hyr_num); ​ 
- hyro_turn_on(hyr_num);​ // Включаем ДУС 
- Sleep(1); 
- printf("​Enable magnetometer %d\n", mag_num); 
- magnetometer_turn_on(mag_num);​ 
- Sleep(1); 
-} 
- 
-int switch_off_all(){ 
-/​*Функция отключает все приборы,​которые будут использоваться в основной программе.*/​ 
- printf("​Finishing..."​);​ 
- int16_t new_speed = mtr_new_speed; ​ 
- printf("​\nDisable angular velocity sensor №%d\n",​ hyr_num); 
- hyro_turn_off(hyr_num);​ 
- printf("​Disable magnetometer %d\n", mag_num); 
- magnetometer_turn_off(mag_num);​ 
- motor_set_speed(mtr_num,​ 0, &​new_speed);​ 
- Sleep (1); 
- motor_turn_off(mtr_num);​ 
- printf("​Finish program\n"​);​ 
- return 0; 
-} 
- 
-int control(){ 
- float mtr_new_speed;​ 
- //​данные магнитометра 
- int16_t mgx_cal=0; 
- int16_t mgy_cal=0; 
- int16_t mgz_cal=0; 
- int16_t *magx_raw = &​mgx_cal;​ 
- int16_t *magy_raw = &​mgy_cal;​ 
- int16_t *magz_raw = &​mgz_cal;​ 
- //​данные ДУС 
- int16_t gx_raw; 
- int16_t gy_raw; 
- int16_t gz_raw; 
- int16_t *hyrox_raw=&​gx_raw;​ 
- int16_t *hyroy_raw= &​gy_raw;​ 
- int16_t *hyroz_raw = &​gz_raw;​ 
- int16_t mtr_speed; 
- int16_t omega; 
- int16_t omega_goal=0; ​ // Целевая угловая скорость 
-  
- initialize_all();​ 
-  
- int mtr_state = 0; // Инициализируем статус маховика 
- int hyro_state = 0; // Инициализируем статус ДУС 
- int mag_state = 0; // Инициализируем статус магнитометра 
-// int alpha_goal = 0; // Целевой угол  
- double mag_alpha = 0; 
- int i; 
- for (i = 0; i < 500; i++) { 
- mag_state = magnetometer_request_raw(mag_num,​ magx_raw, magy_raw, magz_raw); 
- hyro_state = hyro_request_raw(hyr_num,​hyrox_raw,​hyroy_raw,​hyroz_raw); ​ 
- gx_raw = *hyrox_raw; ​ 
- gy_raw = *hyroy_raw; ​ 
- gz_raw = *hyroz_raw; ​ 
- mtr_state = motor_request_speed(mtr_num,​ &​mtr_speed);​ 
- if (!mag_state){ 
- mag_calibrated(magx_raw,​magy_raw,​magz_raw);​ 
- *magy_raw = - *magy_raw;/​* переходим из левой системы координат,​ 
- которая изображена на магнитометре в правую,​ для того чтобы ​ 
- положительное направление угла было против часовой стрелки*/​ 
- mag_alpha = atan2(mgy_cal,​ mgx_cal)/​M_PI*180;​ 
- printf("​magx_cal = %d", mgx_cal); 
- printf("​ magy_cal = %d", mgy_cal); 
- printf("​ magz_cal = %d", mgz_cal); 
- printf("​ mag_alpha atan2 = %f\n", mag_alpha); 
- } 
- else if (mag_state == 1){ 
- printf("​Fail because of access error, check the connection\n"​);​ 
-    } 
- else if (mag_state == 2){ 
- printf("​Fail because of interface error, check your code\n"​);​ 
-   } 
-  
- if (!hyro_state){ 
- float gx_degs = gx_raw * 0.00875; 
- float gy_degs = gy_raw * 0.00875; 
- float gz_degs = gz_raw * 0.00875; 
- /* если ДУС установлен осью z вверх, то угловая скорость 
- // спутника совпадает с показаниями ДУС по оси z, иначе 
- необходимо изменить знак: omega = - gz_degs */ 
- omega = gz_degs; 
- printf("​\ngx_degs = %f", gx_degs); 
- printf("​ gy_degs = %f", gy_degs); 
- printf("​ gz_degs = %f\n", gz_degs); 
- } 
- else if (hyro_state == 1){ 
- printf("​Fail because of access error, check the connection\n"​);​ 
- } 
- else if (hyro_state == 2){ 
- printf("​Fail because of interface error, check your code\n"​);​ 
- } 
-  
- //​Обработка показаний маховика и установка требуемой угловой скорости. 
- if (!mtr_state) { 
- // если код ошибки 0, т.е. ошибки нет 
- int16_t mtr_speed=0;​ 
- motor_request_speed(mtr_num,​ &​mtr_speed);​ 
- printf("​\nMotor_speed:​ %d\n", mtr_speed);​  
- // установка новой скорости маховика 
- mtr_new_speed = motor_new_speed_PD(mtr_speed,​omega,​omega_goal);​ 
- motor_set_speed(mtr_num,​ mtr_new_speed,​ &​omega);​ 
- }  
- Sleep(time_step);​  
- } 
- switch_off_all();​ 
- return 0; 
-} 
-</​file>​ 
- 
-===== Анализ работы программы ===== 
-В программе используются следующие функции для работы с Орбикрафт. 
-<code python>​magnetometer_turn_on(num)</​code>​ – функция включения магнитометра,​ где num – это номер магнитометра. 
-<code python>​magnetometer_turn_off(num)</​code>​ – функция выключения магнитометра,​ где num – это номер магнитометра. 
-<code python>​magnetometer_request_raw(num)</​code>​ – функция возвращающая сырые данные измеренные магнитометром с номером num, представляющие собой список из 4 числовых значений. 
-<code python>​hyro_turn_on(hyr_num)</​code>​ – функция включения ДУС, где hyr_num – это номер ДУС. 
-<code python>​hyro_turn_off(hyr_num)</​code>​ – функция выключения ДУС, где hyr_num – это номер ДУС. 
-<code python>​hyro_request_raw(hyr_num)</​code>​ – функция возвращающая сырые данные измеренные ДУС с номером hyr_num, представляющие собой список из 4 числовых значений. 
-<code python>​motor_turn_on(num)</​code>​ – функция включения маховика,​ где num – это номер маховика. 
-<code python>​motor_turn_off(num)</​code>​ – функция выключения маховика,​ где num – это номер маховика. 
-<code python>​motor_set_speed(mtr_num,​ speed)</​code>​ – функция устанавливающая скорость вращения маховика с номером mtr_num, в значение speed. 
- 
-===== Тест программы ===== 
-Протестируйте работу программы при следующих значениях целевого угла alpha_goal. 
-0, 90, -90, 180. 
-При повороте на целевой угол в 180 градусов программа будет работать неправильно. 
-Для устранения бага в работе в программу необходимо добавить функцию трансформации угла angle_transformation. 
-Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal. По умолчанию угол определяется в диапазоне от -180 до 180 градусов с помощью функции atan2. 
-Функция angle_transformation изменяет диапазона измерения угла. Для решения задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до (alpha_goal+180),​ где alpha_goal - это целевой угол, между осью x магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость. 
-<code python> 
-def angle_transformation(alpha,​ alpha_goal):​ 
- if alpha<​=(alpha_goal - 180): 
- alpha = alpha + 360 
- elif alpha>​(alpha_goal +180): 
- alpha = alpha - 360 
- return alpha 
-</​code>​ 
-===== Тест скорректированной программы ===== 
-Протестируйте работу программы при следующих значениях целевого угла alpha_goal: 0, 90, -90, 180. 
-Теперь при повороте на целевой угол в 180 градусов программа будет работать правильно. 
- 
- 
- 
  
lesson7.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

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