Add step6 sample (#41)

* add step6.sh

* Add step6.py

* Add step6.c

* update example README

* fix step6.sh

SampleProgram/README.md

@@ -85,3 +85,9 @@ Drive the motors to rotate clockwise and counter-clockwise.
 ライトセンサの値を読み込みます。
 
 Read the lightsensors values.
+
+# Step6
+
+モータを回して、パルスカウンタの値を読み込みます。
+
+Drive the motors and read the pulse counters values.

SampleProgram/step6.c

@@ -0,0 +1,110 @@
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+
+#define FILE_MOTOREN "/dev/rtmotoren0"
+#define FILE_MOTOR_L "/dev/rtmotor_raw_l0"
+#define FILE_MOTOR_R "/dev/rtmotor_raw_r0"
+#define FILE_COUNT_L "/dev/rtcounter_l0"
+#define FILE_COUNT_R "/dev/rtcounter_r0"
+#define BUFF_SIZE 256
+
+
+void motor_drive(char *freq_l, char *freq_r) {
+    FILE *motor_l, *motor_r;
+    if ((motor_l = fopen(FILE_MOTOR_L, "w")) != NULL &&
+        (motor_r = fopen(FILE_MOTOR_R, "w")) != NULL) {
+        fputs(freq_l, motor_l);
+        fputs(freq_r, motor_r);
+    }
+    fclose(motor_l);
+    fclose(motor_r);
+}
+
+void delete_newline(char *str) {
+    char *p;
+
+    if ((p = strchr(str, '\n')) != NULL) {
+        *p = '\0';
+    }
+}
+
+void print_counter(const int timeout) {
+    FILE *count_l, *count_r;
+    char buff_l[BUFF_SIZE];
+    char buff_r[BUFF_SIZE];
+
+    time_t start = time(NULL);
+    int elapsed_time = 0;
+
+    while (elapsed_time < timeout) {
+        if ((count_l = fopen(FILE_COUNT_L, "r")) != NULL &&
+            (count_r = fopen(FILE_COUNT_R, "r")) != NULL) {
+            while (fgets(buff_l, BUFF_SIZE, count_l) != NULL) {}
+            while (fgets(buff_r, BUFF_SIZE, count_r) != NULL) {}
+            delete_newline(buff_l);
+            delete_newline(buff_r);
+            printf("count_l:%s, count_r:%s\n", buff_l, buff_r);
+        }
+        fclose(count_l);
+        fclose(count_r);
+
+        elapsed_time = (int)(time(NULL) - start);
+    }
+}
+
+void reset_counters_and_motors(void) {
+    FILE *count_l, *count_r;
+
+    motor_drive("0", "0");
+
+    printf("Reset counter\n");
+    if ((count_l = fopen(FILE_COUNT_L, "w")) != NULL &&
+        (count_r = fopen(FILE_COUNT_R, "w")) != NULL) {
+        fputs("0", count_l);
+        fputs("0", count_r);
+    }
+    fclose(count_l);
+    fclose(count_r);
+}
+
+
+int main(void) {
+    int motoren = open("/dev/rtmotoren0", O_WRONLY);
+    // int motor_l = open("/dev/rtmotor_raw_l0",O_WRONLY);
+
+    printf("Motor On\n");
+    write(motoren, "1", 1);
+
+    printf("Rotate left motor\n");
+    usleep(500*1000);
+    motor_drive("400", "0");
+    print_counter(2);
+    reset_counters_and_motors();
+
+    printf("Rotate right motor\n");
+    usleep(500*1000);
+    motor_drive("0", "400");
+    print_counter(2);
+    reset_counters_and_motors();
+
+    printf("Move forward\n");
+    usleep(500*1000);
+    motor_drive("400", "400");
+    print_counter(2);
+    reset_counters_and_motors();
+
+    printf("Move backward\n");
+    usleep(500*1000);
+    motor_drive("-400", "-400");
+    print_counter(2);
+    reset_counters_and_motors();
+
+    printf("Motor Off\n");
+    write(motoren, "0", 1);
+
+    close(motoren);
+    return 0;
+}

SampleProgram/step6.py

@@ -0,0 +1,73 @@
+#!/usr/bin/python
+import time
+import sys
+
+filename_motoren = "/dev/rtmotoren0"
+filename_motor_r = "/dev/rtmotor_raw_r0"
+filename_motor_l = "/dev/rtmotor_raw_l0"
+filename_motor = "/dev/rtmotor0"
+filename_count_r = "/dev/rtcounter_r0"
+filename_count_l = "/dev/rtcounter_l0"
+
+SPEED = "400"
+
+
+def motor_drive(freq_l="0", freq_r="0"):
+    with open(filename_motor_l, 'w') as f:
+        f.write(freq_l)
+    with open(filename_motor_r, 'w') as f:
+        f.write(freq_r)
+
+
+def print_counter(timeout=2.0):
+    start = time.time()
+    while time.time() - start < timeout:
+        count_r = count_l = 0
+        with open(filename_count_l, 'r') as f:
+            count_l = f.read().strip()
+        with open(filename_count_r, 'r') as f:
+            count_r = f.read().strip()
+        print("count_l:" + count_l + ", count_r:" + count_r)
+
+
+def reset_counters_and_motors():
+    motor_drive("0", "0")
+    print("Reset counter")
+    with open(filename_count_l, 'w') as f:
+        f.write("0")
+    with open(filename_count_r, 'w') as f:
+        f.write("0")
+    print_counter(0)
+
+
+print("Motor On")
+with open(filename_motoren, 'w') as f:
+    f.write("1")
+
+print("Rotate left motor")
+time.sleep(0.5)
+motor_drive(SPEED, "0")
+print_counter(2.0)
+reset_counters_and_motors()
+
+print("Rotate right motor")
+time.sleep(0.5)
+motor_drive("0", SPEED)
+print_counter(2.0)
+reset_counters_and_motors()
+
+print("Move forward")
+time.sleep(0.5)
+motor_drive(SPEED, SPEED)
+print_counter(2.0)
+reset_counters_and_motors()
+
+print("Move backward")
+time.sleep(0.5)
+motor_drive("-"+SPEED, "-"+SPEED)
+print_counter(2.0)
+reset_counters_and_motors()
+
+print("Motor Off")
+with open(filename_motoren, 'w') as f:
+    f.write("0")

SampleProgram/step6.sh

@@ -0,0 +1,56 @@
+#!/bin/bash
+
+MOTOR_EN=/dev/rtmotoren0
+MOTOR_R=/dev/rtmotor_raw_r0
+MOTOR_L=/dev/rtmotor_raw_l0
+COUNTER_R=/dev/rtcounter_r0
+COUNTER_L=/dev/rtcounter_l0
+
+SPEED=400
+
+function echo_counter () {
+    SECONDS=0
+    while [ $SECONDS -lt "$1" ]
+    do
+    echo "count_l:$(cat $COUNTER_L), count_r:$(cat $COUNTER_R)"
+    done
+}
+
+function reset_counters_and_motors () {
+    echo 0 | tee $MOTOR_L $MOTOR_R > /dev/null
+    echo "Reset counter"
+    echo 0 | tee $COUNTER_L $COUNTER_R > /dev/null
+    echo "count_l:$(cat $COUNTER_L), count_r:$(cat $COUNTER_R)"
+}
+
+reset_counters_and_motors
+
+echo "Motor On"
+echo 1 > $MOTOR_EN
+
+echo "Rotate left motor"
+sleep 0.5
+echo $SPEED > $MOTOR_L
+echo_counter 2
+reset_counters_and_motors
+
+echo "Rotate right motor"
+sleep 0.5
+echo $SPEED > $MOTOR_R
+echo_counter 2
+reset_counters_and_motors
+
+echo "Move forward"
+sleep 0.5
+echo $SPEED | tee $MOTOR_L $MOTOR_R > /dev/null
+echo_counter 2
+reset_counters_and_motors
+
+echo "Move backward"
+sleep 0.5
+echo -$SPEED | tee $MOTOR_L $MOTOR_R > /dev/null
+echo_counter 2
+reset_counters_and_motors
+
+echo "Motor Off"
+echo 0 > $MOTOR_EN