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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy