Diff: main.cpp

Revision:

0:3858dd3441ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 06 08:12:52 2017 +0000
@@ -0,0 +1,117 @@
+
+#include "mbed.h"
+#include "rotary_encoder_ab_phase.hpp"
+
+#define sample_num_max  10000
+#define sampling_period  500
+#define reel_radius      25.0 //  unit : mm
+#define countPerRevol    4000
+
+rotary_encoder_ab_phase encoder(TIM1,1000);
+
+DigitalOut myled(LED1);
+DigitalIn button(USER_BUTTON);
+
+Serial pc(USBTX, USBRX);
+
+Timer timer;
+
+
+uint32_t sampling_count = 0;
+int16_t sample_data[sample_num_max] = {0};
+int32_t sample_time_us[sample_num_max] = {0};
+
+
+int32_t get_encoder(void){
+    static int32_t counts = 0;
+    static int32_t p_raw_counts = 0;
+    int32_t raw_counts;
+    
+    raw_counts=encoder.get_counts();
+    
+    if((raw_counts - p_raw_counts) > 30000){
+        counts -= 65535 - (raw_counts - p_raw_counts);
+    }
+    else if((raw_counts - p_raw_counts) < -30000){
+        counts += 65535 - (p_raw_counts - raw_counts);
+    }
+    else{
+        counts += raw_counts - p_raw_counts;
+    }
+    p_raw_counts = raw_counts;
+    return counts;
+}
+
+int32_t get_encoder_diff(void){
+    int32_t diff = 0;
+    static int32_t p_raw_counts = 0;
+    int32_t raw_counts;
+    
+    raw_counts=encoder.get_counts();
+    
+    if((raw_counts - p_raw_counts) > 30000){
+        diff = 65535 - (raw_counts - p_raw_counts);
+    }
+    else if((raw_counts - p_raw_counts) < -30000){
+        diff = 65535 - (p_raw_counts - raw_counts);
+    }
+    else{
+        diff = raw_counts - p_raw_counts;
+    }
+    p_raw_counts = raw_counts;
+    return diff;
+}
+
+double dabs(double val){
+    if(val < 0.0)return val * -1;
+    return val;
+}
+
+int main() {
+    
+    int32_t ex_time_us = 0;
+    int32_t interval_us = 0;
+    double revolPerSec;
+    double speed = 0.0;
+    
+    pc.baud(115200);
+    
+    encoder.start();
+    
+    //pc.printf("Hello\n");
+    wait_ms(100);
+    
+    //pc.printf("wait starting ...\n");
+    while(button == 1);
+    //pc.printf("sampling ...\n");
+    
+    get_encoder_diff();
+    timer.start();
+    timer.reset();
+    while(button == 0) {
+        
+        wait_us(sampling_period);
+        
+        sample_time_us[sampling_count] = timer.read_us();
+        sample_data[sampling_count] = get_encoder_diff();
+        
+        sampling_count += 1;
+        if(sampling_count >= sample_num_max - 1){
+            break;
+        }
+        
+    }
+    
+    //pc.printf("sampling finished\n");
+    
+    for(uint32_t count = 0; count < sampling_count; count++){
+        interval_us = sample_time_us[count] - ex_time_us;
+        ex_time_us = sample_time_us[count];
+        revolPerSec = (double)sample_data[count] * 1000000.0 / countPerRevol / (double)interval_us;
+        speed = 2 * 3.14159265 * reel_radius / 1000.0 * revolPerSec;
+        pc.printf("%d, %f\n", sample_time_us[count], speed);
+    }
+    
+    while(1);
+    
+}