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

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


kiber_tc_2018

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

Предыдущая версия справа и слева Предыдущая версия
kiber_tc_2018 [2019/06/24 20:29]
127.0.0.1 внешнее изменение
kiber_tc_2018 [2019/11/20 23:06] (текущий)
super_admin [Реализация машины состояний при движении по линии]
Строка 1285: Строка 1285:
 </​code>​ </​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 ()
 +{
 +  digitalWrite(STBY,​LOW);​
 +}
 +
 +</​code>​
  
 ==== Вариант 2.1. Простейший релейный регулятор движения по линии + ультразвуковой датчик препятствия (промежуточный вариант,​ требует доработки) ==== ==== Вариант 2.1. Простейший релейный регулятор движения по линии + ультразвуковой датчик препятствия (промежуточный вариант,​ требует доработки) ====
kiber_tc_2018.txt · Последние изменения: 2019/11/20 23:06 — super_admin