Различия

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

--- doc:1308:643.mgul.13080-02_12_01 [2017/02/22 21:37]
imbalanceone created
+++ doc:1308:643.mgul.13080-02_12_01 [2018/04/28 23:47] (текущий)
@@ Строка 1: / Строка 1: @@
+===Аннотация===
+программа control_motor управляет моторами а также web камерой подвижной платформы в зависимости от принятых управляющих сигналов с raspberry.
+<hidden><code c>
+#include <Servo.h>
+#include "Wire.h"
+#include "I2Cdev.h"
+#include "servo.h"
+#define rightIR 11
+#define rearIR 5
+#define frontIR 3
+#define int1 28          // Направление вращения двигателя 1
+#define int2 26          // Направление вращения двигателя 1
+#define int3 24          // Направление вращения двигателя 2
+#define int4 22          // Направление вращения двигателя 2
+#define sp1  8
+#define sp2  7
+#define KursPin  10
+#define TungPin  9
+#define ForwardButton 'F' //0x5F12E8C4    //up button
+#define BackButton 'B'    //0x189D7928    //down button
+#define RightButton 'R'   //0x68733A46    //right button
+#define LeftButton 'L'    //0x83B19366    //left button
+#define LeftSpeedUpButton 'D'    //plus button
+#define LeftSpeedDownButton 'E'  //minus button
+#define StopProgramm 'H'
+#define StartProgButton 'G'//0x89964248  //start button
+#define StopProgButton 'S'//0xE13DDA28   //stop button
+#define RightForwardButton 'C'
+#define LeftForwardButton 'A'
+#define RootPermission 'I'
+#define RootExit 'J'
+#define RightSpeedUpButton 'K'
+#define RightSpeedDownButton 'M'
+#define LostConnection 'N'
+#define SKursLeft 'T'
+#define SKursRight 'O'
+#define STungRight 'P'
+#define STungLeft 'Q'
+#define defaultpos 'U'
+#define EchoPin 4
+#define TrigPin 2
+#define ChangeSpeed 5
+#define ChangeAngle 5
+int save = 0;
+int speed1 = 255;
+int speed2 = 255;
+int rootflag = 0;
+int tungpos = 90;
+int kurspos = 90;
+int KursMaxAngle = 85;
+int TungMaxAngle = 35;
+Servo servokurs; //объявляем переменную  Servo
+Servo servotung;
+void forward()
+{
+  analogWrite(sp1, speed1);
+  analogWrite(sp2, speed2);
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("forward\n");
+  digitalWrite(int1, LOW);
+  digitalWrite(int2, HIGH);
+  digitalWrite(int3, HIGH);
+  digitalWrite(int4, LOW);
+  return;
+}
+void back()
+{
+  analogWrite(sp1, speed1);
+  analogWrite(sp2, speed2);
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("back\n");
+  digitalWrite(int1, HIGH);
+  digitalWrite(int2, LOW);
+  digitalWrite(int3, LOW);
+  digitalWrite(int4, HIGH);
+  return;
+}
+void right()
+{
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("right\n");
+  analogWrite(sp1, speed1);
+  analogWrite(sp2, speed2);
+  digitalWrite(int1, LOW);
+  digitalWrite(int2, HIGH);
+  digitalWrite(int3, LOW);
+  digitalWrite(int4, HIGH);
+  return;
+}
+void left()
+{
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("left\n");
+  analogWrite(sp1, speed1);
+  analogWrite(sp2, speed2);
+  digitalWrite(int1, HIGH);
+  digitalWrite(int2, LOW);
+  digitalWrite(int3, HIGH);
+  digitalWrite(int4, LOW);
+  return;
+}
+void stop1()
+{
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("stop\n");
+  digitalWrite(int1, LOW);
+  digitalWrite(int2, LOW);
+  digitalWrite(int3, LOW);
+  digitalWrite(int4, LOW);
+  return;
+}
+void rightforward()
+{
+  analogWrite(sp1, speed1 - 150);
+  analogWrite(sp2, speed2);
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("rightforward\n");
+  digitalWrite(int1, LOW);
+  digitalWrite(int2, HIGH);
+  digitalWrite(int3, HIGH);
+  digitalWrite(int4, LOW);
+  return;
+}
+void leftforward()
+{
+  analogWrite(sp1, speed1);
+  analogWrite(sp2, speed1 - 150);
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("leftworward\n");
+  digitalWrite(int1, LOW);
+  digitalWrite(int2, HIGH);
+  digitalWrite(int3, HIGH);
+  digitalWrite(int4, LOW);
+  return;
+}
+void servokursright()
+{
+  Serial.println("kursright");
+  if ( kurspos < 90 + KursMaxAngle)
+  {
+    kurspos -= ChangeAngle;
+  }
+  servokurs.write(kurspos);
+  Serial.println(kurspos);
+}
+void servokursleft()
+{
+  Serial.println("kursleft");
+  if ( kurspos > 90 - KursMaxAngle)
+  {
+    kurspos += ChangeAngle;
+  }
+  servokurs.write(kurspos);
+  Serial.println(kurspos);
+}
+void servotungright()
+{
+  Serial.println("tungright");
+  if ( tungpos < 90 + TungMaxAngle)
+  {
+    tungpos += ChangeAngle;
+  }
+  servotung.write(tungpos);
+  Serial.println(tungpos);
+}
+void servotungleft()
+{
+  Serial.println("tungleft");
+  if ( tungpos > 90 - TungMaxAngle)
+  {
+    tungpos -= ChangeAngle;
+  }
+  servotung.write(tungpos);
+  Serial.println(tungpos);
+}
+void DefaultPosition ()
+{
+  Serial.println("defpos");
+  servotung.write(90);
+  servokurs.write(90);
+  return;
+}
+void porog()
+{
+  int fIR;
+  int rIR;
+  fIR = digitalRead(frontIR);
+  rIR = digitalRead(rearIR);
+  if ( fIR == 1 || rIR == 1)
+  {
+    if ( fIR == 1 )
+    {
+      Serial.print(millis());
+      Serial.print("\t");
+      Serial.print("danger in front use left right  or root\n");
+      back();
+      delay(300);
+      stop1();
+    }
+    if ( rIR == 1 )
+    {
+      Serial.print(millis());
+      Serial.print("\t");
+      Serial.print("danger on back use left right  or root\n");
+      forward();
+      delay(300);
+      stop1();
+    }
+    while (1)
+    {
+      if (Serial.available())
+      {
+        int res;
+        res = Serial.read();
+        if (res == RootPermission)
+        {
+          root();
+          return;
+        }
+        switch (res)
+        {
+          case RightButton:
+            right();
+            return;
+            break;
+          case LeftButton:
+            left();
+            return;
+            break;
+          case SKursRight:
+            servokursright();
+            break;
+          case SKursLeft:
+            servokursleft();
+            break;
+          case STungRight:
+            servotungright();
+            break;
+          case STungLeft:
+            servotungleft();
+            break;
+          case defaultpos:
+            DefaultPosition ();
+            break;
+        }
+      }
+    }
+  }
+}
+void KeySelect (int res)
+{
+  switch (res)
+  {
+    case ForwardButton:
+      forward();
+      break;
+    case BackButton:
+      back();
+      break;
+    case RightButton:
+      right();
+      break;
+    case LeftButton:
+      left();
+      break;
+    case StopProgButton:
+      stop1();
+      break;
+    case RightForwardButton:
+      rightforward();
+      break;
+    case LeftForwardButton:
+      leftforward();
+      break;
+    case LostConnection:
+      connection();
+      break;
+    case SKursRight:
+      servokursright();
+      break;
+    case SKursLeft:
+      servokursleft();
+      break;
+    case STungRight:
+      servotungright();
+      break;
+    case STungLeft:
+      servotungleft();
+      break;
+    case defaultpos:
+      DefaultPosition ();
+      break;
+    case LeftSpeedUpButton:
+      if (speed1 < 254)
+      {
+        speed1 += ChangeSpeed;
+        Serial.print(millis());
+        Serial.print("\t");
+        Serial.println(speed1);
+      }
+      break;
+    case LeftSpeedDownButton:
+      if (speed1 > 100)
+      {
+        speed1 -= ChangeSpeed;
+        Serial.print(millis());
+        Serial.print("\t");
+        Serial.println(speed1);
+      }
+      break;
+    case RightSpeedUpButton:
+      if (speed1 < 254)
+      {
+        speed2 += ChangeSpeed;
+        Serial.print(millis());
+        Serial.print("\t");
+        Serial.println(speed2);
+      }
+      break;
+    case RightSpeedDownButton:
+      if (speed1 > 100)
+      {
+        speed2 -= ChangeSpeed;
+        Serial.print(millis());
+        Serial.print("\t");
+        Serial.println(speed2);
+      }
+      break;
+  }
+  return;
+}
+void root()
+{
+  save = 0;
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("root available\n");
+  while (1)
+  {
+    int res;
+    res = Serial.read();
+    if (res == RootExit)
+    {
+      Serial.print(millis());
+      Serial.print("\t");
+      Serial.print ("root disable\n");
+      break;
+    }
+    KeySelect(res);
+  }
+  return;
+}
+void connection ()
+{
+  stop1();
+  Serial.print(millis());
+  Serial.print("\t");
+  Serial.print("lost_connection\n");
+  digitalWrite(39, HIGH);
+  delay (2000);
+  digitalWrite(39, LOW);
+}
+long getEchoTiming()
+{
+  digitalWrite(TrigPin, LOW);
+  delayMicroseconds(2);
+  digitalWrite(TrigPin, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(TrigPin, LOW);
+  long duration = pulseIn(EchoPin, HIGH);
+  return duration;
+}
+void setup()
+{
+  pinMode(frontIR, INPUT);
+  Serial.begin(9600); // Выставляем скорость COM порта
+  Wire.begin();
+  servokurs.attach(KursPin);
+  servotung.attach(TungPin);
+  servokurs.write(kurspos);
+  servotung.write(tungpos);
+  pinMode(TrigPin, OUTPUT);
+  pinMode(EchoPin, INPUT);
+  pinMode(int1, OUTPUT);
+  pinMode(int2, OUTPUT);
+  pinMode(int3, OUTPUT);
+  pinMode(int4, OUTPUT);
+  pinMode(sp1, OUTPUT);
+  pinMode(sp2, OUTPUT);
+  pinMode(rightIR, INPUT);
+  pinMode(rearIR, INPUT);
+}
+void loop()
+{
+  if (Serial.available())
+  {
+    int res;
+    res = Serial.read();
+    if (res == RootPermission)
+    {
+      root();
+    }
+    KeySelect(res);
+  }
+  porog();
+}
+</code>
+</hidden>
+ [[doc:1308:643.MGUL.13080-02_13_01|описание программы для работы теле-управляемой подвижной платформой]]

Robo wiki

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

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

Различия

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