-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathstepperMotor.c
91 lines (78 loc) · 1.83 KB
/
stepperMotor.c
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
#include <stdio.h>
#include <unistd.h>
#include "mraa/aio.h"
#include <time.h>
#define P_IN1 8
#define P_IN2 9
#define P_IN3 10
#define P_IN4 11
#define MAX_DISPLACEMENT 3000
#define MIN_DISPLACEMENT -3000
int displacement = 0;
int currentStep = 0;
void nextStep(int direction, mraa_gpio_context p_in1, mraa_gpio_context p_in2, mraa_gpio_context p_in3, mraa_gpio_context p_in4){
if (direction > 0){
if(displacement >= MAX_DISPLACEMENT){ return;}
}
else{
if(displacement <= MIN_DISPLACEMENT){ return;}
}
if(direction > 0){
currentStep = (currentStep+1)%4;
displacement++;
}else{
currentStep = currentStep - 1;
if (currentStep < 0){
currentStep = 3;
}
displacement = displacement -1;
}
switch(currentStep){
case 0:
mraa_gpio_write(p_in1, 1);
mraa_gpio_write(p_in2, 0);
mraa_gpio_write(p_in3, 0);
mraa_gpio_write(p_in4, 0);
break;
case 1:
mraa_gpio_write(p_in1, 0);
mraa_gpio_write(p_in2, 1);
mraa_gpio_write(p_in3, 0);
mraa_gpio_write(p_in4, 0);
break;
case 2:
mraa_gpio_write(p_in1, 0);
mraa_gpio_write(p_in2, 0);
mraa_gpio_write(p_in3, 1);
mraa_gpio_write(p_in4, 0);
break;
case 3:
mraa_gpio_write(p_in1, 0);
mraa_gpio_write(p_in2, 0);
mraa_gpio_write(p_in3, 0);
mraa_gpio_write(p_in4, 1);
break;
}
}
int main(int argc, char *argv[]){
mraa_gpio_context p_in1;
mraa_gpio_context p_in2;
mraa_gpio_context p_in3;
mraa_gpio_context p_in4;
p_in1 = mraa_gpio_init(P_IN1);
mraa_gpio_dir(p_in1, MRAA_GPIO_OUT);
p_in2 = mraa_gpio_init(P_IN2);
mraa_gpio_dir(p_in2, MRAA_GPIO_OUT);
p_in3 = mraa_gpio_init(P_IN3);
mraa_gpio_dir(p_in3, MRAA_GPIO_OUT);
p_in4 = mraa_gpio_init(P_IN4);
mraa_gpio_dir(p_in4, MRAA_GPIO_OUT);
int i = 0, dir = 1;
if(argc > 1 ){
dir = argv[1];
}
for(i=0; i<85; i++){
nextStep(dir, p_in1, p_in2, p_in3, p_in4);
usleep(25000);
}
}