Grass-Designer_Arduino/Motor_Test at master · NCU-ARC/Grass-Designer_Arduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
/****************************************************************************

* Copyright(C) : National Central University ROD Lab Jen Hao Chen (Howard)
* Create Date  : 2014/07/28 by Howard Chen
* Modified Date: 2014/09/25 by Howard Chen
* Abstract     : This program will make 4 motors move. Forward for 1 sec and stop for 0.5 sec.
                    and backward, turn right and turn left. The time intervals are the same.
* Reference    : None

/*****************************************************************************/
const int motorIn1 = 5;
const int motorIn2 = 6;
const int motorIn3 = 9;
const int motorIn4 = 10;
const int DELAY = 1000;

void setup()
{
    pinMode(motorIn1, OUTPUT);
    pinMode(motorIn2, OUTPUT);
    pinMode(motorIn3, OUTPUT);
    pinMode(motorIn4, OUTPUT);
}

void loop()
{
  selfTest();
}


void selfTest()
{
    forward();
    delay(DELAY);
    motorstop();
    delay(500);

    backward();
    delay(DELAY);
    motorstop();
    delay(500);

    right();
    delay(DELAY);
    motorstop();
    delay(500);

    left();
    delay(DELAY);
    motorstop();
    delay(500);

    motorstop();
    delay(3000);
}

void motorstop()
{
  digitalWrite(motorIn1, LOW);
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, LOW);
  digitalWrite(motorIn4, LOW);
}

void forward()
{
  digitalWrite(motorIn1, HIGH );
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, HIGH);
  digitalWrite(motorIn4, LOW);
}

void backward()
{
  digitalWrite(motorIn1, LOW);
  digitalWrite(motorIn2, HIGH);
  digitalWrite(motorIn3, LOW);
  digitalWrite(motorIn4, HIGH);
}

// Let right motor keep running, but stop left motor
void right()
{
  digitalWrite(motorIn1, HIGH);
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, LOW);
  digitalWrite(motorIn4, LOW);
}

// Let left motor keep running, but stop right motor
void left()
{
  digitalWrite(motorIn1, LOW);
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, HIGH);
  digitalWrite(motorIn4, LOW);
}