Инструменты пользователя
Инструменты сайта
lesson8
Различия
Здесь показаны различия между двумя версиями данной страницы.
|
lesson8 [2019/05/14 11:30] golikov [Сбор значений для калибровки солнечных датчиков] |
lesson8 [2020/03/25 16:28] |
||
|---|---|---|---|
| Строка 1: | Строка 1: | ||
| - | ====== 08 Урок. Знакомство с солнечными датчиками ====== | ||
| - | ===== Знакомство с датчиком освещенности смартфона ===== | ||
| - | Установите в смартфон приложение Andro sensor или аналогичное приложение с выводом на экран информации со встроенных датчиков смартфона. | ||
| - | Запустите приложение и найдите датчик освещенности под названием LIGHT. | ||
| - | |||
| - | {{::08image001.png?nolink&400|}} | ||
| - | |||
| - | Освещенность изменяется в люксах (lux), один люкс – это равен освещённости поверхности площадью 1 м² при световом потоке падающего на неё излучения, равном 1 люмен. Соответственно, выполнятся: 1 лк = 1 лм/м2. | ||
| - | |||
| - | {{::08image003.png?nolink&200|}} | ||
| - | |||
| - | Один люмен равен световому потоку, испускаемому точечным источником, c силой света, равной одной канделе, в телесный угол величиной в один стерадиан. | ||
| - | |||
| - | {{::08image004.png?nolink&200|}} | ||
| - | |||
| - | Полный световой поток, создаваемый точечным источником, с силой света одна кандела, равен 4π люменам. | ||
| - | Кандела – это сила света, энергетическая сила которого составляет 1/683 Вт/стерадиан. | ||
| - | Сила света, излучаемого парафиновой свечой, близка к одной канделе. | ||
| - | |||
| - | {{::08image006.jpg?nolink&100|}} | ||
| - | |||
| - | Требования к освещению на рабочих местах, оборудованных ПЭВМ согласно СанПиН 2.2.2/2.4.1340-03: | ||
| - | Освещенность на рабочем столе: 300-500 лк | ||
| - | Освещенность на экране ПЭВМ: не выше 300лк | ||
| - | |||
| - | ===== Проверка работоспособности солнечных датчиков ===== | ||
| - | |||
| - | Соедините БКУ с СЭП и четырьмя солнечными датчиками. | ||
| - | Загрузите в БКУ следующую программу, которая выведет значения, считанные с солнечных датчиков. | ||
| - | |||
| - | Код на Python. | ||
| - | <file python sun_test.py> | ||
| - | |||
| - | def control(): # Основная функция программы, в которой нужно вызывать остальные функции | ||
| - | sun_result = [0,0,0] # Инициализируем sun_result | ||
| - | num = 1 | ||
| - | print "Enable sun sensor №", num | ||
| - | sun_sensor_turn_on(num) | ||
| - | sleep(1) | ||
| - | print "Get RAW data from sun sensor" | ||
| - | |||
| - | for i in range(10): | ||
| - | sun_result = sun_sensor_request_raw(num) | ||
| - | |||
| - | if not sun_result[0]: # если датчик вернул сообщение об ошибке, | ||
| - | |||
| - | print "state:", sun_result[0], "raw =", sun_result[1], \ | ||
| - | sun_result[2] | ||
| - | |||
| - | elif sun_result[0] == 1: | ||
| - | print "Fail because of access error, check the connection" | ||
| - | |||
| - | elif sun_result[0] == 2: | ||
| - | print "Fail because of interface error, check your code" | ||
| - | |||
| - | sleep(1) | ||
| - | |||
| - | print "Disable sun sensor №", num | ||
| - | sun_sensor_turn_off(num) | ||
| - | </file> | ||
| - | |||
| - | Код на С. | ||
| - | <file c sun_test.c> | ||
| - | #include <stdio.h> | ||
| - | #include <stdint.h> | ||
| - | #include "libschsat.h" | ||
| - | #define LSS_OK 0 | ||
| - | #define LSS_ERROR 1 | ||
| - | #define LSS_BREAK 2 | ||
| - | |||
| - | int control(){ //Основная функция программы | ||
| - | uint16_t sun_result[] = {0, 0, 0}; // инициализируем sun_result | ||
| - | uint16_t num = 1; // номер солнечного датчика | ||
| - | printf("Enable sun sensor №%d\n", num); | ||
| - | sun_sensor_turn_on(num); //включаем датчик | ||
| - | Sleep(1); //Ждем включения 1 секунду | ||
| - | printf("Get RAW data from sun sensor №%d\n", num); | ||
| - | int i; | ||
| - | for (i = 0; i < 10; i++) //считываем показаия 10 раз | ||
| - | { | ||
| - | sun_result[0] = sun_sensor_request_raw(num,& sun_result[1],& sun_result[2]);/*проверить как работает, очень странно, что работает | ||
| - | если не работает задать sun_result[0] */ | ||
| - | if (!sun_result[0]){ //если датчик не вернул сообщение об ошибке, | ||
| - | printf("state: %d raw = %d, %d\n", i, sun_result[1], sun_result[2]); | ||
| - | } | ||
| - | |||
| - | else if (sun_result[0] == 1) { //если датчик вернул сообщение об ошибке 1 | ||
| - | printf("Fail because of access error, check the connection\n"); | ||
| - | } | ||
| - | else if (sun_result[0] == 2) { //если датчик вернул сообщение об ошибке 2 | ||
| - | printf("Fail because of interface error, check you code\n"); | ||
| - | } | ||
| - | Sleep(1); //показания считываются раз в секунду | ||
| - | |||
| - | } | ||
| - | printf("Disable sun sensor №%d\n", num); | ||
| - | sun_sensor_turn_off(num); //выключаем солнечный датчик | ||
| - | return 0; | ||
| - | } | ||
| - | </file> | ||
| - | |||
| - | Запустите программу, при комнатном освещении значения датчиков будут находится в диапазоне от 70 до 300. | ||
| - | Направьте на датчики свет от имитатора солнца – теперь значения будут изменяться от 70 до 20000. | ||
| - | |||
| - | ===== Сбор значений для калибровки солнечных датчиков ===== | ||
| - | Установите на верхнюю панель Орбикрафт ДУС и магнитометр, и 4 солнечных датчика на 4 стороны Орбикрафта. Солнечные датчики установите перевернутыми, чтобы шлейфы не закрывали окно датчика. | ||
| - | |||
| - | {{::08image008.png?nolink&200|}} | ||
| - | |||
| - | Загрузите в БКУ следующую программу. | ||
| - | |||
| - | Код на Python. | ||
| - | <file python sun_raw.py> | ||
| - | |||
| - | import time | ||
| - | import math | ||
| - | #!/usr/bin/env python | ||
| - | # -*- coding: utf-8 -*- | ||
| - | # Коэффициент дифференциальной обратной связи. | ||
| - | # Коэффициент подбирается экспериментально в зависимости от формы | ||
| - | # и массы вашего спутника. | ||
| - | kd = 200.0 | ||
| - | # Временной шаг работы алгоритма, с | ||
| - | time_step = 0.05 | ||
| - | # Целевая угловая скорость спутника, град/с. | ||
| - | # Для режима стабилизации равна 0.0. | ||
| - | omega_goal = 0.0 | ||
| - | # Максимально допустимая скорость маховика, об/мин | ||
| - | mtr_max_speed = 5000 | ||
| - | # Номер маховика | ||
| - | mtr_num = 1 | ||
| - | # Номер ДУС (датчика угловой скорости) | ||
| - | hyr_num = 1 | ||
| - | # Номер магнитометра | ||
| - | mag_num = 1 | ||
| - | # Номер результата измерений | ||
| - | i = 1 | ||
| - | # Угол поворота текущий | ||
| - | alpha = 0.0 | ||
| - | |||
| - | # Функция включает все приборы, | ||
| - | # которые будут использоваться в основной программе. | ||
| - | 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) # Ждем включения 1 секунду | ||
| - | 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) | ||
| - | |||
| - | # Функция отключает все приборы, | ||
| - | # которые будут использоваться в основной программе. | ||
| - | 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): | ||
| - | # вместо этих 3-х строк кода с коэффициентами калибровки, должны быть строки с коэффициентами калибровки для Вашего магнитометра | ||
| - | #magx_cal = 1.04*magx - 0.26*magy + 0.05*magz - 68.76 # это 1-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра | ||
| - | #magy_cal = 0.24*magx + 1.04*magy + 0.29*magz + 256.92 # это 2-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра | ||
| - | #magz_cal = -0.09*magx - 0.19*magy + 0.77*magz + 159.41 # это 3-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра | ||
| - | 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 | ||
| - | |||
| - | # Функции для определение новой скорости маховика. | ||
| - | # Новая скорость маховика складывается из | ||
| - | # текущей скорости маховика и приращения скорости. | ||
| - | # Приращение скорости пропорционально ошибке по углу | ||
| - | # и ошибке по угловой скорости. | ||
| - | # mtr_speed - текущая угловая скорость маховика, об/мин | ||
| - | # omega - текущая угловая скорость спутника, град/с | ||
| - | # omega_goal - целевая угловая скорость спутника, град/с | ||
| - | # mtr_new_speed - требуемая угловая скорость маховика, об/мин | ||
| - | 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 | ||
| - | |||
| - | # Основная функция программы, в которой вызываются остальные функции. | ||
| - | def control(): | ||
| - | omega_goal = 0 # omega_goal - целевая угловая скорость спутника, град/с | ||
| - | initialize_all() | ||
| - | # Инициализируем статус маховика | ||
| - | mtr_state = 0 | ||
| - | # Инициализируем статус ДУС | ||
| - | hyro_state = 0 | ||
| - | sun_sensor_num = 0 # Инициализируем переменную для номера солнечного датчика | ||
| - | sun_result_1 = [0,0,0] # Инициализируем sun_result_1 | ||
| - | sun_result_2 = [0,0,0] # Инициализируем sun_result_2 | ||
| - | sun_result_3 = [0,0,0] # Инициализируем sun_result_3 | ||
| - | sun_result_4 = [0,0,0] # Инициализируем 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] | ||
| - | # Номер результата измерений | ||
| - | i = 0 | ||
| - | # Запоминаем время начала вращения | ||
| - | time_start = time.time() | ||
| - | # Запоминаем время начала секундного интервала | ||
| - | time_interval = time.time() | ||
| - | # Интервал вывода данных с солнечных датчиков в секундах | ||
| - | time_output = 0.1 | ||
| - | |||
| - | while True: | ||
| - | |||
| - | # Опрос датчика угловой скорости и маховика. | ||
| - | 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) | ||
| - | |||
| - | # Обработка показаний датчика угловой скорости, | ||
| - | # вычисление угловой скорости спутника по показаниям ДУС. | ||
| - | # Если код ошибки ДУС равен 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 | ||
| - | 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, т.е. ошибки нет | ||
| - | # установка новой скорости маховика | ||
| - | 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: # если магнитометр вернул код ошибки 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 | ||
| - | |||
| - | if (time.time() - time_interval) > time_output: | ||
| - | # Запоминаем время начала следующего секундного интервала | ||
| - | 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: # Начинаем вращение через 5с после запуска | ||
| - | omega_goal = 6.0 # omega_goal - целевая угловая скорость спутника, град/с | ||
| - | |||
| - | 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] | ||
| - | |||
| - | </file> | ||
| - | |||
| - | Код на С. | ||
| - | <file c sun_raw.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> | ||
| - | #include <time.h> | ||
| - | |||
| - | /*Коэффициент дифференциальной обратной связи. | ||
| - | Коэффициент положительный, если маховик расположен осью z вверх | ||
| - | и ДУС расположен осью z также вверх. | ||
| - | Коэффициент подбирается экспериментально в зависимости от формы | ||
| - | и массы вашего спутника.*/ | ||
| - | const float kd = 200.0; | ||
| - | |||
| - | // Временной шаг работы алгоритма, с | ||
| - | const float time_step = 0.1; | ||
| - | |||
| - | /* Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0.*/ | ||
| - | const float omega_goal = 0.0; | ||
| - | // Максимально допустимая скорость маховика, об/мин | ||
| - | const int mtr_max_speed = 5000; | ||
| - | const uint16_t mtr_num = 1; // Номер маховика | ||
| - | const uint16_t hyr_num = 1; // Номер ДУС | ||
| - | const uint16_t mag_num = 1; // Номер магнитометра | ||
| - | // Номер результата измерений | ||
| - | int i = 1; | ||
| - | // Угол поворота текущий | ||
| - | const float alpha = 0.0; | ||
| - | |||
| - | |||
| - | |||
| - | void initialize_all(void){/* Функция включает все приборы, | ||
| - | которые будут использоваться в основной программе.*/ | ||
| - | 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); // Ждем включения 1 секунду | ||
| - | printf("Enable Sun sensors 1-4\n"); | ||
| - | sun_sensor_turn_on(1); | ||
| - | sun_sensor_turn_on(2); | ||
| - | sun_sensor_turn_on(3); | ||
| - | sun_sensor_turn_on(4); | ||
| - | Sleep(1); | ||
| - | printf("Enable motor №%d\n", mtr_num); | ||
| - | motor_turn_on(mtr_num); | ||
| - | Sleep(1); | ||
| - | } | ||
| - | |||
| - | void switch_off_all(void){/* Функция отключает все приборы, | ||
| - | которые будут использоваться в основной программе.*/ | ||
| - | printf("Finishing..."); | ||
| - | int16_t new_speed = 0; | ||
| - | 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, &new_speed); | ||
| - | Sleep(1); | ||
| - | motor_turn_off(mtr_num); | ||
| - | printf("\nFinish program\n"); | ||
| - | } | ||
| - | |||
| - | int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ | ||
| - | /*Функция mag_calibrated вносит поправки | ||
| - | в показания магнитометра с учетом калибровочных коэффициентов | ||
| - | вместо этих 3-х строк кода с коэффициентами калибровки, должны быть строки с коэффициентами калибровки для Вашего магнитометра | ||
| - | //magx_cal = 1.04*magx - 0.26*magy + 0.05*magz - 68.76 # это 1-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра | ||
| - | //magy_cal = 0.24*magx + 1.04*magy + 0.29*magz + 256.92 # это 2-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра | ||
| - | //magz_cal = -0.09*magx - 0.19*magy + 0.77*magz + 159.41 # это 3-я строчка, которую нужно заменить по результатам калибровки Вашего магнитометра*/ | ||
| - | float magx_cal; | ||
| - | float magy_cal; | ||
| - | float magz_cal; | ||
| - | 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); | ||
| - | *magx = magx_cal; | ||
| - | *magy = magy_cal; | ||
| - | *magz = magz_cal; | ||
| - | return 0; | ||
| - | } | ||
| - | |||
| - | int motor_new_speed_PD(int mtr_speed, float omega, int16_t omega_goal){ | ||
| - | /* Функция для определения новой скорости маховика. | ||
| - | Новая скорость маховика складывается из | ||
| - | текущей скорости маховика и приращения скорости. | ||
| - | Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. | ||
| - | mtr_speed - текущая угловая скорость маховика, об/мин | ||
| - | omega - текущая угловая скорость спутника, град/с | ||
| - | omega_goal - целевая угловая скорость спутника, град/с | ||
| - | mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ | ||
| - | int16_t 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; | ||
| - | } | ||
| - | |||
| - | int control(){// Основная функция программы, в которой вызываются остальные функции. | ||
| - | int16_t omega; | ||
| - | int omega_goal = 0; // omega_goal - целевая угловая скорость спутника, град/с | ||
| - | initialize_all(); | ||
| - | int mtr_state = 0; // Инициализируем статус маховика | ||
| - | int hyro_state = 0; // Инициализируем статус ДУС | ||
| - | int mag_state = 0; // Инициализируем статус магнитометра | ||
| - | int16_t mtr_speed; | ||
| - | int16_t mtr_new_speed; | ||
| - | //данные ДУС | ||
| - | 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 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; | ||
| - | |||
| - | float gx_degs; | ||
| - | float gy_degs; | ||
| - | float gz_degs; | ||
| - | uint16_t sun_result_1[] = {0,0,0}; // Инициализируем sun_result_1 | ||
| - | uint16_t sun_result_2[] = {0,0,0}; // Инициализируем sun_result_2 | ||
| - | uint16_t sun_result_3[] = {0,0,0}; // Инициализируем sun_result_3 | ||
| - | uint16_t sun_result_4[] = {0,0,0}; // Инициализируем sun_result_4 | ||
| - | int mag_alpha = 0; | ||
| - | const int sizeOD = 10; | ||
| - | int sizeODA = 0; | ||
| - | int tempSODA; | ||
| - | // double* output_data = (double*)calloc(sizeOD, sizeof(double)); | ||
| - | double* output_data_all = (double*)calloc(sizeODA, sizeof(double)); | ||
| - | |||
| - | // Номер результата измерений | ||
| - | int i = 0; | ||
| - | // Запоминаем время начала вращения | ||
| - | long int time_start = time(NULL); | ||
| - | // Запоминаем время начала секундного интервала | ||
| - | long int time_interval = time(NULL); | ||
| - | // Интервал вывода данных с солнечных датчиков в секундах | ||
| - | int time_output = 0.1; | ||
| - | int j; | ||
| - | char a=1; | ||
| - | |||
| - | while (a==1){ | ||
| - | // Опрос датчика угловой скорости и маховика. | ||
| - | hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); | ||
| - | mtr_state = motor_request_speed(mtr_num, &mtr_speed); | ||
| - | mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw); | ||
| - | |||
| - | |||
| - | if (!hyro_state){ | ||
| - | /*Обработка показаний датчика угловой скорости, | ||
| - | вычисление угловой скорости спутника по показаниям ДУС. | ||
| - | Если код ошибки ДУС равен 0, т.е. ошибки нет*/ | ||
| - | 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; | ||
| - | // printf("gx_degs=%f, gy_degs=%f, gz_degs=%f\n", gx_degs, gy_degs, 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("Motor_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); | ||
| - | long int time_current = time(NULL) - time_start; | ||
| - | |||
| - | 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; | ||
| - | } | ||
| - | |||
| - | if ((time(NULL) - time_interval) > time_output){ | ||
| - | // Запоминаем время начала следующего секундного интервала | ||
| - | time_interval = time(NULL); | ||
| - | 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_3[0] = sun_sensor_request_raw(3,&sun_result_3[1],&sun_result_3[2]); | ||
| - | sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]); | ||
| - | |||
| - | int 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}; | ||
| - | tempSODA = sizeODA; | ||
| - | sizeODA += sizeOD; | ||
| - | output_data_all = (double*)realloc(output_data_all, sizeODA*sizeof(double)); | ||
| - | for (j=tempSODA; j<sizeODA; j++) { | ||
| - | output_data_all[j] = output_data[j-sizeODA+1]; | ||
| - | } | ||
| - | } | ||
| - | if (i > 100){ // Начинаем вращение через 5с после запуска | ||
| - | omega_goal = 6.0; // omega_goal - целевая угловая скорость спутника, град/с | ||
| - | } | ||
| - | |||
| - | if (time_current > 90){ | ||
| - | break; | ||
| - | } | ||
| - | i += 1; | ||
| - | } | ||
| - | |||
| - | switch_off_all(); | ||
| - | |||
| - | printf("time_end = %ld" , time(NULL) - time_start); | ||
| - | |||
| - | for (i = 0; i < 5000; i = i + 10){ | ||
| - | printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",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], output_data_all[i+9]); | ||
| - | } | ||
| - | printf ("Ok\n"); | ||
| - | return 0; | ||
| - | } | ||
| - | </file> | ||
| - | |||
| - | |||
| - | По результатам работы программы будут выведены 500 строк с данными следующего вида: | ||
| - | <code python> | ||
| - | onmessage0.132400989532 74 117 27 25 156 214 156 61 -34.6566118491 | ||
| - | onmessage0.281419992447 74 116 27 25 156 215 156 61 -34.555539497 | ||
| - | onmessage0.438189029694 74 116 27 25 156 214 156 61 -33.6820711721 | ||
| - | onmessage0.585952043533 74 116 27 25 156 215 156 61 -34.6566118491 | ||
| - | onmessage0.733724832535 74 116 27 25 156 214 156 61 -33.6705914701 | ||
| - | onmessage0.88149189949 74 117 27 25 156 214 156 61 -33.7733709383 | ||
| - | onmessage1.02926301956 74 117 27 25 156 214 156 61 -33.6745869595 | ||
| - | onmessage1.1782848835 74 117 27 25 156 214 156 61 -33.4062495287 | ||
| - | onmessage1.32730197906 74 117 27 25 156 214 156 61 -33.2488503276 | ||
| - | onmessage1.47507381439 74 117 27 25 156 214 156 61 -33.8923918648 | ||
| - | </code> | ||
| - | |||
| - | Где первое значение - время с начала измерений, два следующих – данные с первого солнечного датчика, следующие два значения со второго солнечного датчика, следующие два с третьего и с четвертого. Десятое значение – показания магнетометра (угол поворота Орбикрафт относительно направления на магнитный полюс). | ||
| - | Для анализа полученных данных следует скопировать их из браузера (выбрав с помощью Ctrl-A и скопировав с помощью Ctrl-С) и сохранить в новом текстовом документе в Notepad++ (вставка с помощью (Ctrl-V). Затем следует очистить их от служебной информации в начале и в конце файла. | ||
| - | Часто встречающееся служебное слово onmessage следует удалить с помощью функции замены Notepad++. Нажмите на клавиатуре Ctrl-H, введите в поле «Найти» onmessage, поле «Заменить на» оставьте пустым и нажмите на «Заменить все» или «Заменить во всех открытых документах». | ||
| - | |||
| - | {{::08image009.png?nolink&600|}} | ||
| - | |||
| - | Сохраните очищенный документ в txt файле. Теперь его можно проанализировать в Excel. | ||
| - | |||
lesson8.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)
Инструменты страницы
Если не указано иное, содержимое этой вики предоставляется на условиях следующей лицензии: CC Attribution-Noncommercial-Share Alike 4.0 International
