Инструменты пользователя
Инструменты сайта
lesson10
Различия
Здесь показаны различия между двумя версиями данной страницы.
| Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия | ||
|
lesson10 [2019/11/07 14:03] golikov |
lesson10 [2019/11/07 15:41] golikov |
||
|---|---|---|---|
| Строка 1: | Строка 1: | ||
| - | ====== 10 Урок Ориентация Орбикрафт по солнечным датчикам ====== | + | ====== OrbiCraft positioning with solar sensors ====== |
| - | ===== Загрузка программы ===== | + | ===== Program loading ===== |
| - | Откройте файл sun_orient.py и вставьте в массив data_sun = [] все данные из текстового файла с данными, и удалите в конце массива пробел и запятую. | + | Open the file SUN.py and enter to the array data_sun = [] all data from the data text file; delete the space and comma characters at the end of array. |
| {{::10image001.png?nolink&200|}} | {{::10image001.png?nolink&200|}} | ||
| - | Код на Python. | + | Python code. |
| <file python sun_orient.py> | <file python sun_orient.py> | ||
| Строка 189: | Строка 189: | ||
| const int 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).*/ | const int 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).*/ | ||
| const float time_step = 0.05; //working time-step for the positioning and stabilization system | const float time_step = 0.05; //working time-step for the positioning and stabilization system | ||
| - | const int mtr_max_speed = 3000; // Максимально допустимая скорость маховика, об/мин | + | const int mtr_max_speed = 3000; // Maximum allowed flywheel speed, rev/min |
| - | const uint16_t mtr_num = 1; // Номер маховика | + | const uint16_t mtr_num = 1; // flywheel number |
| - | const uint16_t hyr_num = 1; // Номер ДУС | + | const uint16_t hyr_num = 1; // angular velocity sensor number |
| - | const uint16_t mag_num = 1; // Номер магнитометра | + | const uint16_t mag_num = 1; // magnetometer number |
| - | const int sun_max = 25000;//Максимальное значение солнечного датчика больше которого считаем ошибкой | + | const int sun_max = 25000;//Maximum value of the solar sensor (higher values are deemed as errors) |
| - | const int sun_min = 50;//Минимальное значение солнечного датчика меньше которого считаем ошибкой | + | const int sun_min = 50;//Minimum value of the solar sensor (lower values are deemed as errors) |
| int16_t mtr_new_speed; | int16_t mtr_new_speed; | ||
| int angle_transformation(int alpha, int alpha_goal){ | int angle_transformation(int alpha, int alpha_goal){ | ||
| - | /* Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal | + | /* 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 |
| - | // По умолчанию угол определяется в диапазоне от -180 до 180 градусов | + | 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. |
| - | // Функция angle_transformation изменяет диапазона измерения угла. Для решения | + | */ |
| - | задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до | + | |
| - | (alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра | + | |
| - | и проекцией вектора магнитного поля на горизонтальную плоскость.*/ | + | |
| if (alpha <= (alpha_goal - 180)){ | if (alpha <= (alpha_goal - 180)){ | ||
| alpha = alpha + 360; | alpha = alpha + 360; | ||
| Строка 215: | Строка 212: | ||
| int motor_new_speed_PD(int mtr_speed,int alpha,int alpha_goal, float omega, int omega_goal){ | int motor_new_speed_PD(int mtr_speed,int alpha,int alpha_goal, float omega, int omega_goal){ | ||
| - | /* Определение новой скорости маховика | + | /* 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 |
| + | */ | ||
| int mtr_new_speed = mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal); | int mtr_new_speed = mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal); | ||
| if (mtr_new_speed > mtr_max_speed){ | if (mtr_new_speed > mtr_max_speed){ | ||
| Строка 228: | Строка 226: | ||
| } | } | ||
| - | void initialize_all(void){/* Функция включает все приборы, | + | void initialize_all(void){/* All systems’ initialization function*/ |
| - | которые будут использоваться в основной программе.*/ | + | |
| printf("Enable motor №%d\n", mtr_num); | printf("Enable motor №%d\n", mtr_num); | ||
| motor_turn_on(mtr_num); | motor_turn_on(mtr_num); | ||
| Строка 244: | Строка 241: | ||
| } | } | ||
| - | void switch_off_all(void){/* Функция отключает все приборы, | + | void switch_off_all(void){/* All systems’ switching off function*/ |
| - | которые будут использоваться в основной программе.*/ | + | |
| printf("Finishing...\n"); | printf("Finishing...\n"); | ||
| int16_t new_speed = mtr_new_speed; | int16_t new_speed = mtr_new_speed; | ||
| Строка 272: | Строка 268: | ||
| int delta_min = 35000; | int delta_min = 35000; | ||
| int delta_i = 0; | int delta_i = 0; | ||
| - | float data_sun[] = {-47.329501295977, 239, 102, -48.1458278265892}; //вставить данные из текстового файла с данными | + | float data_sun[] = {-47.329501295977, 239, 102, -48.1458278265892}; //insert data from text data file |
| int i; | int i; | ||
| for (i = 0; i < 4; i++){ | for (i = 0; i < 4; i++){ | ||
| Строка 315: | Строка 311: | ||
| } | } | ||
| - | int control(){ // основная функция программы | + | int control(){ // Program’s main function |
| - | initialize_all(); // Инициализируем статус маховика | + | initialize_all(); // Initialize flywheel status |
| - | int mtr_state = 0; // Инициализируем статус ДУС | + | int mtr_state = 0; // Initialize angular velocity sensor status |
| int hyro_state = 0; | int hyro_state = 0; | ||
| - | //int mag_state = 0; //Инициализируем статус магнитометра | + | //int mag_state = 0; // Initialize magnetometer status |
| - | uint16_t sun_result_1[] = {0,0,0}; // Инициализируем sun_result_1 | + | uint16_t sun_result_1[] = {0,0,0}; // Initialize sun_result_1 |
| - | uint16_t sun_result_2[] = {0,0,0}; // Инициализируем sun_result_2 | + | uint16_t sun_result_2[] = {0,0,0}; // Initialize sun_result_2 |
| - | uint16_t sun_result_3[] = {0,0,0}; // Инициализируем sun_result_3 | + | uint16_t sun_result_3[] = {0,0,0}; // Initialize sun_result_3 |
| - | uint16_t sun_result_4[] = {0,0,0}; // Инициализируем sun_result_4 | + | uint16_t sun_result_4[] = {0,0,0}; // Initialize sun_result_4 |
| - | uint16_t sun_result_1_Old[] = {0,0,0}; // Инициализируем sun_result_1 | + | uint16_t sun_result_1_Old[] = {0,0,0}; // Initialize sun_result_1_Old |
| - | uint16_t sun_result_2_Old[] = {0,0,0}; // Инициализируем sun_result_2 | + | uint16_t sun_result_2_Old[] = {0,0,0}; // Initialize sun_result_2_Old |
| - | uint16_t sun_result_3_Old[] = {0,0,0}; // Инициализируем sun_result_3 | + | uint16_t sun_result_3_Old[] = {0,0,0}; // Initialize sun_result_3_Old |
| - | uint16_t sun_result_4_Old[] = {0,0,0}; // Инициализируем sun_result_4 | + | uint16_t sun_result_4_Old[] = {0,0,0}; // Initialize sun_result_4_Old |
| int16_t mtr_speed; | int16_t mtr_speed; | ||
| - | //данные ДУС | + | //angular velocity sensor data |
| int16_t gx_raw; | int16_t gx_raw; | ||
| int16_t gy_raw; | int16_t gy_raw; | ||
| Строка 336: | Строка 332: | ||
| int16_t *hyroy_raw = &gy_raw; | int16_t *hyroy_raw = &gy_raw; | ||
| int16_t *hyroz_raw = &gz_raw; | int16_t *hyroz_raw = &gz_raw; | ||
| - | //данные магнитометра | + | //magnetometer data |
| /*int16_t mgx; | /*int16_t mgx; | ||
| int16_t mgy; | int16_t mgy; | ||
| Строка 345: | Строка 341: | ||
| int16_t omega; | int16_t omega; | ||
| - | int omega_goal = 0; // omega_goal - целевая угловая скорость спутника, град/с | + | int omega_goal = 0; // omega_goal - Target angular velocity |
| - | int alpha_goal = 30; // Целевой угол поворота | + | int alpha_goal = 30; // Target turning angle |
| int i; | int i; | ||
| int j; | int j; | ||
| for (i = 0; i < 300; i++){ | for (i = 0; i < 300; i++){ | ||
| - | // Запоминаем старые значения | + | // Put into memory the old values |
| for(j = 0; j < 3; j++){ | for(j = 0; j < 3; j++){ | ||
| sun_result_1_Old[j] = sun_result_1[j]; | sun_result_1_Old[j] = sun_result_1[j]; | ||
| Строка 357: | Строка 353: | ||
| sun_result_4_Old[j] = sun_result_4[j]; | sun_result_4_Old[j] = sun_result_4[j]; | ||
| } | } | ||
| - | // опрос датчиков и маховика | + | // sensors and flywheel data request |
| sun_result_1[0] = sun_sensor_request_raw(1,&sun_result_1[1],&sun_result_1[2]); | sun_result_1[0] = sun_sensor_request_raw(1,&sun_result_1[1],&sun_result_1[2]); | ||
| sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]); | sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]); | ||
| Строка 363: | Строка 359: | ||
| sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]); | sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]); | ||
| //print sun_result_1, sun_result_2, sun_result_3, sun_result_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[0] = sun_range( & sun_result_1[1], & sun_result_1[2], sun_result_1_Old[1], sun_result_1_Old[2]); | sun_result_1[0] = sun_range( & sun_result_1[1], & sun_result_1[2], sun_result_1_Old[1], sun_result_1_Old[2]); | ||
| sun_result_2[0] = sun_range( & sun_result_2[1], & sun_result_2[2], sun_result_2_Old[1], sun_result_2_Old[2]); | sun_result_2[0] = sun_range( & sun_result_2[1], & sun_result_2[2], sun_result_2_Old[1], sun_result_2_Old[2]); | ||
| Строка 381: | Строка 377: | ||
| - | if (!hyro_state){ // если ДУС вернул код ошибки 0, т.е. ошибки нет | + | if (!hyro_state){ // if angular velocity sensor returned error code 0 (no error) |
| float gx_degs = gx_raw * 0.00875; | float gx_degs = gx_raw * 0.00875; | ||
| float gy_degs = gy_raw * 0.00875; | float gy_degs = gy_raw * 0.00875; | ||
| float gz_degs = gz_raw * 0.00875; | float gz_degs = gz_raw * 0.00875; | ||
| - | omega = (int16_t)gz_degs; // если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z | + | omega = (int16_t)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 |
| - | printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Выводим данные | + | printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Data output |
| } | } | ||
| - | else if (hyro_state == 1){ // если датчик вернул сообщение об ошибке 1 | + | else if (hyro_state == 1){ // if the sensor returned error code 1 |
| printf("Fail because of access error, check the connection"); | printf("Fail because of access error, check the connection"); | ||
| } | } | ||
| - | else if (hyro_state == 2){ // если датчик вернул сообщение об ошибке 2 | + | else if (hyro_state == 2){ // if the sensor returned error code 2 |
| printf("Fail because of interface error, check your code"); | printf("Fail because of interface error, check your code"); | ||
| } | } | ||
| - | if (!mtr_state) { // если код ошибки 0, т.е. ошибки нет | + | if (!mtr_state) { // if the flywheel returned error code 0 (no error) |
| int16_t mtr_speed = 0; | int16_t mtr_speed = 0; | ||
| motor_request_speed(mtr_num, &mtr_speed); | motor_request_speed(mtr_num, &mtr_speed); | ||
| printf("Motor_speed: %d\n", mtr_speed); | printf("Motor_speed: %d\n", mtr_speed); | ||
| - | // установка новой скорости маховика | + | // set the flywheel’s new speed |
| mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun, alpha_goal,omega,omega_goal ); | mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun, alpha_goal,omega,omega_goal ); | ||
| motor_set_speed(mtr_num, mtr_new_speed, &omega); | motor_set_speed(mtr_num, mtr_new_speed, &omega); | ||
| Строка 414: | Строка 410: | ||
| - | ===== Принцип работы программы ===== | + | ===== Program’s method of procedure is: ===== |
| + | |||
| + | At the large data array there stored the values measured during solar data calibration and angle values. check_sun function receives 8 values read from solar sensors (two from each sensor), calculates 8 deviation values delta1…delta8 as absolute value of remainder between measured values and values from the table. Then there calculated total deviation delta_sum. The program processes all values from the array searching the minimum deviation delta_sum with corresponding angle value. As a result the check_sun function returns angle value at which ОrbiCraft turned. | ||
| + | sun_range function rejects erroneous values received from solar sensor; if the measured value is higher than 25000 or lower than 50 (higher than sun_max or lower than sun_min), than the program will return the old value from allowed range. The use of this function allows to amend the measurement errors that arises sometimes. | ||
| + | The program’s main function control reads 300 times in cycle the readings of solar sensors checking that the values are not out of range via sun_range function. The correct values are transferred to the check_sun function that returns the angle value at which ОrbiCraft turned. | ||
| + | Than via angle_transformation function there performed the transformation of the angle value to the range of -180…180, and via motor_new_speed_PD function there are calculated the new speed which is required for flywheels rotation. | ||
| + | Than program operation is paused for the time_step time. | ||
| - | В огромном массиве с данными хранятся значения, измеренные при калибровке солнечных датчиков и значения углов. Функция check_sun получает 8 значений, считанных с солнечных датчиков (по два с каждого), и вычисляет 8 значений отклонений delta1…delta8, как модуль разности между измеренными значениями и значениями из таблицы. Затем вычисляется суммарное отклонение delta_sum. Программа проходит по всему массиву значений и ищет минимальное отклонение delta_sum и соответствующее значение угла. В результате функция check_sun возвращает значение угла, на который повернут Орбикрафт. | ||
| - | Функция sun_range отбраковывает ошибочные значения, полученные от солнечного датчика, и если измеренное значение больше 25000 или меньше 50 (больше sun_max или меньше sun_min), то она вернет старое значение из нормального диапазона. Применение этой функции позволяет избавиться от ошибок измерения, которые иногда возникают. | ||
| - | Основная функция программы control 300 раз в цикле считывает значения с солнечных датчиков и проверяет чтобы значения не выходили за пределы диапазона с помощью функции sun_range. Корректные значения передаются в функцию check_sun, которая возвращает значение угла, на который повернут Орбикрафт. | ||
| - | Затем с помощью функции angle_transformation происходит трансформация угла к диапазону -180…180, и с помощью функции motor_new_speed_PD вычисляется новая скорость с которой необходимо вращаться маховику. | ||
| - | Затем работа программы приостанавливается на время time_step. | ||
lesson10.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)
Инструменты страницы
Если не указано иное, содержимое этой вики предоставляется на условиях следующей лицензии: CC Attribution-Noncommercial-Share Alike 4.0 International
