-
Notifications
You must be signed in to change notification settings - Fork 0
/
part_1.ino
68 lines (60 loc) · 1.49 KB
/
part_1.ino
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
int DIRECTIONA = 4;
int MOTORA = 5;
int DIRECTIONB = 7;
int MOTORB = 6;
void setup ()
{
pinMode(MOTORA, OUTPUT);
pinMode(DIRECTIONA, OUTPUT);
pinMode(MOTORB, OUTPUT);
pinMode(DIRECTIONB, OUTPUT);
}
int phase = 0;
int timeToGo;
unsigned long startTime;
void loop ()
{
// Defining speed 120.
analogWrite(MOTORA, 120);
analogWrite(MOTORB, 120);
// Moving to some direction with some speed, which were defined previously, until timeout.
startTime = millis(); while (millis() - startTime < timeToGo)
{
// Check current drain (consumption).
// If current is too high - break.
if (analogRead(A0) > 325) // > 1.46 amps
break;
}
// Move forwards.
if (phase == 0)
{
digitalWrite(DIRECTIONA, 1);
digitalWrite(DIRECTIONB, 1);
timeToGo = 1500; phase = 1; }
// Move backwards.
else if (phase == 1)
{
digitalWrite(DIRECTIONA, 0);
digitalWrite(DIRECTIONB, 0);
timeToGo = 1500; phase = 2;
}
// Move left.
else if (phase == 2)
{
digitalWrite(DIRECTIONA, 1);
digitalWrite(DIRECTIONB, 0);
timeToGo = 2200; phase = 3;
}
// Move right.
else
{
digitalWrite(DIRECTIONA, 0);
digitalWrite(DIRECTIONB, 1);
timeToGo = 2200;
phase = 0;
}
// Defining speed 0.
analogWrite(MOTORA, 0);
analogWrite(MOTORB, 0);
}