-
Notifications
You must be signed in to change notification settings - Fork 1
/
21_STEPER motor.cpp
82 lines (73 loc) · 1.61 KB
/
21_STEPER motor.cpp
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
#include <Arduino.h>
#define X_os A0
#define Y_os A1
byte servo[] = {2,3,4,5};
int hitrost = 1000;
void korak(bool a, bool b, bool c, bool d){
digitalWrite(servo[0], a);
digitalWrite(servo[1], b);
digitalWrite(servo[2], c);
digitalWrite(servo[3], d);
}
void naprej(){
korak(1,0,0,0);
delayMicroseconds(hitrost);
korak(1,1,0,0);
delayMicroseconds(hitrost);
korak(0,1,0,0);
delayMicroseconds(hitrost);
korak(0,1,1,0);
delayMicroseconds(hitrost);
korak(0,0,1,0);
delayMicroseconds(hitrost);
korak(0,0,1,1);
delayMicroseconds(hitrost);
korak(0,0,0,1);
delayMicroseconds(hitrost);
korak(1,0,0,1);
delayMicroseconds(hitrost);
}
void nazaj(){
korak(0,0,0,1);
delayMicroseconds(hitrost);
korak(0,0,1,1);
delayMicroseconds(hitrost);
korak(0,0,1,0);
delayMicroseconds(hitrost);
korak(0,1,1,0);
delayMicroseconds(hitrost);
korak(0,1,0,0);
delayMicroseconds(hitrost);
korak(1,1,0,0);
delayMicroseconds(hitrost);
korak(1,0,0,0);
delayMicroseconds(hitrost);
korak(1,0,0,1);
delayMicroseconds(hitrost);
}
void setup() {
for(int i = 0; i < 4; i++){
pinMode(servo[i], OUTPUT);
digitalWrite(servo[i], LOW);
}
Serial.begin(9600);
}
void loop() {
int v_x = analogRead(X_os);
int v_y = analogRead(Y_os);
Serial.print("X: ");
Serial.print(v_x);
Serial.print(", Y: ");
Serial.println(v_y);
//delay(100);
// SMER PREMIKANJA
if(v_x > 750){
naprej();
}
else if(v_x < 250){
nazaj();
}
else {
korak(0,0,0,0);
}
}