Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия Следующая версия Следующая версия справа и слева | ||
kiber_tc_2018 [2019/04/01 20:29] super_admin [Ультразвуковой датчик] |
kiber_tc_2018 [2019/04/09 20:18] super_admin [Вариант 1.1. Простейший релейный регулятор движения по линии] |
||
---|---|---|---|
Строка 509: | Строка 509: | ||
</code> | </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> | ||
==== Управление питанием посредством кнопки ==== | ==== Управление питанием посредством кнопки ==== | ||
Строка 764: | Строка 975: | ||
} | } | ||
- | void testmove(){ | + | |
- | //startUp(); | + | |
- | //goForward(); | + | void goForward (int v) |
- | //delay(1500); | + | { |
- | //turnAround(); | + | digitalWrite (AIN1,HIGH); |
- | //goForward(); | + | digitalWrite (AIN2,LOW); |
- | //delay(1500); | + | analogWrite(PWMA,v); |
- | //turnAround(); | + | digitalWrite (BIN1,HIGH); |
- | //goBackward(); | + | digitalWrite (BIN2,LOW); |
- | //delay(1500); | + | analogWrite(PWMB,v); |
- | //rotateLeft(); | + | } |
- | //delay(560); | + | |
- | //rotateRight(); | + | void goBackward (int v) |
- | //delay(2240); | + | { |
- | // goForward(); | + | digitalWrite (AIN1,LOW); |
- | // delay(1000); | + | digitalWrite (AIN2,HIGH); |
- | //applyBrakes(); | + | analogWrite(PWMA,v); |
- | // delay(2000); | + | 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() { | ||
+ | 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(){ | ||
} | } | ||
Строка 860: | Строка 1276: | ||
} | } | ||
- | //void turnAround() | + | |
- | //{ | + | |
- | // rotateLeft(); | + | |
- | // delay(1370); | + | |
- | //} | + | |
void shutDown () | void shutDown () |