Skip to content

Commit 07e8293

Browse files
author
Shota Aoki
authored
Add step6 sample (#41)
* add step6.sh * Add step6.py * Add step6.c * update example README * fix step6.sh
1 parent dd03434 commit 07e8293

File tree

4 files changed

+245
-0
lines changed

4 files changed

+245
-0
lines changed

‎SampleProgram/README.md

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,3 +85,9 @@ Drive the motors to rotate clockwise and counter-clockwise.
8585
ライトセンサの値を読み込みます。
8686

8787
Read the lightsensors values.
88+
89+
# Step6
90+
91+
モータを回して、パルスカウンタの値を読み込みます。
92+
93+
Drive the motors and read the pulse counters values.

‎SampleProgram/step6.c

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include <fcntl.h>
2+
#include <unistd.h>
3+
#include <stdio.h>
4+
#include <time.h>
5+
#include <string.h>
6+
7+
#define FILE_MOTOREN "/dev/rtmotoren0"
8+
#define FILE_MOTOR_L "/dev/rtmotor_raw_l0"
9+
#define FILE_MOTOR_R "/dev/rtmotor_raw_r0"
10+
#define FILE_COUNT_L "/dev/rtcounter_l0"
11+
#define FILE_COUNT_R "/dev/rtcounter_r0"
12+
#define BUFF_SIZE 256
13+
14+
15+
void motor_drive(char *freq_l, char *freq_r) {
16+
FILE *motor_l, *motor_r;
17+
if ((motor_l = fopen(FILE_MOTOR_L, "w")) != NULL &&
18+
(motor_r = fopen(FILE_MOTOR_R, "w")) != NULL) {
19+
fputs(freq_l, motor_l);
20+
fputs(freq_r, motor_r);
21+
}
22+
fclose(motor_l);
23+
fclose(motor_r);
24+
}
25+
26+
void delete_newline(char *str) {
27+
char *p;
28+
29+
if ((p = strchr(str, '\n')) != NULL) {
30+
*p = '\0';
31+
}
32+
}
33+
34+
void print_counter(const int timeout) {
35+
FILE *count_l, *count_r;
36+
char buff_l[BUFF_SIZE];
37+
char buff_r[BUFF_SIZE];
38+
39+
time_t start = time(NULL);
40+
int elapsed_time = 0;
41+
42+
while (elapsed_time < timeout) {
43+
if ((count_l = fopen(FILE_COUNT_L, "r")) != NULL &&
44+
(count_r = fopen(FILE_COUNT_R, "r")) != NULL) {
45+
while (fgets(buff_l, BUFF_SIZE, count_l) != NULL) {}
46+
while (fgets(buff_r, BUFF_SIZE, count_r) != NULL) {}
47+
delete_newline(buff_l);
48+
delete_newline(buff_r);
49+
printf("count_l:%s, count_r:%s\n", buff_l, buff_r);
50+
}
51+
fclose(count_l);
52+
fclose(count_r);
53+
54+
elapsed_time = (int)(time(NULL) - start);
55+
}
56+
}
57+
58+
void reset_counters_and_motors(void) {
59+
FILE *count_l, *count_r;
60+
61+
motor_drive("0", "0");
62+
63+
printf("Reset counter\n");
64+
if ((count_l = fopen(FILE_COUNT_L, "w")) != NULL &&
65+
(count_r = fopen(FILE_COUNT_R, "w")) != NULL) {
66+
fputs("0", count_l);
67+
fputs("0", count_r);
68+
}
69+
fclose(count_l);
70+
fclose(count_r);
71+
}
72+
73+
74+
int main(void) {
75+
int motoren = open("/dev/rtmotoren0", O_WRONLY);
76+
// int motor_l = open("/dev/rtmotor_raw_l0",O_WRONLY);
77+
78+
printf("Motor On\n");
79+
write(motoren, "1", 1);
80+
81+
printf("Rotate left motor\n");
82+
usleep(500*1000);
83+
motor_drive("400", "0");
84+
print_counter(2);
85+
reset_counters_and_motors();
86+
87+
printf("Rotate right motor\n");
88+
usleep(500*1000);
89+
motor_drive("0", "400");
90+
print_counter(2);
91+
reset_counters_and_motors();
92+
93+
printf("Move forward\n");
94+
usleep(500*1000);
95+
motor_drive("400", "400");
96+
print_counter(2);
97+
reset_counters_and_motors();
98+
99+
printf("Move backward\n");
100+
usleep(500*1000);
101+
motor_drive("-400", "-400");
102+
print_counter(2);
103+
reset_counters_and_motors();
104+
105+
printf("Motor Off\n");
106+
write(motoren, "0", 1);
107+
108+
close(motoren);
109+
return 0;
110+
}

‎SampleProgram/step6.py

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
#!/usr/bin/python
2+
import time
3+
import sys
4+
5+
filename_motoren = "/dev/rtmotoren0"
6+
filename_motor_r = "/dev/rtmotor_raw_r0"
7+
filename_motor_l = "/dev/rtmotor_raw_l0"
8+
filename_motor = "/dev/rtmotor0"
9+
filename_count_r = "/dev/rtcounter_r0"
10+
filename_count_l = "/dev/rtcounter_l0"
11+
12+
SPEED = "400"
13+
14+
15+
def motor_drive(freq_l="0", freq_r="0"):
16+
with open(filename_motor_l, 'w') as f:
17+
f.write(freq_l)
18+
with open(filename_motor_r, 'w') as f:
19+
f.write(freq_r)
20+
21+
22+
def print_counter(timeout=2.0):
23+
start = time.time()
24+
while time.time() - start < timeout:
25+
count_r = count_l = 0
26+
with open(filename_count_l, 'r') as f:
27+
count_l = f.read().strip()
28+
with open(filename_count_r, 'r') as f:
29+
count_r = f.read().strip()
30+
print("count_l:" + count_l + ", count_r:" + count_r)
31+
32+
33+
def reset_counters_and_motors():
34+
motor_drive("0", "0")
35+
print("Reset counter")
36+
with open(filename_count_l, 'w') as f:
37+
f.write("0")
38+
with open(filename_count_r, 'w') as f:
39+
f.write("0")
40+
print_counter(0)
41+
42+
43+
print("Motor On")
44+
with open(filename_motoren, 'w') as f:
45+
f.write("1")
46+
47+
print("Rotate left motor")
48+
time.sleep(0.5)
49+
motor_drive(SPEED, "0")
50+
print_counter(2.0)
51+
reset_counters_and_motors()
52+
53+
print("Rotate right motor")
54+
time.sleep(0.5)
55+
motor_drive("0", SPEED)
56+
print_counter(2.0)
57+
reset_counters_and_motors()
58+
59+
print("Move forward")
60+
time.sleep(0.5)
61+
motor_drive(SPEED, SPEED)
62+
print_counter(2.0)
63+
reset_counters_and_motors()
64+
65+
print("Move backward")
66+
time.sleep(0.5)
67+
motor_drive("-"+SPEED, "-"+SPEED)
68+
print_counter(2.0)
69+
reset_counters_and_motors()
70+
71+
print("Motor Off")
72+
with open(filename_motoren, 'w') as f:
73+
f.write("0")

‎SampleProgram/step6.sh

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
#!/bin/bash
2+
3+
MOTOR_EN=/dev/rtmotoren0
4+
MOTOR_R=/dev/rtmotor_raw_r0
5+
MOTOR_L=/dev/rtmotor_raw_l0
6+
COUNTER_R=/dev/rtcounter_r0
7+
COUNTER_L=/dev/rtcounter_l0
8+
9+
SPEED=400
10+
11+
function echo_counter () {
12+
SECONDS=0
13+
while [ $SECONDS -lt "$1" ]
14+
do
15+
echo "count_l:$(cat $COUNTER_L), count_r:$(cat $COUNTER_R)"
16+
done
17+
}
18+
19+
function reset_counters_and_motors () {
20+
echo 0 | tee $MOTOR_L $MOTOR_R > /dev/null
21+
echo "Reset counter"
22+
echo 0 | tee $COUNTER_L $COUNTER_R > /dev/null
23+
echo "count_l:$(cat $COUNTER_L), count_r:$(cat $COUNTER_R)"
24+
}
25+
26+
reset_counters_and_motors
27+
28+
echo "Motor On"
29+
echo 1 > $MOTOR_EN
30+
31+
echo "Rotate left motor"
32+
sleep 0.5
33+
echo $SPEED > $MOTOR_L
34+
echo_counter 2
35+
reset_counters_and_motors
36+
37+
echo "Rotate right motor"
38+
sleep 0.5
39+
echo $SPEED > $MOTOR_R
40+
echo_counter 2
41+
reset_counters_and_motors
42+
43+
echo "Move forward"
44+
sleep 0.5
45+
echo $SPEED | tee $MOTOR_L $MOTOR_R > /dev/null
46+
echo_counter 2
47+
reset_counters_and_motors
48+
49+
echo "Move backward"
50+
sleep 0.5
51+
echo -$SPEED | tee $MOTOR_L $MOTOR_R > /dev/null
52+
echo_counter 2
53+
reset_counters_and_motors
54+
55+
echo "Motor Off"
56+
echo 0 > $MOTOR_EN

0 commit comments

Comments
 (0)