1212// See the License for the specific language governing permissions and
1313// limitations under the License.
1414
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)
1671{
1772 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);
2373
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);
2480 obj_step = (int )((float )len * 2.0 / PULSE);
25- digitalWrite (CW_R, HIGH);
26- digitalWrite (CW_L, LOW);
2781 g_motor_move = 1 ;
2882
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+ }
3188 }
3289}
3390
34- void oneStep (int len, int tar_speed )
91+ void RUN:: oneStep (int len, int init_speed )
3592{
3693 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);
42101 obj_step = (int )((float )len * 2.0 / PULSE);
43- digitalWrite (CW_R, HIGH);
44- digitalWrite (CW_L, LOW);
45102
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+ }
48108 }
49109}
50110
51- void decelerate (int len, int tar_speed )
111+ void RUN:: decelerate (int len, int init_speed )
52112{
53113 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);
59121 obj_step = (int )((float )len * 2.0 / PULSE);
60- digitalWrite (CW_R, HIGH);
61- digitalWrite (CW_L, LOW);
62122
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+ }
66130 }
67- g_accel = -1.5 ;
68- g_min_speed = MIN_SPEED;
131+ accel = -1.5 ;
132+ min_speed = MIN_SPEED;
69133
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+ }
72139 }
73140
74- g_motor_move = 0 ;
141+ stop () ;
75142}
0 commit comments