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

Перевод этой страницы:

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


Боковая панель

Для чего нужен ОрбиКрафт

Подсистемы конструктора

Инструкции по работе с ОрбиКрафт

Уроки

Лабораторная оснастка

Знакомство с Arduino

Полезная нагрузка на базе Arduino

Обратная связь

Новости

arduino_02

Управление солнечными панелями

Задача учеников - установить на конструктор ОрбиКрафт две «солнечные панели», и запрограммировать 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()
arduino_02.txt · Последние изменения: 2021/09/06 14:16 — golikov

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