Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия | ||
kiber_tc_2018 [2019/03/18 20:29] super_admin [Вариант 2] |
kiber_tc_2018 [2019/11/20 23:06] (текущий) super_admin [Реализация машины состояний при движении по линии] |
||
---|---|---|---|
Строка 3: | Строка 3: | ||
===== Основы электроники ===== | ===== Основы электроники ===== | ||
* [[electronics|Основы электроники]] | * [[electronics|Основы электроники]] | ||
+ | * [[http://www.radio-samodel.ru/kontur.html]] - Нужная теория | ||
+ | * [[https://phet.colorado.edu/en/simulation/ohms-law]] - модели, симуляторы | ||
* [[http://www.intepra.ru/wiki/doku.php?id=electronics#%D1%82%D0%B5%D0%BE%D1%80%D0%B8%D1%8F|Теория]] | * [[http://www.intepra.ru/wiki/doku.php?id=electronics#%D1%82%D0%B5%D0%BE%D1%80%D0%B8%D1%8F|Теория]] | ||
* [[http://wiki.amperka.ru/%D0%BA%D0%BE%D0%BD%D1%81%D0%BF%D0%B5%D0%BA%D1%82-arduino:%D0%BF%D1%80%D0%B8%D0%BD%D1%86%D0%B8%D0%BF%D0%B8%D0%B0%D0%BB%D1%8C%D0%BD%D1%8B%D0%B5-%D1%81%D1%85%D0%B5%D0%BC%D1%8B|Принципиальные схемы]] | * [[http://wiki.amperka.ru/%D0%BA%D0%BE%D0%BD%D1%81%D0%BF%D0%B5%D0%BA%D1%82-arduino:%D0%BF%D1%80%D0%B8%D0%BD%D1%86%D0%B8%D0%BF%D0%B8%D0%B0%D0%BB%D1%8C%D0%BD%D1%8B%D0%B5-%D1%81%D1%85%D0%B5%D0%BC%D1%8B|Принципиальные схемы]] | ||
Строка 367: | Строка 369: | ||
</code> | </code> | ||
+ | ==== Релейный регулятор датчика линии для мобильного робота (L293D) ==== | ||
+ | <code C++> | ||
+ | #define PWMA 6 // выходы arduino | ||
+ | #define PWMB 3 | ||
+ | #define AIN1 7 | ||
+ | #define AIN2 8 | ||
+ | #define BIN1 2 | ||
+ | #define BIN2 4 | ||
+ | |||
+ | |||
+ | #define lS 12 | ||
+ | #define rS 11 | ||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 180; | ||
+ | int dif = 170; | ||
+ | uint8_t lSState=0; | ||
+ | uint8_t rSState=0; | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
+ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | |||
+ | } | ||
+ | |||
+ | void loop() { | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | Serial.println(lSState); | ||
+ | Serial.println(rSState); | ||
+ | Serial.println(""); | ||
+ | |||
+ | |||
+ | if (lSState == 0 && rSState == 0){ | ||
+ | goForward(n_speed); | ||
+ | } | ||
+ | |||
+ | if (lSState == 1 && rSState == 1){ | ||
+ | applyBrakes (); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0){ | ||
+ | veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1){ | ||
+ | veerLeft(n_speed, dif); | ||
+ | } | ||
+ | |||
+ | delay(1); | ||
+ | |||
+ | } | ||
+ | |||
+ | |||
+ | void goForward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void goBackward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateRight (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateLeft (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void veerLeft (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v -d); | ||
+ | } | ||
+ | |||
+ | void veerRight (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v -d); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v ); | ||
+ | } | ||
+ | |||
+ | void applyBrakes () | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,255); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,255); | ||
+ | } | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | </code> | ||
+ | |||
+ | |||
+ | ==== Релейный регулятор датчиков линии на драйвере L293D (обновленная версия) ==== | ||
+ | |||
+ | <code> | ||
+ | |||
+ | |||
+ | |||
+ | #define PWMA 6 // выходы arduino | ||
+ | #define PWMB 3 | ||
+ | #define AIN1 7 | ||
+ | #define AIN2 8 | ||
+ | #define BIN1 2 | ||
+ | #define BIN2 4 | ||
+ | |||
+ | |||
+ | #define lS 12 | ||
+ | #define rS 11 | ||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 180; | ||
+ | int dif = 180; | ||
+ | |||
+ | uint8_t lSState; | ||
+ | uint8_t rSState; | ||
+ | |||
+ | int state; | ||
+ | int prevState; | ||
+ | int readyState = 0; | ||
+ | |||
+ | void resetState(int st){ | ||
+ | prevState = state; | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
+ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | while (readyState == 0) { | ||
+ | testSensors(); | ||
+ | if (prevState != 11) | ||
+ | readyState = 1; | ||
+ | delay(1000); | ||
+ | } | ||
+ | delay(2000); | ||
+ | |||
+ | } | ||
+ | |||
+ | void testSensors(){ | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | //Serial.println(lSState); | ||
+ | //Serial.println(rSState); | ||
+ | //Serial.println(""); | ||
+ | | ||
+ | if (lSState == 0 && rSState == 0 ){ | ||
+ | resetState(0); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0 ){ | ||
+ | resetState(10); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | // if (current_dist <= dangerous_dist ){ | ||
+ | // resetState(1); | ||
+ | //applyBrakes (); | ||
+ | // } | ||
+ | if (lSState == 1 && rSState == 1 ){ | ||
+ | //if (readyState == 0) | ||
+ | resetState(11); | ||
+ | //veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1 ){ | ||
+ | resetState(1); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | } | ||
+ | | ||
+ | } | ||
+ | |||
+ | |||
+ | |||
+ | void loop() { | ||
+ | testSensors(); | ||
+ | updateMotion(n_speed, dif); | ||
+ | |||
+ | // centralLineFollower(); | ||
+ | |||
+ | delay(1); | ||
+ | |||
+ | } | ||
+ | |||
+ | void updateMotion(int sp, int d){ | ||
+ | switch ( state ) { | ||
+ | case 0: | ||
+ | if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | else if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | break; | ||
+ | case 1: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 10: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 11: | ||
+ | // Code | ||
+ | if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | } | ||
+ | | ||
+ | } | ||
+ | |||
+ | void leftLineFollower(){ | ||
+ | //goForward(n_speed); | ||
+ | //applyBrakes (); | ||
+ | //veerRight(n_speed, dif); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | |||
+ | } | ||
+ | void centralLineFollower(){ | ||
+ | |||
+ | } | ||
+ | |||
+ | void goForward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void goBackward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateRight (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateLeft (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void veerLeft (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v -d); | ||
+ | } | ||
+ | |||
+ | void veerRight (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v -d); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v ); | ||
+ | } | ||
+ | |||
+ | void applyBrakes () | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,255); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,255); | ||
+ | } | ||
+ | |||
+ | </code> | ||
==== Управление питанием посредством кнопки ==== | ==== Управление питанием посредством кнопки ==== | ||
Строка 556: | Строка 908: | ||
- | ==== Вариант 2 ==== | ||
+ | ==== Вариант 1.1. Простейший релейный регулятор движения по линии ==== | ||
<code C++> | <code C++> | ||
- | #define PWMA 9 // выходы arduino | + | |
+ | #define PWMA 11 // выходы arduino | ||
#define PWMB 10 | #define PWMB 10 | ||
#define AIN1 6 | #define AIN1 6 | ||
Строка 567: | Строка 920: | ||
#define BIN1 5 | #define BIN1 5 | ||
#define BIN2 4 | #define BIN2 4 | ||
- | #define STBY 13 | + | #define STBY 13 |
- | void setup() { | + | #define lS 9 |
+ | #define rS 8 | ||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 180; | ||
+ | int dif = 170; | ||
+ | uint8_t lSState=0; | ||
+ | uint8_t rSState=0; | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
/* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
pinMode(PWMA,OUTPUT); | pinMode(PWMA,OUTPUT); | ||
Строка 579: | Строка 944: | ||
pinMode(BIN2,OUTPUT); | pinMode(BIN2,OUTPUT); | ||
pinMode(STBY,OUTPUT); | pinMode(STBY,OUTPUT); | ||
- | delay(5000); | + | pinMode(lS,INPUT); |
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | startUp(); | ||
+ | } | ||
+ | |||
+ | void loop() { | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | Serial.println(lSState); | ||
+ | Serial.println(rSState); | ||
+ | Serial.println(""); | ||
+ | |||
+ | |||
+ | if (lSState == 0 && rSState == 0){ | ||
+ | goForward(n_speed); | ||
+ | } | ||
+ | |||
+ | if (lSState == 1 && rSState == 1){ | ||
+ | applyBrakes (); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0){ | ||
+ | veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1){ | ||
+ | veerLeft(n_speed, dif); | ||
+ | } | ||
+ | |||
+ | delay(1); | ||
+ | |||
+ | } | ||
+ | |||
+ | void goForward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
} | } | ||
+ | |||
+ | void goBackward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateRight (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateLeft (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void veerLeft (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v -d); | ||
+ | } | ||
+ | |||
+ | void veerRight (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v -d); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v ); | ||
+ | } | ||
+ | |||
+ | void applyBrakes () | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,255); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,255); | ||
+ | } | ||
+ | |||
+ | void startUp () | ||
+ | { | ||
+ | digitalWrite(STBY,HIGH); | ||
+ | } | ||
+ | |||
+ | |||
+ | void shutDown () | ||
+ | { | ||
+ | digitalWrite(STBY,LOW); | ||
+ | } | ||
+ | |||
+ | </code> | ||
+ | |||
+ | ==== Реализация машины состояний при движении по линии ==== | ||
+ | |||
+ | |||
+ | <code C++> | ||
+ | |||
+ | #define PWMA 11 // выходы arduino | ||
+ | #define PWMB 10 | ||
+ | #define AIN1 6 | ||
+ | #define AIN2 7 | ||
+ | #define BIN1 5 | ||
+ | #define BIN2 4 | ||
+ | #define STBY 13 | ||
+ | |||
+ | #define lS 9 | ||
+ | #define rS 8 | ||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 200; | ||
+ | int dif = 200; | ||
+ | |||
+ | uint8_t lSState; | ||
+ | uint8_t rSState; | ||
+ | |||
+ | int state; | ||
+ | int prevState; | ||
+ | int readyState = 0; | ||
+ | |||
+ | void resetState(int st){ | ||
+ | prevState = state; | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
+ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | while (readyState == 0) { | ||
+ | testSensors(); | ||
+ | if (prevState != 11) | ||
+ | readyState = 1; | ||
+ | delay(1000); | ||
+ | } | ||
+ | delay(2000); | ||
+ | startUp(); | ||
+ | } | ||
+ | |||
+ | void testSensors(){ | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | //Serial.println(lSState); | ||
+ | //Serial.println(rSState); | ||
+ | //Serial.println(""); | ||
+ | | ||
+ | if (lSState == 0 && rSState == 0 ){ | ||
+ | resetState(0); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0 ){ | ||
+ | resetState(10); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | // if (current_dist <= dangerous_dist ){ | ||
+ | // resetState(1); | ||
+ | //applyBrakes (); | ||
+ | // } | ||
+ | if (lSState == 1 && rSState == 1 ){ | ||
+ | //if (readyState == 0) | ||
+ | resetState(11); | ||
+ | //veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1 ){ | ||
+ | resetState(1); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | } | ||
+ | | ||
+ | } | ||
+ | |||
+ | |||
+ | |||
void loop() { | void loop() { | ||
- | + | testSensors(); | |
- | startUp(); | + | updateMotion(n_speed, dif); |
- | goForward(); | + | |
- | delay(1500); | + | // centralLineFollower(); |
- | turnAround(); | + | |
- | goForward(); | + | //delay(1); |
- | delay(1500); | + | |
- | turnAround(); | + | } |
- | goBackward(); | + | |
- | delay(1500); | + | |
- | rotateLeft(); | + | |
- | delay(560); | + | |
- | rotateRight(); | + | |
- | delay(2240); | + | |
- | goForward(); | + | |
- | delay(1000); | + | |
- | applyBrakes(); | + | |
- | delay(2000); | + | |
- | } | + | |
- | /* Определение функций */ | + | void updateMotion(int sp, int d){ |
- | /* Опытным путем было выяснено, что при коэффициенте ШИМ равном 233 */ | + | switch ( state ) { |
- | /* для левого двигателя и 255 для правого позволяют роботу */ | + | case 0: |
- | /* двигаться по прямой на полной скорости. Тесты показали, что робот */ | + | if (prevState == 1) |
- | /* совершает 27 оборотов вокруг своей оси в минуту при вращении двигателей */ | + | //veerLeft(sp, d); |
- | /* в противоположные стороны при полном заполнении ШИМ. Эта величина */ | + | veerRight(sp, d); |
- | /* была использована для определения времени, */ | + | else if (prevState == 10) |
- | /* за которое робот повернется на 90, 180 и 360 градусов. */ | + | //veerRight(sp, d); |
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | case 1: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 10: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 11: | ||
+ | // Code | ||
+ | if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | } | ||
+ | |||
+ | } | ||
- | void goForward () | + | void leftLineFollower(){ |
+ | //goForward(n_speed); | ||
+ | //applyBrakes (); | ||
+ | //veerRight(n_speed, dif); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | |||
+ | } | ||
+ | void centralLineFollower(){ | ||
+ | |||
+ | } | ||
+ | |||
+ | void goForward (int v) | ||
{ | { | ||
- | digitalWrite (AIN1,HIGH); | + | digitalWrite (AIN1,HIGH); |
- | digitalWrite (AIN2,LOW); | + | digitalWrite (AIN2,LOW); |
- | analogWrite(PWMA,155); | + | analogWrite(PWMA,v); |
- | digitalWrite (BIN1,HIGH); | + | digitalWrite (BIN1,HIGH); |
- | digitalWrite (BIN2,LOW); | + | digitalWrite (BIN2,LOW); |
- | analogWrite(PWMB,155); | + | analogWrite(PWMB,v); |
} | } | ||
- | + | ||
- | void goBackward () | + | void goBackward (int v) |
{ | { | ||
- | digitalWrite (AIN1,LOW); | + | digitalWrite (AIN1,LOW); |
- | digitalWrite (AIN2,HIGH); | + | digitalWrite (AIN2,HIGH); |
- | analogWrite(PWMA,155); | + | analogWrite(PWMA,v); |
- | digitalWrite (BIN1,LOW); | + | digitalWrite (BIN1,LOW); |
- | digitalWrite (BIN2,HIGH); | + | digitalWrite (BIN2,HIGH); |
- | analogWrite(PWMB,155); | + | analogWrite(PWMB,v); |
} | } | ||
- | + | ||
- | void rotateRight () | + | void rotateRight (int v) |
{ | { | ||
- | digitalWrite (AIN1,HIGH); | + | digitalWrite (AIN1,HIGH); |
- | digitalWrite (AIN2,LOW); | + | digitalWrite (AIN2,LOW); |
- | analogWrite(PWMA,100); | + | analogWrite(PWMA,v); |
- | digitalWrite (BIN1,LOW); | + | digitalWrite (BIN1,LOW); |
- | digitalWrite (BIN2,HIGH); | + | digitalWrite (BIN2,HIGH); |
- | analogWrite(PWMB,100); | + | analogWrite(PWMB,v); |
} | } | ||
- | + | ||
- | void rotateLeft () | + | void rotateLeft (int v) |
{ | { | ||
- | digitalWrite (AIN1,LOW); | + | digitalWrite (AIN1,LOW); |
- | digitalWrite (AIN2,HIGH); | + | digitalWrite (AIN2,HIGH); |
- | analogWrite(PWMA,100); | + | analogWrite(PWMA,v); |
- | digitalWrite (BIN1,HIGH); | + | digitalWrite (BIN1,HIGH); |
- | digitalWrite (BIN2,LOW); | + | digitalWrite (BIN2,LOW); |
- | analogWrite(PWMB,100); | + | analogWrite(PWMB,v); |
} | } | ||
- | + | ||
- | void veerLeft () | + | void veerLeft (int v, int d) |
{ | { | ||
- | digitalWrite (AIN1,HIGH); | + | digitalWrite (AIN1,HIGH); |
- | digitalWrite (AIN2,LOW); | + | digitalWrite (AIN2,LOW); |
- | analogWrite(PWMA,100); | + | analogWrite(PWMA,v); |
- | digitalWrite (BIN1,HIGH); | + | digitalWrite (BIN1,HIGH); |
- | digitalWrite (BIN2,LOW); | + | digitalWrite (BIN2,LOW); |
- | analogWrite(PWMB,155); | + | analogWrite(PWMB,v -d); |
} | } | ||
- | + | ||
- | void veerRight () | + | void veerRight (int v, int d) |
{ | { | ||
- | digitalWrite (AIN1,HIGH); | + | digitalWrite (AIN1,HIGH); |
- | digitalWrite (AIN2,LOW); | + | digitalWrite (AIN2,LOW); |
- | analogWrite(PWMA,155); | + | analogWrite(PWMA,v -d); |
- | digitalWrite (BIN1,HIGH); | + | digitalWrite (BIN1,HIGH); |
- | digitalWrite (BIN2,LOW); | + | digitalWrite (BIN2,LOW); |
- | analogWrite(PWMB,100); | + | analogWrite(PWMB,v ); |
} | } | ||
+ | |||
void applyBrakes () | void applyBrakes () | ||
{ | { | ||
- | digitalWrite (AIN1,HIGH); | + | digitalWrite (AIN1,HIGH); |
- | digitalWrite (AIN2,HIGH); | + | digitalWrite (AIN2,HIGH); |
- | analogWrite(PWMA,155); | + | analogWrite(PWMA,255); |
- | digitalWrite (BIN1,HIGH); | + | digitalWrite (BIN1,HIGH); |
- | digitalWrite (BIN2,HIGH); | + | digitalWrite (BIN2,HIGH); |
- | analogWrite(PWMB,155); | + | analogWrite(PWMB,255); |
} | } | ||
+ | |||
void startUp () | void startUp () | ||
{ | { | ||
- | digitalWrite(STBY,HIGH); | + | digitalWrite(STBY,HIGH); |
} | } | ||
+ | |||
- | void turnAround() | + | |
+ | void shutDown () | ||
{ | { | ||
- | rotateLeft(); | + | digitalWrite(STBY,LOW); |
- | delay(5000); | + | |
} | } | ||
+ | </code> | ||
+ | |||
+ | ==== Реализация машины состояний при движении по линии (Nano ATmega328P Old) ==== | ||
+ | |||
+ | <code C++> | ||
+ | |||
+ | #define PWMA 9 // выходы arduino | ||
+ | #define PWMB 10 | ||
+ | #define AIN1 6 | ||
+ | #define AIN2 7 | ||
+ | #define BIN1 5 | ||
+ | #define BIN2 4 | ||
+ | #define STBY 13 | ||
+ | |||
+ | #define lS 2 | ||
+ | #define rS 3 | ||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 200; | ||
+ | int dif = 200; | ||
+ | |||
+ | uint8_t lSState; | ||
+ | uint8_t rSState; | ||
+ | |||
+ | int state; | ||
+ | int prevState; | ||
+ | int readyState = 0; | ||
+ | |||
+ | void resetState(int st){ | ||
+ | prevState = state; | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
+ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | while (readyState == 0) { | ||
+ | testSensors(); | ||
+ | if (prevState != 11) | ||
+ | readyState = 1; | ||
+ | delay(1000); | ||
+ | } | ||
+ | delay(2000); | ||
+ | startUp(); | ||
+ | } | ||
+ | |||
+ | void testSensors(){ | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | //Serial.println(lSState); | ||
+ | //Serial.println(rSState); | ||
+ | //Serial.println(""); | ||
+ | |||
+ | if (lSState == 0 && rSState == 0 ){ | ||
+ | resetState(0); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0 ){ | ||
+ | resetState(10); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | // if (current_dist <= dangerous_dist ){ | ||
+ | // resetState(1); | ||
+ | //applyBrakes (); | ||
+ | // } | ||
+ | if (lSState == 1 && rSState == 1 ){ | ||
+ | //if (readyState == 0) | ||
+ | resetState(11); | ||
+ | //veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1 ){ | ||
+ | resetState(1); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | } | ||
+ | |||
+ | } | ||
+ | |||
+ | |||
+ | |||
+ | void loop() { | ||
+ | testSensors(); | ||
+ | updateMotion(n_speed, dif); | ||
+ | |||
+ | // centralLineFollower(); | ||
+ | |||
+ | //delay(1); | ||
+ | |||
+ | } | ||
+ | |||
+ | void updateMotion(int sp, int d){ | ||
+ | switch ( state ) { | ||
+ | case 0: | ||
+ | if (prevState == 1) | ||
+ | //veerLeft(sp, d); | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 10) | ||
+ | //veerRight(sp, d); | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | case 1: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 10: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 11: | ||
+ | // Code | ||
+ | if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | } | ||
+ | |||
+ | } | ||
+ | |||
+ | void leftLineFollower(){ | ||
+ | //goForward(n_speed); | ||
+ | //applyBrakes (); | ||
+ | //veerRight(n_speed, dif); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | |||
+ | } | ||
+ | void centralLineFollower(){ | ||
+ | |||
+ | } | ||
+ | |||
+ | void goForward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void goBackward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateRight (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateLeft (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void veerLeft (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v -d); | ||
+ | } | ||
+ | |||
+ | void veerRight (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v -d); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v ); | ||
+ | } | ||
+ | |||
+ | void applyBrakes () | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,255); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,255); | ||
+ | } | ||
+ | |||
+ | void startUp () | ||
+ | { | ||
+ | digitalWrite(STBY,HIGH); | ||
+ | } | ||
+ | |||
+ | |||
+ | |||
void shutDown () | void shutDown () | ||
{ | { | ||
- | digitalWrite(STBY,LOW); | + | digitalWrite(STBY,LOW); |
} | } | ||
</code> | </code> | ||
+ | ==== Вариант 2.1. Простейший релейный регулятор движения по линии + ультразвуковой датчик препятствия (промежуточный вариант, требует доработки) ==== | ||
- | Вариант 2.1. Простейший релейный регулятор движения по линии | ||
<code C++> | <code C++> | ||
- | #define PWMA 9 // выходы arduino | + | #include <Ping.h> |
+ | |||
+ | |||
+ | #define PWMA 11 // выходы arduino | ||
#define PWMB 10 | #define PWMB 10 | ||
#define AIN1 6 | #define AIN1 6 | ||
Строка 714: | Строка 1524: | ||
#define BIN2 4 | #define BIN2 4 | ||
#define STBY 13 | #define STBY 13 | ||
+ | |||
+ | #define lS 9 | ||
+ | #define rS 8 | ||
- | #define lS 8 // левый датчик линии | + | int distance = 10; |
- | #define rS 12 | + | int ping_distance = 400; |
- | int min_speed = 100; | + | Ping ping = Ping(2); |
- | int max_speed = 255; | + | |
- | int n_speed = 110; | + | |
- | int dif = 110; | + | |
- | uint8_t lSState=0; | + | |
- | uint8_t rSState=0; | + | |
+ | uint32_t previousMillis = 0; | ||
+ | uint16_t timePeriod = 50; | ||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 220; | ||
+ | int dif = 220; | ||
+ | |||
+ | uint8_t lSState; | ||
+ | uint8_t rSState; | ||
+ | |||
+ | int state; | ||
+ | int prevState; | ||
+ | int readyState = 0; | ||
+ | |||
+ | void resetState(int st){ | ||
+ | prevState = state; | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | |||
//int state; | //int state; | ||
+ | void setup() { | ||
- | void setup() { | + | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ |
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | while (readyState == 0) { | ||
+ | testSensors(); | ||
+ | if (prevState != 11) | ||
+ | readyState = 1; | ||
+ | delay(1000); | ||
+ | } | ||
+ | delay(2000); | ||
+ | startUp(); | ||
+ | } | ||
+ | /* | ||
+ | void checkTimer(){ | ||
+ | uint32_t currentMillis = millis(); | ||
+ | if (currentMillis - previousMillis >= timePeriod){ | ||
+ | Serial.println(millis()); | ||
+ | previousMillis = currentMillis; | ||
+ | } | ||
+ | } | ||
+ | */ | ||
+ | void testSensors(){ | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | //Serial.println(lSState); | ||
+ | //Serial.println(rSState); | ||
+ | //Serial.println(""); | ||
+ | |||
+ | if (lSState == 0 && rSState == 0 ){ | ||
+ | resetState(0); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | if (lSState == 1 && rSState == 0 ){ | ||
+ | resetState(10); | ||
+ | //goForward(n_speed); | ||
+ | } | ||
+ | // if (current_dist <= dangerous_dist ){ | ||
+ | // resetState(1); | ||
+ | //applyBrakes (); | ||
+ | // } | ||
+ | if (lSState == 1 && rSState == 1 ){ | ||
+ | //if (readyState == 0) | ||
+ | resetState(11); | ||
+ | //veerRight(n_speed, dif); | ||
+ | } | ||
+ | if (lSState == 0 && rSState == 1 ){ | ||
+ | resetState(1); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | } | ||
- | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | + | } |
- | pinMode(PWMA,OUTPUT); | + | |
- | pinMode(AIN1,OUTPUT); | + | void ping_fire(){ |
- | pinMode(AIN2,OUTPUT); | + | ping.fire(); |
- | pinMode(PWMB,OUTPUT); | + | //Serial.println("***"); |
- | pinMode(BIN1,OUTPUT); | + | |
- | pinMode(BIN2,OUTPUT); | + | |
- | pinMode(STBY,OUTPUT); | + | |
- | pinMode(lS,INPUT); | + | |
- | pinMode(rS,INPUT); | + | |
- | Serial.begin(9600); | + | |
- | startUp(); | + | |
} | } | ||
void loop() { | void loop() { | ||
- | lSState = digitalRead(lS); | + | // time ===================== |
- | rSState = digitalRead(rS); | + | uint32_t currentMillis = millis(); |
- | Serial.println(lSState); | + | if (currentMillis - previousMillis >= timePeriod){ |
- | Serial.println(rSState); | + | //Serial.println(millis()); |
- | Serial.println(""); | + | //Serial.println(ping.centimeters()); |
+ | ping_distance = ping.centimeters(); | ||
+ | ping_fire(); | ||
+ | previousMillis = currentMillis; | ||
+ | } | ||
+ | //======================= | ||
+ | testSensors(); | ||
+ | if (ping_distance > distance){ | ||
+ | updateMotion(n_speed, dif); | ||
+ | } else { | ||
+ | applyBrakes (); | ||
+ | } | ||
+ | |||
+ | // centralLineFollower(); | ||
+ | |||
+ | //delay(1); | ||
+ | |||
+ | } | ||
+ | |||
+ | void updateMotion(int sp, int d){ | ||
+ | switch ( state ) { | ||
+ | case 0: | ||
+ | if (prevState == 1) | ||
+ | //veerLeft(sp, d); | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 10) | ||
+ | //veerRight(sp, d); | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | case 1: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 10: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 11: | ||
+ | // Code | ||
+ | if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | } | ||
+ | |||
+ | } | ||
+ | |||
+ | void leftLineFollower(){ | ||
+ | //goForward(n_speed); | ||
+ | //applyBrakes (); | ||
+ | //veerRight(n_speed, dif); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | |||
+ | } | ||
+ | void centralLineFollower(){ | ||
+ | |||
+ | } | ||
+ | |||
+ | void goForward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void goBackward (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateRight (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,LOW); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void rotateLeft (int v) | ||
+ | { | ||
+ | digitalWrite (AIN1,LOW); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v); | ||
+ | } | ||
+ | |||
+ | void veerLeft (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v -d); | ||
+ | } | ||
+ | |||
+ | void veerRight (int v, int d) | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,LOW); | ||
+ | analogWrite(PWMA,v -d); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,LOW); | ||
+ | analogWrite(PWMB,v ); | ||
+ | } | ||
+ | |||
+ | void applyBrakes () | ||
+ | { | ||
+ | digitalWrite (AIN1,HIGH); | ||
+ | digitalWrite (AIN2,HIGH); | ||
+ | analogWrite(PWMA,255); | ||
+ | digitalWrite (BIN1,HIGH); | ||
+ | digitalWrite (BIN2,HIGH); | ||
+ | analogWrite(PWMB,255); | ||
+ | } | ||
+ | |||
+ | void startUp () | ||
+ | { | ||
+ | digitalWrite(STBY,HIGH); | ||
+ | } | ||
+ | |||
+ | |||
+ | |||
+ | void shutDown () | ||
+ | { | ||
+ | digitalWrite(STBY,LOW); | ||
+ | } | ||
- | + | /* | |
- | if (lSState == 0 && rSState == 0){ | + | В библиотеке Ping.cpp |
- | goForward(n_speed); | + | вместо строки |
+ | _duration = pulseIn(_pin, HIGH); | ||
+ | следует использовать | ||
+ | _duration = pulseIn(_pin, HIGH, 40000); | ||
+ | */ | ||
+ | |||
+ | </code> | ||
+ | |||
+ | ==== Вариант 2.2. Простейший релейный регулятор движения по линии + ультразвуковой датчик препятствия (боле-менее работает :-) ==== | ||
+ | |||
+ | |||
+ | <code C++> | ||
+ | #include <Ping.h> | ||
+ | |||
+ | /* | ||
+ | |||
+ | */ | ||
+ | #define PWMA 11 // выходы arduino | ||
+ | #define PWMB 10 | ||
+ | #define AIN1 6 | ||
+ | #define AIN2 7 | ||
+ | #define BIN1 5 | ||
+ | #define BIN2 4 | ||
+ | #define STBY 13 | ||
+ | |||
+ | #define lS 9 | ||
+ | #define rS 8 | ||
+ | |||
+ | int distance = 10; | ||
+ | int ping_distance = 400; | ||
+ | |||
+ | Ping ping = Ping(2); | ||
+ | |||
+ | uint32_t previousMillis = 0; | ||
+ | uint16_t timePeriod = 50; | ||
+ | |||
+ | |||
+ | int min_speed = 120; | ||
+ | int max_speed = 255; | ||
+ | int n_speed = 200; | ||
+ | int dif = 200; | ||
+ | |||
+ | uint8_t lSState; | ||
+ | uint8_t rSState; | ||
+ | |||
+ | int state; | ||
+ | int prevState; | ||
+ | int readyState = 0; | ||
+ | |||
+ | int is_obstacle = 0; | ||
+ | |||
+ | void updateState(int st){ | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | void resetState(int st){ | ||
+ | prevState = state; | ||
+ | state = st; | ||
+ | } | ||
+ | |||
+ | |||
+ | //int state; | ||
+ | void setup() { | ||
+ | |||
+ | /* Настроить все 7 выводов на выходы, идущие к драйверу TB6612FNG */ | ||
+ | pinMode(PWMA,OUTPUT); | ||
+ | pinMode(AIN1,OUTPUT); | ||
+ | pinMode(AIN2,OUTPUT); | ||
+ | pinMode(PWMB,OUTPUT); | ||
+ | pinMode(BIN1,OUTPUT); | ||
+ | pinMode(BIN2,OUTPUT); | ||
+ | pinMode(lS,INPUT); | ||
+ | pinMode(rS,INPUT); | ||
+ | Serial.begin(9600); | ||
+ | while (readyState == 0) { | ||
+ | testSensors(); | ||
+ | if (prevState != 11) | ||
+ | readyState = 1; | ||
+ | delay(1000); | ||
+ | } | ||
+ | delay(2000); | ||
+ | startUp(); | ||
+ | } | ||
+ | /* | ||
+ | void checkTimer(){ | ||
+ | uint32_t currentMillis = millis(); | ||
+ | if (currentMillis - previousMillis >= timePeriod){ | ||
+ | Serial.println(millis()); | ||
+ | previousMillis = currentMillis; | ||
+ | } | ||
+ | } | ||
+ | */ | ||
+ | void testSensors(){ | ||
+ | lSState = digitalRead(lS); | ||
+ | rSState = digitalRead(rS); | ||
+ | //Serial.println(lSState); | ||
+ | //Serial.println(rSState); | ||
+ | //Serial.println(""); | ||
+ | |||
+ | if (lSState == 0 && rSState == 0 ){ | ||
+ | //resetState(0); | ||
+ | updateState(0); | ||
+ | //goForward(n_speed); | ||
} | } | ||
- | + | if (lSState == 1 && rSState == 0 ){ | |
- | if (lSState == 1 && rSState == 1){ | + | resetState(10); |
- | applyBrakes (); | + | //goForward(n_speed); |
} | } | ||
- | if (lSState == 1 && rSState == 0){ | + | // if (current_dist <= dangerous_dist ){ |
- | veerRight(n_speed, dif); | + | // resetState(1); |
+ | //applyBrakes (); | ||
+ | // } | ||
+ | if (lSState == 1 && rSState == 1 ){ | ||
+ | //if (readyState == 0) | ||
+ | //resetState(11); | ||
+ | updateState(11); | ||
+ | |||
+ | //veerRight(n_speed, dif); | ||
} | } | ||
- | if (lSState == 0 && rSState == 1){ | + | if (lSState == 0 && rSState == 1 ){ |
- | veerLeft(n_speed, dif); | + | resetState(1); |
- | } | + | //veerLeft(n_speed, dif); |
- | + | } | |
- | delay(10); | + | |
- | | + | } |
+ | |||
+ | void ping_fire(){ | ||
+ | ping.fire(); | ||
+ | //Serial.println("***"); | ||
+ | } | ||
+ | |||
+ | void loop() { | ||
+ | // time ===================== | ||
+ | uint32_t currentMillis = millis(); | ||
+ | if (currentMillis - previousMillis >= timePeriod){ | ||
+ | //Serial.println(millis()); | ||
+ | //Serial.println(ping.centimeters()); | ||
+ | if (ping_distance > distance){ | ||
+ | is_obstacle = 0; | ||
+ | } else { | ||
+ | is_obstacle = 1; | ||
+ | } | ||
+ | ping_distance = ping.centimeters(); | ||
+ | ping_fire(); | ||
+ | previousMillis = currentMillis; | ||
} | } | ||
- | void testmove(){ | + | //======================= |
- | //startUp(); | + | |
- | //goForward(); | + | if (is_obstacle != 1){ |
- | //delay(1500); | + | testSensors(); |
- | //turnAround(); | + | updateMotion(n_speed, dif); |
- | //goForward(); | + | } else { |
- | //delay(1500); | + | applyBrakes (); |
- | //turnAround(); | + | } |
- | //goBackward(); | + | |
- | //delay(1500); | + | // centralLineFollower(); |
- | //rotateLeft(); | + | |
- | //delay(560); | + | //delay(1); |
- | //rotateRight(); | + | |
- | //delay(2240); | + | } |
- | // goForward(); | + | |
- | // delay(1000); | + | void updateMotion(int sp, int d){ |
- | //applyBrakes(); | + | switch ( state ) { |
- | // delay(2000); | + | case 0: |
- | | + | if (prevState == 1) |
+ | //veerLeft(sp, d); | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 10) | ||
+ | //veerRight(sp, d); | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | case 1: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 10: | ||
+ | // Code | ||
+ | goForward(sp); | ||
+ | break; | ||
+ | case 11: | ||
+ | // Code | ||
+ | if (prevState == 10) | ||
+ | veerRight(sp, d); | ||
+ | else if (prevState == 1) | ||
+ | veerLeft(sp, d); | ||
+ | break; | ||
+ | } | ||
+ | |||
+ | } | ||
+ | |||
+ | void leftLineFollower(){ | ||
+ | //goForward(n_speed); | ||
+ | //applyBrakes (); | ||
+ | //veerRight(n_speed, dif); | ||
+ | //veerLeft(n_speed, dif); | ||
+ | |||
+ | } | ||
+ | void centralLineFollower(){ | ||
+ | |||
} | } | ||
Строка 865: | Строка 2046: | ||
} | } | ||
- | //void turnAround() | + | |
- | //{ | + | |
- | // rotateLeft(); | + | |
- | // delay(1370); | + | |
- | //} | + | |
void shutDown () | void shutDown () | ||
Строка 876: | Строка 2053: | ||
} | } | ||
+ | |||
+ | /* | ||
+ | В библиотеке Ping.cpp | ||
+ | вместо строки | ||
+ | _duration = pulseIn(_pin, HIGH); | ||
+ | следует использовать | ||
+ | _duration = pulseIn(_pin, HIGH, 40000); | ||
+ | */ | ||
</code> | </code> | ||
+ | ==== Простой таймер (прерывание по таймеру) ==== | ||
+ | |||
+ | <code C++> | ||
+ | uint32_t previousMillis = 0; | ||
+ | uint16_t timePeriod = 1500; | ||
+ | |||
+ | void setup() { | ||
+ | // put your setup code here, to run once: | ||
+ | Serial.begin(9600); | ||
+ | } | ||
+ | |||
+ | void loop() { | ||
+ | uint32_t currentMillis = millis(); | ||
+ | if (currentMillis - previousMillis >= timePeriod){ | ||
+ | Serial.println(millis()); | ||
+ | previousMillis = currentMillis; | ||
+ | } | ||
+ | } | ||
+ | </code> | ||
+ | |||
==== Ультразвуковой датчик ==== | ==== Ультразвуковой датчик ==== | ||
* [[http://playground.arduino.cc/Code/Ping]] | * [[http://playground.arduino.cc/Code/Ping]] | ||
+ | * [[http://embeddedlab.csuohio.edu/RoboticSwarms/ultrasonic_sensors.html]] | ||
+ | * [[http://arduino.ru/forum/programmirovanie/nestabilnaya-rabota-uz-datchika]] - Форум: "Нестабильная работа УЗ датчика" | ||
+ | |||
+ | ==== Bluetooth модуль HC-06 ==== | ||
+ | |||
+ | * [[https://lesson.iarduino.ru/page/bluetooth-modul-hc-06-podklyuchenie-k-arduino-upravlenie-ustroystvami-s-telefona]] | ||
+ | * [[https://arduinomaster.ru/datchiki-arduino/arduino-bluetooth-hc05-hc06/]] | ||
+ | * [[http://zelectro.cc/HC-06_bluetooth_module]] | ||
+ | * [[http://psenyukov.ru/%D0%BF%D0%BE%D0%B4%D0%BA%D0%BB%D1%8E%D1%87%D0%B5%D0%BD%D0%B8%D0%B5-bluetooth-hc-06-%D0%BA-arduino-%D1%81-%D0%BF%D1%80%D0%BE%D1%88%D0%B8%D0%B2%D0%BA%D0%BE%D0%B9-grbl/]] | ||
+ | * [[https://play.google.com/store/apps/details?id=braulio.calle.bluetoothRCcontroller&hl=ru]] - Google Play bluetooth-rc-car | ||
+ | * [[https://create.arduino.cc/projecthub/samanfern/bluetooth-controlled-car-d5d9ca]] - Arduino Bluetooth Controlled Car | ||
+ | * [[https://tutorial45.com/arduino-bluetooth-rc-car-project/]] - Arduino Bluetooth Controlled Car | ||
+ | * [[http://www.ardumotive.com/bluetooth-rc-car.html]] - Arduino Bluetooth Controlled Car | ||
+ | * | ||
+ | ==== Указатели в C для Ардуино ==== | ||
+ | |||
+ | * [[http://mypractic.ru/urok-15-ukazateli-v-c-dlya-arduino-preobrazovanie-raznyx-tipov-dannyx-v-bajty.html]] | ||
+ | |||
==== Принципы программирования оптических энкодеров ==== | ==== Принципы программирования оптических энкодеров ==== | ||
Строка 901: | Строка 2124: | ||
* [[http://robot-kit.ru/manual/Motor_Wheel_Encoder_Sensor.pdf]] Карпов В.Э. Управление движением роботов с использованием энкодеров | * [[http://robot-kit.ru/manual/Motor_Wheel_Encoder_Sensor.pdf]] Карпов В.Э. Управление движением роботов с использованием энкодеров | ||
* http://easyelectronics.ru/avr-uchebnyj-kurs-inkrementalnyj-enkoder.html | * http://easyelectronics.ru/avr-uchebnyj-kurs-inkrementalnyj-enkoder.html | ||
+ | * [[http://robofob.ru/materials/articles/pages/avprog.pdf]] Карпов В.Э. Автоматное программирование и робототехника | ||
+ | |||
+ | ==== Прерывания по таймеру ==== | ||
+ | |||
+ | * [[https://playground.arduino.cc/Code/Timer1/]] :: Timer1 :: !!! | ||
+ | * [[https://code.google.com/archive/p/arduino-timerone/downloads]] | ||
+ | * [[https://github.com/PaulStoffregen/TimerOne]] | ||
+ | * [[https://wm-help.net/lib/b/book/1248084587/6]] | ||
+ | * [[https://habr.com/ru/post/337430/]] !!! | ||
+ | * [[http://arduino.ru/forum/programmirovanie/preryvaniya-po-taimeru]] | ||
+ | * [[https://all-arduino.ru/arduino-dlya-nachinayushhih-urok-14-preryvaniya/]] | ||
+ | * [[http://robotosha.ru/arduino/multitasking-and-interrupts-arduino.html]] | ||
==== О прерываниях ==== | ==== О прерываниях ==== |