12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
- void accelerate (int len, int tar_speed)
15
+ RUN g_run;
16
+
17
+ RUN::RUN ()
18
+ {
19
+ speed = 0.0 ;
20
+ accel = 0.0 ;
21
+ }
22
+
23
+ // 割り込み
24
+ void controlInterrupt (void ) { g_run.interrupt (); }
25
+
26
+ void RUN::interrupt (void )
27
+ { // 割り込み内からコール
28
+ speed += accel;
29
+
30
+ if (speed > max_speed) {
31
+ speed = max_speed;
32
+ }
33
+ if (speed < min_speed) {
34
+ speed = min_speed;
35
+ }
36
+
37
+ speedSet (speed, speed);
38
+ }
39
+
40
+ void RUN::dirSet (t_CW_CCW dir_left, t_CW_CCW dir_right)
41
+ {
42
+ if (dir_left == MOT_FORWARD) {
43
+ digitalWrite (CW_L, LOW);
44
+ } else {
45
+ digitalWrite (CW_L, HIGH);
46
+ }
47
+ if (dir_right == MOT_FORWARD) {
48
+ digitalWrite (CW_R, HIGH);
49
+ } else {
50
+ digitalWrite (CW_R, LOW);
51
+ }
52
+ }
53
+
54
+ void RUN::counterClear (void ) { g_step_r = g_step_l = 0 ; }
55
+
56
+ void RUN::speedSet (double l_speed, double r_speed)
57
+ {
58
+ g_step_hz_l = (int )(l_speed / PULSE);
59
+ g_step_hz_r = (int )(r_speed / PULSE);
60
+ }
61
+
62
+ void RUN::stepGet (void )
63
+ {
64
+ step_lr = g_step_r + g_step_l;
65
+ step_lr_len = (int )((float )step_lr / 2.0 * PULSE);
66
+ }
67
+
68
+ void RUN::stop (void ) { g_motor_move = 0 ; }
69
+
70
+ void RUN::accelerate (int len, int finish_speed)
16
71
{
17
72
int obj_step;
18
- g_max_speed = tar_speed;
19
- g_accel = 1.5 ;
20
- g_step_r = g_step_l = 0 ;
21
- g_speed = g_min_speed = MIN_SPEED;
22
- g_step_hz_r = g_step_hz_l = (unsigned short )(g_speed / PULSE);
23
73
74
+ accel = 1.5 ;
75
+ speed = min_speed = MIN_SPEED;
76
+ max_speed = finish_speed;
77
+ counterClear ();
78
+ speedSet (speed, speed);
79
+ dirSet (MOT_FORWARD, MOT_FORWARD);
24
80
obj_step = (int )((float )len * 2.0 / PULSE);
25
- digitalWrite (CW_R, HIGH);
26
- digitalWrite (CW_L, LOW);
27
81
g_motor_move = 1 ;
28
82
29
- while ((g_step_r + g_step_l) < obj_step) {
30
- continue ;
83
+ while (1 ) {
84
+ stepGet ();
85
+ if (step_lr > obj_step) {
86
+ break ;
87
+ }
31
88
}
32
89
}
33
90
34
- void oneStep (int len, int tar_speed )
91
+ void RUN:: oneStep (int len, int init_speed )
35
92
{
36
93
int obj_step;
37
- g_max_speed = tar_speed;
38
- g_accel = 0.0 ;
39
- g_step_r = g_step_l = 0 ;
40
- g_speed = g_min_speed = tar_speed;
41
- g_step_hz_r = g_step_hz_l = (unsigned short )(g_speed / PULSE);
94
+
95
+ accel = 0.0 ;
96
+ max_speed = init_speed;
97
+ speed = min_speed = init_speed;
98
+ counterClear ();
99
+ speedSet (init_speed, init_speed);
100
+ dirSet (MOT_FORWARD, MOT_FORWARD);
42
101
obj_step = (int )((float )len * 2.0 / PULSE);
43
- digitalWrite (CW_R, HIGH);
44
- digitalWrite (CW_L, LOW);
45
102
46
- while ((g_step_r + g_step_l) < obj_step) {
47
- continue ;
103
+ while (1 ) {
104
+ stepGet ();
105
+ if (step_lr > obj_step) {
106
+ break ;
107
+ }
48
108
}
49
109
}
50
110
51
- void decelerate (int len, int tar_speed )
111
+ void RUN:: decelerate (int len, int init_speed )
52
112
{
53
113
int obj_step;
54
- g_max_speed = tar_speed;
55
- g_accel = 0.0 ;
56
- g_step_r = g_step_l = 0 ;
57
- g_speed = g_min_speed = tar_speed;
58
- g_step_hz_r = g_step_hz_l = (unsigned short )(g_speed / PULSE);
114
+
115
+ accel = 1.5 ;
116
+ max_speed = init_speed;
117
+ speed = min_speed = init_speed;
118
+ counterClear ();
119
+ speedSet (init_speed, init_speed);
120
+ dirSet (MOT_FORWARD, MOT_FORWARD);
59
121
obj_step = (int )((float )len * 2.0 / PULSE);
60
- digitalWrite (CW_R, HIGH);
61
- digitalWrite (CW_L, LOW);
62
122
63
- while ((len - (g_step_r + g_step_l) / 2.0 * PULSE) >
64
- (((g_speed * g_speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * 1.5 ))) {
65
- continue ;
123
+ while (1 ) {
124
+ stepGet ();
125
+ if (
126
+ (len - step_lr_len) <
127
+ (int )(((speed * speed) - (MIN_SPEED * MIN_SPEED)) / (2.0 * 1000.0 * accel))) {
128
+ break ;
129
+ }
66
130
}
67
- g_accel = -1.5 ;
68
- g_min_speed = MIN_SPEED;
131
+ accel = -1.5 ;
132
+ min_speed = MIN_SPEED;
69
133
70
- while ((g_step_r + g_step_l) < obj_step) {
71
- continue ;
134
+ while (1 ) {
135
+ stepGet ();
136
+ if (step_lr > obj_step) {
137
+ break ;
138
+ }
72
139
}
73
140
74
- g_motor_move = 0 ;
141
+ stop () ;
75
142
}
0 commit comments