Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Ardu remote: очень простая и дешёвая аппаратура управления своими руками

Добавляем радиоуправление

Управлять будем с телефона под android.

При запуске приложения попросим пользователя выбрать к кому подключаться, bt-модуль должен быть уже сопряжен с телефоном (стандартный код 1234).

BluetoothAdapter bluetooth;
String []boundedItems;
protected static final int RECIEVE_MESSAGE = 1;

@Override
protected void onCreate(Bundle savedInstanceState) {
		//...
		bluetooth = BluetoothAdapter.getDefaultAdapter();		
		if(bluetooth != null){
			if (!bluetooth.isEnabled()) {
			    bluetooth.enable();
			}
		}
		Set<BluetoothDevice> bounded=bluetooth.getBondedDevices();
		boundedItems=new String[bounded.size()];
		int i=0;
		for (BluetoothDevice bluetoothDevice : bounded) {
			boundedItems[i  ]=bluetoothDevice.getName();
		}
		
		showListDialog();		
		//...
}

public void showListDialog(){
		AlertDialog.Builder builder = new AlertDialog.Builder(this);

		builder.setTitle("Pick a device");
		builder.setItems(boundedItems, new DialogInterface.OnClickListener() {

		   public void onClick(DialogInterface dialog, int item) {
		        connectTo(item);
		   }

		});

		AlertDialog alert = builder.create();

		alert.show();
	}

После выбора устройства подключимся к нему:

	private static final UUID MY_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
	BluetoothSocket btSocket;

	public void connectTo(int id){
		Set<BluetoothDevice> bounded=bluetooth.getBondedDevices();
		for (BluetoothDevice bluetoothDevice : bounded) {
			if(bluetoothDevice.getName().equalsIgnoreCase(boundedItems[id])){
				try {
					btSocket=bluetoothDevice.createRfcommSocketToServiceRecord(MY_UUID);
					btSocket.connect();
					ct=new ConnectionThread(btSocket);
					ct.start();
				} catch (IOException e) {
					e.printStackTrace();
					try {
						btSocket.close();
					} catch (IOException e1) {
						e1.printStackTrace();
					}
					showListDialog();
				}
				return;
			}
		}
		
	}

После подключения стартует поток, который занимается коммуникацией между приложением и роботом:

private class ConnectionThread extends Thread{
	private final InputStream mmInStream;		
	private final BufferedReader br;
        private final OutputStream mmOutStream;
      
        public ConnectionThread(BluetoothSocket socket) throws IOException {
            mmInStream =  socket.getInputStream();
            br=new BufferedReader(new InputStreamReader(mmInStream));
            mmOutStream = socket.getOutputStream();
            
        }
      
        public void run() {
            while (true) {                    
                                try {
                			String line=br.readLine();
					h.obtainMessage(RECIEVE_MESSAGE, line).sendToTarget();
				} catch (IOException e) {
					e.printStackTrace();
				}
            }
        }
        public void sendCmd(char cmd){
        	try{
        		mmOutStream.write(cmd);
        	}catch (IOException e) {
			e.printStackTrace();
		}
        }
}

Поток посылает сообщение основному потоку приложения через Handler, который определяется так:

h = new Handler() {
      public void handleMessage(android.os.Message msg) {
      switch (msg.what) {
            case RECIEVE_MESSAGE:                                                
            	String line=(String)msg.obj;
            	try{
            		float a=Float.parseFloat(line.trim());
            		balancerView.setAngle((float) (a-Math.PI/2.f));		      
            	}catch (Exception e) { }	            	
            break;
	}
      };
};

balancerView это потомок класса SurfaceView, он занимается выводом на экран текущего положения робота.Вот его метод перерисовки:

public void draw(Canvas canvas) {
	Paint paint=new Paint();
	paint.setStrokeWidth(3);
	canvas.save();
	canvas.rotate((float) (angle*180.f/Math.PI), cx, cy);		
		
	paint.setColor(Color.BLACK);        
	canvas.drawRect(cx-15, cy-150, cx 15, cy, paint);
	paint.setColor(Color.WHITE);        
	canvas.drawRect(cx-12, cy-147, cx 12, cy-3, paint);
        
	paint.setColor(Color.BLACK);        
	canvas.drawCircle(cx, cy, 30, paint);
	paint.setColor(Color.WHITE);        
	canvas.drawCircle(cx, cy, 25, paint);
     
	canvas.restore();		
}

Команды роботу посылаются при появлении событий onTouch, чтобы можно было управлять роботом удерживая кнопку.

	@Override
	public boolean onTouch(View v, MotionEvent me) {
		if(me.getAction()==MotionEvent.ACTION_UP){
			ct.sendCmd('c');
			return false;
		}
		if(v==wB){
			ct.sendCmd('w');
		}else if(v==aB){
			ct.sendCmd('a');
		}else if(v==sB){
			ct.sendCmd('s');
		}else if(v==dB){
			ct.sendCmd('d');
		}
		return false;
	}

Драйвер двигателя sn754410ne

Для управления двигателями я использовал драйвер SN754410NE. Я применил его, потому что он у меня был, но вы можете использовать другой, например L293.

Теперь о подключении драйвера к Arduino Uno. Все выводы GND (4,5,12,13) подключите к GND макетной платы.  Контакты драйвера  1 и 16 подключите к выводам 9 и 10 Arduino. Контакты драйвера 2 и 7 подключите к контактам 3 и 4 Arduino, это контакты управления левого двигателя.

Контакты драйвера 10 и 15 подключите к выводам 5 и 6 Arduino, это контакты управления правого двигателя.  Контакты 3 и 6 подключите к левому мотору, а контакты 14 и 11 к правому. Контакты   8 и 16 должны быть подключены к питанию на макетной плате. Источник питания: 9В батарея.

Как и чем управлять

Нормальные люди берут приемник, втыкают в него сервомашинки, регулятор скорости, двигают рычажки на пульте и радуются жизни не задаваясь принципами работы и не углубляясь в подробности. В нашем случае такое не пройдет. Первой задачей стало узнать каким макаром управляются сервомашинки.

Итак, если мы хотим установить привод в крайнее левое положение нужно слать импульсы длительностью 0,9мс с интервалом 20мс, если в крайнее правое — длительность 2,1мс, интервал тот же, ну со средними положениями аналогично. Как оказалось, регуляторы скорости управляются аналогично. Те, кто в теме скажут что это

, который реализовать на любом микроконтроллере — плевое дело. Вот и я так решил, купил в местном магазине сервомашинку и склепал на макетке для нее так называемый сервотестер на ATtiny13. И тут оказалось, что ШИМ не совсем простой, а с подводными камнями.

Как видно из вышеприведенной диаграммы, скважность (отношение длительности импульса к длительности периода) от 5% до 10% (в дальнейшем я за крайние положения принимаю импульсы длительностью 1,0мс и 2,0мс) для 256-значного ШИМ счетчика ATtiny13 это соответствует значениям от 25 до 50.

Но это при условии, что на заполнение счетчика уйдет 20мс, а на деле так не получится и для частоты 9,6МГц и предделителя 1024 нужно ограничить счетчик значением 187(ТОР), в таком случае у нас получится частота 50,134Гц. В большинстве (если не во всех) сервомашинок нету точного генератора опорной частоты и поэтому частота управляющего сигнала может немного плавать.

Если оставить ТОР счетчика 255, то частота управляющего сигнала будет 36,76Гц — на некоторых приводах оно будет работать (возможно с глюками), но далеко не на всех. Итак, теперь у нас 187-значный счетчик, для него 5-10% соответствуют значениям от 10 до 20 — всего 10 значений, немного дискретно получится. Если думаете поиграть с тактовой частотой и предделителем ниже привожу сравнительную табличку для 8-битного ШИМа:

Я не думаю, что для китайской сервомашинки есть существенная разница в 600 и 1200 значений, поэтому вопрос с точностью позиционирования можно считать закрытым.

Математическая модель

Я не буду приводить вывод уравнений движения, все таки это третий курс института. Для тех же, кому интересен вывод, в конце статьи ссылка, где он описан подробнее.

Систему представим в следующем виде:

Маятник это масса mp прикрепленная на конце невесомого стержня длины l. На другой конец стержня прикреплен двигатель, развивающий максимальный момент Mk и передающий его на колесо массой mw и радиусом r. Задача управления — стабилизировать маятник в вертикальном положении и возвращать в начальное положение колесо.

Уравнения движения, описывающие обратный маятник, представимы в следующем виде:

Многоканальное управление

С одной сервомашинкой разобрались, но для самолета их нужно минимум три и еще регулятор скорости. Решение «в лоб» — взять микроконтроллер с четырьмя каналами 16-битного ШИМ, но такой контроллер будет стоять дорого и, скорее всего, займет много места на плате.

Второй вариант — запилить программный ШИМ, но занимать процессорное время — это тоже не вариант. Если снова посмотреть на диаграммы сигнала, то 80% времени он не несет никакой информации, поэтому рациональнее было бы ШИМом задавать только сам импульс 1-2мс.

Почему скважность изменяется в таких узких пределах, ведь проще было бы и формировать и считывать импульсы со скважностью хотя бы 10-90%? Зачем нужен тот неинформативный кусок сигнала занимающий 80% времени? Я заподозрил, что, возможно, эти 80% могут занимать импульсы для других исполнительных механизмов, а потом этот сигнал разделяется на несколько разных.

То есть, в периоде длительностью 20мс могут уместится 10 импульсов длительностью 1-2мс, затем этот сигнал каким-то демультиплексором разделяется на 10 различных с длительностью периода как раз 20мс. Сказано — сделано, нарисовал в PROTEUS такую схемку:

В роли демультиплексора — 74HC238, на его вход E подаются импульсы с выхода микроконтроллера. Эти импульсы — ШИМ с периодом 2мс (500Гц) и скважностью 50-100%. У каждого импульса своя скважность, обозначающая состояние каждого канала. Вот так выглядит сигнал на входе Е:

Для того, чтобы 74HC238 знал на какой выход подать текущий сигнал используем PORTC микроконтроллера и входы A, B, C демультиплексора. В результате на выходах получаем такие сигналы:

Сигналы на выходе получаются правильной частоты (50Гц) и скважности (5-10%). Итак, нужно генерировать ШИМ частотой 500Гц и заполнением 50-100%, вот табличка для настройки предделителя и ТОР 16-битного счетчика:

Интересно, что возможное количество значений ШИМа ровно в 1000 раз меньше частоты таймера.

Передающая часть

Самолетная часть есть, осталось разобраться с наземной аппаратурой. Как уже писалось ранее, данные передаются по UART, на каждый канал по одному байту. Вначале подключал свою систему проводом через

к компьютеру и команды слал через терминал. Чтобы дешифратор определял начало посылки, а в будущем выделял посылки адресуемые именно ему, вначале шлется байт-идентификатор, затем 8 байт определяющих состояние каналов. Позже стал использовать радиомодули, при отключении передатчика все моторчики начинали дико дергаться.

Дабы отфильтровать сигнал от шумов, десятым байтом шлю XOR всех 9 предыдущих байт. Помогло, но слабо, добавил еще проверку на таймаут между байтами, если он превышается — вся посылка игнорится и прием начинается заново, с ожидания байта-идентификатора.

Число в нижнем левом углу — контрольная сумма. Передвигая ползунки на компе двигались рули на самолете! Вообщем отладил я все это и стал думать о пульте ДУ, купил для него вот такие джойстики:

Но потом меня посетила одна мысль. В свое время я тащился от всяких авиасимуляторов: «Ил-2 Штурмовик», «Lock On», «MSFSX», «Ка-50 Черная Акула» и др. Соответственно был у меня джойстик Genius F-23 и решил я прикрутить его к вышеописанной проге с ползунками. Погуглил как это реализовать, нашел этот

и получилось! Управлять самолетиком с помощью полноценного джойстика, мне кажется, гораздо круче, чем маленькой палочкой на пульте. Вообщем все вместе изображено на первой фотке — это нетбук, джойстик, преобразователь на FT232, и подключенный к нему передатчик HM-T868.

Питание

Arduino питается от батарейки 9В через соответствующий разъём. Для питания двигателей я использовал 4 батарейки размера D и соответствующий разъём. Для питания двигателей подключите провода от держателя к плате с SN754410NE.

Программная реализация

Для ATmega8 с тактовой частотой 16МГц в AtmelStudio6 все реализуется следующим образом: вначале задефайним значения счетчика для крайних положений сервомашинок:

#define LOW 16000U
#define HIGH 32000U

затем инициализируем генератор ШИМа на таймере/счетчике1:

OCR1A = HIGH; //Устанавливаем ТОР
TCCR1A = 0<<COM1A1 | 0<<COM1A0 | 1<<COM1B1 | 0<<COM1B0 | 0<<FOC1A | 0<<FOC1B | 1<<WGM11 | 1<<WGM10; //Запускаем неинвертированный Fast PWM на выходе OC1B с верхним значением счетчика, которое записанно в OCR1A
TCCR1B = 0<<ICNC1 | 0<<ICES1 | 1<<WGM13 | 1<<WGM12 | 0<<CS12 | 0<<CS11 | 1<<CS10; //предделитель 1
TIMSK = 1<<OCIE1A | 1<<OCIE1B | 0<<TOIE1; //Разрешаем прерывания по совпадению

Остается реализовать прерывания:

Пуск!

Итак, есть самолетик, есть радиоуправление — Поехали!(с) Первый полет производился над асфальтом, результат — сломанный пополам фюзеляж и полувырванный двигатель. Второй полет производился над более мягкой поверхностью:

Последующие полетов 10 были тоже не особо удачными. Основной причиной я считаю сильную дискретность джойстика — по крену он выдавал только 16 значений (вместо возможных 256), с осью тангажа — не лучше. Но так как в результате испытаний самолет был значительно поврежден и не подлежит ремонту:

Радиоуправляемый танк на arduino nano | каталог самоделок

Реализация в железе

Сам каркас робота это алюминиевые профиля 12мм и 14мм, они входят друг в друга. Соединены заклепками. Электроника прикреплена на кусок стеклотекстолита в форме буквы T. Моторы так же прикреплены через стеклотекстолитовый переходник.

Их крутящий момент 2,2кг*см или 0.2Нм. Исходя из симуляции нам нужно гораздо больше, поэтому были выбраны другие моторы:

ссылка на производителя
Максимальный крутящий момент 14кг*см или 1.4Нм. Тока они потребляют до 5A, поэтому популярный у ардуинщиков L293D тут не подойдет.

Для определения угла и угловой скорости используется IMU — гироскоп и акселерометр. У меня завалялась плата с гироскопом L3G и акселерометром с магнетометром LSM303. Подобных плат очень много и я не стану приводить код получения значений сенсоров. Однако показания датчиков нужно отфильтровать, так как гироскоп постоянно уходит, а акселерометр шумит и сильно врет, если робот начинает двигаться не меняя угла.

float lastCompTime=0;
float filterAngle=1.50;
float dt=0.005;

float comp_filter(float newAngle, float newRate) {

dt=(millis()-lastCompTime)/1000.0;
float filterTerm0;
float filterTerm1;
float filterTerm2;
float timeConstant;

timeConstant=0.5;

filterTerm0 = (newAngle - filterAngle) * timeConstant * timeConstant;
filterTerm2  = filterTerm0 * dt;
filterTerm1 = filterTerm2   ((newAngle - filterAngle) * 2 * timeConstant)   newRate;
filterAngle = (filterTerm1 * dt)   filterAngle;
lastCompTime=millis();
return filterAngle;
}

Работает не идеально, но достаточно хорошо для данной задачи.

Следующий сенсор — квадратурный энкодер на моторе. Он генерирует прямоугольные импульсы на 2х своих выводах:

Считать их можно либо прерываниями, либо считыванием значений в цикле. На arduino playground есть замечательная статья с примерами кода.

Осталось получить угловую скорость колеса. Тут на помощь приходит школьная формула пройденное расстояние/затраченное время.

#define ToPhiRad(x) ((x)*0.00280357142)

timer_old = timer;
timer=millis();
G_Dt = (timer-timer_old)/1000.0;    
dPhi=(ToPhiRad(encoder0Pos)-lastPhi)/G_Dt;

ToPhiRad переводит количество тиков энкодера в угол колеса, мой энкодер выдает около 2240 тиков на оборот. Чтобы получить угол нужно умножить тики на 2 Пи и разделить на их количество при полном обороте колеса.

Показания сенсоров поступают в LQR регулятор:

float K1=0.1,K2=0.29,K3=6.5,K4=1.12;

long getLQRSpeed(float phi,float dphi,float angle,float dangle){
  return constrain((phi*K1 dphi*K2 K3*angle dangle*K4)*285,-400,400);
}

Коэффициенты взяты из Matlab, правда для большей стабильности я подправил 2 первых коэффициента.

Мой драйвер, вернее его библиотека, принимает значения от -400 до 400. Я предположил, что на 400 он выдает на мотор 12В, т.е. мотор развивает максимальный момент (1.4Нм). Разделив 400 на 1.4 получаем коэффициент перевода из Нм, которые выдает LQR, в значения, понятные драйверу.

Просто стабилизировать робота в одной точке не очень интересно, поэтому к нему добавился BT-модуль HC-05. Модуль подключается к серийному порту микроконтроллера. Он работает на 3.3В, а arduino на 5В, поэтому подключать принимающий вход модуля надо через делитель напряжения. Вот схема подключения:

Во время цикла микроконтроллер опрашивает модуль на предмет символов:

float phiDif=0.f;
float factorDif=0.f;

float getPhiAdding(float dif){ //сколько прибавлять к углу колес для движения вперед-назад

  if(dif<200 && dif>-200){return 0.f;}
  float ret = dif*0.08;

  return ret;
}

float getFactorAdding(float dif){//сколько добавлять к управлению для поворота
  if(dif<200 && dif>-200){return 0.f;}
  float ret = dif/500*20;
  return ret;
}
//========
if (Serial.available()){
    BluetoothData=Serial.read();
     if(BluetoothData=='w'){   
      phiDif=200;
     } else if(BluetoothData=='s'){ 
      phiDif=-200;
     } else if(BluetoothData=='a'){   
      factorDif=200;
     } else if(BluetoothData=='d'){   
      factorDif=-200;
     } else if(BluetoothData=='c'){   
      factorDif=0;
      phiDif=0;
     }
    }

В конечном итоге показания сенсоров поступают в регулятор, а его управление и воздействие пользователя поступают на моторы:

    encoder0Pos =getPhiAdding(phiDif);
    lastPhi=ToPhiRad(encoder0Pos);    
    spd=getLQRSpeed(ToPhiRad(encoder0Pos),dPhi,balanceAt-angle,gyroRate[coordY]);
    float factorL=getFactorAdding(factorDif);
    md.setSpeeds(spd-factorL,spd factorL);

Раз в 50 миллисекунд посылается телеметрия-угол робота:

    if(millis()P==0){
      Serial.println(angle);
    }

Сборка

Когда все части готовы, пришло время собрать их. Сначала мы должны прикрепить Arduino на основу. Потом при помощи термоклея прикрепим дальномер с сервоприводом на переднюю часть робота. Потом необходимо прикрепить батареи. Вы можете разместить их где вам угодно, но я разместил их рядом с Arduino. Когда всё готово,  вы можете включить робота, чтобы удостоверится что Arduino работает.

Симуляция

В Matlab simulink можно легко эмулировать систему, если кому-нибудь необходимо ссылка на репозитарий с математической моделью в конце статьи. Здесь же я только приведу графики.

Угол отклонения маятника:

Угол отклонения колеса:

Момент двигателя:

Синтезирование управления

Завидую людям, у которых работает PID-регулятор. Я потратил несколько часов на подгонку его коэффициентов, но так и не сумел добиться стоящего результата. Научный руководитель посоветовал воспользоваться линейно-квадратичным регулятором(

). Этот регулятор, в отличие от PID-регулятора, представляет собой просто произведение своих коэффициентов на ошибки по каждой координате. Никаких дискретных аналогов производной и интеграла. Однако для его вычисления нужна модель системы и умение решать уравнение Риккати, ну или Matlab.

В матлабе расчет регулятора представляет собой такой набор команд:

A=[0 1.0 0 0;0 0 -140 0;0 0 0 1.0;0 0 28 0]
B=[0;212.85;0;-19.15]
Q=[5 0 0 0;0 5 0 0;0 0 1 0;0 0 0 1]
R=1500
[K,S,e]=lqr(A,B,Q,R)

Здесь матрицы A и B — соответствующие матрицы из линеаризованной модели с подставленными значениями реального робота.

Матрица Q определяет на сколько нужно штрафовать систему за отклонение от начала координат, заметьте, в нашем случае в координаты входят скорости.

Матрица R определяет на сколько нужно штрафовать систему за растрату энергии управлением.

В переменной K будут лежать коэффициенты регулятора.

Установка дальномера

Ультразвуковой дальномер помогает роботу избегать препятствий при движении. Он располагается на стандартном сервоприводе, который расположен на передней части робота. Когда робот замечает объект на расстоянии 10 см, сервопривод начинает крутиться, ища проход,  а затем Arduino решает, какая сторона наиболее приятна для передвижения.

На датчике есть три контакта GND, 5В и сигнал. GND подключите к GND, 5В к 5В Arduino и сигнал подключите на 7 контакт Arduino.

Шасси

Шасси я взял из танка, купленного за 10$. Основу можно прикрепить к нему в любом месте, но я прикрепил её посередине.

Заключение

Самое приятное во всей постройке это то, что математическая модель сошлась с физической реализацией. Сама постройка железки не представляет из себя какой-то сложности, однако подбор правильных моторов, высоты робота, массы груза сверху и синтезирование управления довольно интересная задача.

Как и обещал вывод уравнений движения перевернутого маятника на колесе: Вывод уравнений и немного о постройкерепозитарий на GitHub