エンコーダから速度を測定する。 ボーレートは115200bps、データのみを送信する。
Dependencies: mbed nucleo_rotary_encoder
Diff: main.cpp
- Revision:
- 0:3858dd3441ba
diff -r 000000000000 -r 3858dd3441ba main.cpp --- /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); + +}