-
Notifications
You must be signed in to change notification settings - Fork 0
/
DCmotor.c
132 lines (105 loc) · 2.31 KB
/
DCmotor.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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#include "DCmotor.h"
// M1 left
// M2 right
void motorInit(){
// Keep pins low if unused.
set_outputs(M1reverse, M1enable, 0b0000);
set_outputs(M2reverse, M2enable, 0b0000);
set_directions(M1reverse, M1enable, 0b1111);
set_directions(M2reverse, M2enable, 0b1111);
}
void motorTest()
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
// Turn motors counterclockwise for 3 s.
high(M1forward);
high(M2forward);
pwm_set(M1enable, 0, 1000);
pwm_set(M2enable, 1, 1000);
pause(2000);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
void moveMotorT(int left, int right, int time)
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
if(left >=0){
high(M1forward);
} else {
high(M1reverse);
left=abs(left);
}
if(right >=0){
high(M2forward);
} else {
high(M2reverse);
right=abs(right);
}
pwm_set(M1enable, 0, left);
pwm_set(M2enable, 1, right);
pause(time);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
void moveDistance(int distance)
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
// Turn motors counterclockwise for 3 s.
if(distance >=0){
high(M1forward);
high(M2forward);
} else {
high(M1reverse);
high(M2reverse);
distance=abs(distance);
}
pwm_set(M1enable, 0, 1000);
pwm_set(M2enable, 1, 1000);
pause(distance);
// Stop again
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}
void moveMotors(int left, int right)
{
motorInit();
// Start PWM process. Period 1 ms, Freq 1 kHz
pwm_start(basepwm);
if(left >=0){
high(M1forward);
} else {
high(M1reverse);
left=abs(left);
}
if(right >=0){
high(M2forward);
} else {
high(M2reverse);
right=abs(right);
}
pwm_set(M1enable, 0, left);
pwm_set(M2enable, 1, right);
}
void stopMotors()
{
motorInit();
// Stop
pwm_set(M1enable, 0, 0);
pwm_set(M2enable, 1, 0);
// End the PWM process
pwm_stop();
}