Здесь показаны различия между двумя версиями данной страницы.
lesson7 [2019/05/14 11:42] 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 | ||
- | |||
- | kd = 200 # К-т дифференциальной обратной связи. Если угловая скорость спутника положительна, то спутник надо раскручивать по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке. | ||
- | kp = -50 # К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки. | ||
- | time_step = 0.05 # Временной шаг работы | ||
- | |||
- | mtr_num = 1 # Номер маховика | ||
- | hyr_num = 1 # Номер ДУС | ||
- | mag_num = 1 # Номер магнитометра | ||
- | |||
- | # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | ||
- | 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 определяет новую скорость маховика | ||
- | # Новая скорость маховика равна текущей скорости маховика + приращение скорости | ||
- | # Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости | ||
- | 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(): # Функция инициализации всех систем | ||
- | 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 = 0 # Целевой угол | ||
- | omega_goal = 0 # Целевая угловая скорость | ||
- | mag_alpha = 0 | ||
- | |||
- | for i in range(500): | ||
- | # опрос датчиков и маховика | ||
- | 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 | ||
- | 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" | ||
- | |||
- | if not hyro_state: # если ДУС вернул код ошибки 0, т.е. ошибки нет | ||
- | gx_degs = gx_raw * 0.00875 | ||
- | gy_degs = gy_raw * 0.00875 | ||
- | gz_degs = gz_raw * 0.00875 | ||
- | omega = gz_degs # если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z | ||
- | print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные | ||
- | elif hyro_state == 1: # если датчик вернул сообщение об ошибке 1 | ||
- | print "Fail because of access error, check the connection" | ||
- | elif hyro_state == 2: # если датчик вернул сообщение об ошибке 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,gz_degs,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 градусов программа будет работать правильно. | ||
- | |||
- | |||
- | |||