Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Задача учеников - установить на конструктор ОрбиКрафт две «солнечные панели», и запрограммировать Arduino для поворота «солнечных панелей» к имитатору Солнца. Конструктор ОрбиКрафт также должен быть запрограммирован на поворот в сторону имитатора Солнца и затем стабилизирован на 30 секунд.
«Солнечные панели» могут быть изготовлены из фанеры или пластика. В качестве датчика освещенности используются два фоторезистора. «Солнечные панели» поворачиваются с помощью сервоприводов.
Также на 3D-принтере распечатан корпус для Arduino.
Корпус для Arduino.
Крепление сервопривода.
Крепление «солнечной панели».
Ссылка на архив с 3D-моделями.
Код для Arduino.
#include <Servo.h> // include Servo library Servo vertical1; Servo vertical2;// вертикальный серво int servov = 90; int srv2 = 90; // номера фоторезисторов // name = analogpin; int ldrt = 1; // верхний фоторезистор int ldrd = 0; // нижний фоторезистор int tMax= 0; int tMin = 1000; int dMax= 0; int dMin = 1000; void setup() { Serial.begin(9600); // servo connections // name.attacht(pin); vertical1.attach(3); vertical2.attach(5); } void loop() { int lt = analogRead(ldrt)-200; // верхний фоторезистор int ld = analogRead(ldrd)-400; // нижний фоторезистор if(lt>tMax) tMax=lt; if(lt<tMin) tMin=lt; if(ld>dMax) dMax=ld; if(ld<dMin) dMin=ld; //lt=map(lt, tMin,tMax, 0,100); //ld=map(ld, dMin,dMax, 0,100); //сообщения для дебага Serial.print("lt"); Serial.print(lt); Serial.print("\n"); Serial.print("ld"); Serial.print(ld); Serial.print("\n"); int dtime = 10; //пауза между считываниями int tol = 210; //параметр чувствительности int dvert = lt - ld; // считаем разницу в количестве света между верхом и низом if ((dvert*dvert > tol*tol)) // проверяем есть ли разница в допустимом диапазоне, иначе измените вертикальный угол { if (lt > ld) //определяем вниз или вверх { servov = ++servov;// поворачиваем в сторону пола if (servov > 130) //ограничение если далеко повернулись { servov = 130; } } else // если внизу больше { servov= --servov;// поворачиваем в сторону верха if (servov < 0) //ограничение если далеко повернулись { servov = 0; } } srv2 = 180-servov; vertical1.write(servov); vertical2.write(srv2); // задаем положение соответствующему серво Serial.println(servov); Serial.println(srv2); } delay(dtime); //пауза перед новым замером }
Код для ОрбиКрафта на Python.
import math num = 1 frame = 0 err = 0 omega_goal = 0 mag_alpha = 0 err_transmitter = 0 time_step = 0.05 time_stepgyro = 0.1 omega_goal = 0.0 alpha_goal = 0 mtr_max_speed = 4500 mtr_num = 1 hyr_num = 1 mag_num = 1 sun_max = 25000 sun_min = 50 omega=0 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 kp = -100 kd = -0.001494 error = 0 P = 0 D = 0 I = 0 Kp = 20 Kd = -20 Ki = 0.7 lastError = 0 Integrator = 0 PID_omega = 0 Integrator_max = 10 Integrator_min = -10 def gyrostb(): gx_degs=0 gy_degs=0 gz_degs=0 ok=0 omega=0 hyro_state = 0 omega_goal = 0.0 while ok<15: hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) if not hyro_state: gx_degs = gx_raw * 0.00875 gy_degs = gy_raw * 0.00875 gz_degs = gz_raw * 0.00875 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 gy_degs<5 and gy_degs>-5 and gz_degs<5 and gz_degs>-5: ok=ok+1 print "ok=" , ok if not mtr_state: print "Motor_speed: ", mtr_speed mtr_new_speed = motor_new_speed_PDM(mtr_speed,omega,omega_goal)-200 motor_set_speed(mtr_num, mtr_new_speed) sleep(time_stepgyro) def motor_new_speed_PDM(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 mag_calibrated(magx,magy,magz): magx_cal = 0.341987*(magx + 233.745978) + -0.006580*(magy + 624.402618) + -0.005068*(magz + 60.880340) magy_cal = -0.006580*(magx + 233.745978) + 0.238206*(magy + 624.402618) + -0.003819*(magz + 60.880340) magz_cal = -0.005068*(magx + 233.745978) + -0.003819*(magy + 624.402618) + 0.395613*(magz + 60.880340) return magx_cal, magy_cal, magz_cal def motor_new_speed_PDF(mtr_speed, alpha, alpha_goal, omega, omega_goal): mtr_new_speed = int(mtr_speed + Kd*(alpha-alpha_goal)+Kp*(omega-omega_goal)+100) 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 angle_transformation(alpha, alpha_goal): if alpha<=(alpha_goal - 180): alpha = alpha + 360 elif alpha>(alpha_goal +180): alpha = alpha - 360 return alpha def magstb(): alpha_goal = 90 gx_degs=0 gy_degs=0 gz_degs=0 ok=0 mag_alpha = 0 mtr_new_speed=0 omega=0 while ok<50: #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 gy_degs<3 and gy_degs>-3 and gz_degs<3 and gz_degs>-3: ok=ok+1 #Обработка показаний маховика и установка требуемой угловой скорости. if not mtr_state: # если код ошибки 0, т.е. ошибки нет print "Motor_speed: ", mtr_speed # установка новой скорости маховика mtr_new_speed = motor_new_speed_PDF(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal) motor_set_speed(mtr_num, mtr_new_speed) sleep(time_step) # Функция включает все приборы # которые будут использоваться в основной программе. 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 cam(): camera_turn_on() transmitter_turn_on(num) sleep(1) err_camera = camera_take_photo(frame) if err == 1: print 'Camera access error, check the connection', frame elif err == 2: print 'Camera interface errorr, check the code', frame #Передаем снимок err_transmitter = transmitter_transmit_photo(num, frame) err_camera=0 err_transmitter=0 sleep(1) if err_transmitter == 1: print 'Transmition failed' camera_turn_off() transmitter_turn_off(num) def sun(): ok=0 while ok<5 : #снятие показаний sun_result1 = sun_sensor_request_raw(1) sun_result2 = sun_sensor_request_raw(2) sun_result3 = sun_sensor_request_raw(3) sun_result4 = sun_sensor_request_raw(4) sun1=(sun_result1[1]+sun_result1[2])/2 sun2=(sun_result2[1]+sun_result2[2])/2 sun3=(sun_result3[1]+sun_result3[2])/2 sun4=(sun_result4[1]+sun_result4[2])/2 #max ищем max=1 maxx=sun1 if sun1<sun2: max=2 maxx=sun2 if maxx<sun3: max=3 maxx=sun3 if maxx<sun4: max=4 maxx=sun4 #max нашли print "max=", max,"maxx=", maxx #max вывели if max==1 : motor_set_speed(1, 4000) print "0" ok=ok+1 if max==2 : motor_set_speed(1, 3500) print "+" if max==3 : motor_set_speed(1, 0) print "++" if max==4 : motor_set_speed(1, -3500) print "--" sleep(1) print "grubook" ok=0 while ok<5 : sun_result1 = sun_sensor_request_raw(1) sun_result2 = sun_sensor_request_raw(2) sun_result3 = sun_sensor_request_raw(3) sun_result4 = sun_sensor_request_raw(4) print "1",sun_result1[1],"2",sun_result1[2] if sun_result1[1]>sun_result1[2]: motor_set_speed(1, -3000) print "+" if sun_result1[2]>sun_result1[1]: motor_set_speed(1, 3000) print "-" if (abs(sun_result1[1]-sun_result1[2]))<10: motor_set_speed(1, 0) print "0" ok=ok+1 sleep(1.5) print "gtovo" for i in range(5000) gyrostb() def control(): mtr_max_speed = 4500 initialize_all() mtr_state = 0 hyro_state = 0 mag_state = 0 alpha_goal = 0 omega_goal = 0 mag_alpha = 0 sun_result = [0,0,0,0] sun_result1 = [0,0,0] sun_result2 = [0,0,0] sun_result3 = [0,0,0] sun_result4 = [0,0,0] #cam() while(1): print "mag" magstb(0) print "gyro" gyrostb() print "cam" cam() print "sun" sun() while(1): gyrostb() switch_off_all()