Различия

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

--- 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>
+==== Реализация машины состояний при движении по линии (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. Простейший релейный регулятор движения по линии + ультразвуковой датчик препятствия (промежуточный вариант, требует доработки) ====

INTEPRA WIKI

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

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

Различия

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