-
Notifications
You must be signed in to change notification settings - Fork 0
/
motor_driver.ino
94 lines (74 loc) · 1.47 KB
/
motor_driver.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
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
int enA = 10;
int enB = 11;
int in1 = 9;
int in2 = 8;
int in3 = 4;
int in4 = 6;
const int pinIRd = 7;
const int pinIRa = A0;
const int pinLED = 13;
int IRvalueA = 0;
int IRvalueD = 0;
void setup(){
Serial.begin(9600);
pinMode(pinIRd,INPUT);
pinMode(pinIRa,INPUT);
pinMode(pinLED,OUTPUT);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
}
void loop(){
Serial.print("Analog Reading=");
Serial.print(IRvalueA);
Serial.print("\t Digital Reading=");
Serial.println(IRvalueD);
if (IRvalueD == LOW) {
digitalWrite(LED_BUILTIN, HIGH);
MotorControl(1,1,255);
}
else {
digitalWrite(LED_BUILTIN, LOW);
MotorControl(1,-1,255);
}
delay(500);
IRvalueA = analogRead(pinIRa);
IRvalueD = digitalRead(pinIRd);
}
void MotorControl(int Mname, int rotation, int spd){
int pwm;
int pA;
int pB;
// 1 = left motor
// 2 = right motor
if (Mname == 1){
pwm = enA;
pA = in1;
pB = in2;
}else if (Mname == 2){
pwm = enB;
pA = in3;
pB = in4;
}else{
return;
}
/*
* -1 = backword
* 0 = stop
* 1 = forword
*/
if (rotation == 0){
digitalWrite (pA, LOW);
digitalWrite (pB, LOW);
}else if (rotation == 1){
digitalWrite (pA, HIGH);
digitalWrite (pB, LOW);
}else if (rotation == -1){
digitalWrite (pA, LOW);
digitalWrite (pB, HIGH);
}
analogWrite (pwm, spd);
}