Akito / Mbed 2 deprecated speed_mesurement

エンコーダから速度を測定する。 ボーレートは115200bps、データのみを送信する。

Dependencies:   mbed nucleo_rotary_encoder

main.cpp@0:3858dd3441ba, 2017-09-06 (annotated)

Committer:
Akito914
Date:
Wed Sep 06 08:12:52 2017 +0000
Revision:
0:3858dd3441ba
??????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Akito914 0:3858dd3441ba 1
Akito914 0:3858dd3441ba 2 #include "mbed.h"
Akito914 0:3858dd3441ba 3 #include "rotary_encoder_ab_phase.hpp"
Akito914 0:3858dd3441ba 4
Akito914 0:3858dd3441ba 5 #define sample_num_max 10000
Akito914 0:3858dd3441ba 6 #define sampling_period 500
Akito914 0:3858dd3441ba 7 #define reel_radius 25.0 // unit : mm
Akito914 0:3858dd3441ba 8 #define countPerRevol 4000
Akito914 0:3858dd3441ba 9
Akito914 0:3858dd3441ba 10 rotary_encoder_ab_phase encoder(TIM1,1000);
Akito914 0:3858dd3441ba 11
Akito914 0:3858dd3441ba 12 DigitalOut myled(LED1);
Akito914 0:3858dd3441ba 13 DigitalIn button(USER_BUTTON);
Akito914 0:3858dd3441ba 14
Akito914 0:3858dd3441ba 15 Serial pc(USBTX, USBRX);
Akito914 0:3858dd3441ba 16
Akito914 0:3858dd3441ba 17 Timer timer;
Akito914 0:3858dd3441ba 18
Akito914 0:3858dd3441ba 19
Akito914 0:3858dd3441ba 20 uint32_t sampling_count = 0;
Akito914 0:3858dd3441ba 21 int16_t sample_data[sample_num_max] = {0};
Akito914 0:3858dd3441ba 22 int32_t sample_time_us[sample_num_max] = {0};
Akito914 0:3858dd3441ba 23
Akito914 0:3858dd3441ba 24
Akito914 0:3858dd3441ba 25 int32_t get_encoder(void){
Akito914 0:3858dd3441ba 26 static int32_t counts = 0;
Akito914 0:3858dd3441ba 27 static int32_t p_raw_counts = 0;
Akito914 0:3858dd3441ba 28 int32_t raw_counts;
Akito914 0:3858dd3441ba 29
Akito914 0:3858dd3441ba 30 raw_counts=encoder.get_counts();
Akito914 0:3858dd3441ba 31
Akito914 0:3858dd3441ba 32 if((raw_counts - p_raw_counts) > 30000){
Akito914 0:3858dd3441ba 33 counts -= 65535 - (raw_counts - p_raw_counts);
Akito914 0:3858dd3441ba 34 }
Akito914 0:3858dd3441ba 35 else if((raw_counts - p_raw_counts) < -30000){
Akito914 0:3858dd3441ba 36 counts += 65535 - (p_raw_counts - raw_counts);
Akito914 0:3858dd3441ba 37 }
Akito914 0:3858dd3441ba 38 else{
Akito914 0:3858dd3441ba 39 counts += raw_counts - p_raw_counts;
Akito914 0:3858dd3441ba 40 }
Akito914 0:3858dd3441ba 41 p_raw_counts = raw_counts;
Akito914 0:3858dd3441ba 42 return counts;
Akito914 0:3858dd3441ba 43 }
Akito914 0:3858dd3441ba 44
Akito914 0:3858dd3441ba 45 int32_t get_encoder_diff(void){
Akito914 0:3858dd3441ba 46 int32_t diff = 0;
Akito914 0:3858dd3441ba 47 static int32_t p_raw_counts = 0;
Akito914 0:3858dd3441ba 48 int32_t raw_counts;
Akito914 0:3858dd3441ba 49
Akito914 0:3858dd3441ba 50 raw_counts=encoder.get_counts();
Akito914 0:3858dd3441ba 51
Akito914 0:3858dd3441ba 52 if((raw_counts - p_raw_counts) > 30000){
Akito914 0:3858dd3441ba 53 diff = 65535 - (raw_counts - p_raw_counts);
Akito914 0:3858dd3441ba 54 }
Akito914 0:3858dd3441ba 55 else if((raw_counts - p_raw_counts) < -30000){
Akito914 0:3858dd3441ba 56 diff = 65535 - (p_raw_counts - raw_counts);
Akito914 0:3858dd3441ba 57 }
Akito914 0:3858dd3441ba 58 else{
Akito914 0:3858dd3441ba 59 diff = raw_counts - p_raw_counts;
Akito914 0:3858dd3441ba 60 }
Akito914 0:3858dd3441ba 61 p_raw_counts = raw_counts;
Akito914 0:3858dd3441ba 62 return diff;
Akito914 0:3858dd3441ba 63 }
Akito914 0:3858dd3441ba 64
Akito914 0:3858dd3441ba 65 double dabs(double val){
Akito914 0:3858dd3441ba 66 if(val < 0.0)return val * -1;
Akito914 0:3858dd3441ba 67 return val;
Akito914 0:3858dd3441ba 68 }
Akito914 0:3858dd3441ba 69
Akito914 0:3858dd3441ba 70 int main() {
Akito914 0:3858dd3441ba 71
Akito914 0:3858dd3441ba 72 int32_t ex_time_us = 0;
Akito914 0:3858dd3441ba 73 int32_t interval_us = 0;
Akito914 0:3858dd3441ba 74 double revolPerSec;
Akito914 0:3858dd3441ba 75 double speed = 0.0;
Akito914 0:3858dd3441ba 76
Akito914 0:3858dd3441ba 77 pc.baud(115200);
Akito914 0:3858dd3441ba 78
Akito914 0:3858dd3441ba 79 encoder.start();
Akito914 0:3858dd3441ba 80
Akito914 0:3858dd3441ba 81 //pc.printf("Hello\n");
Akito914 0:3858dd3441ba 82 wait_ms(100);
Akito914 0:3858dd3441ba 83
Akito914 0:3858dd3441ba 84 //pc.printf("wait starting ...\n");
Akito914 0:3858dd3441ba 85 while(button == 1);
Akito914 0:3858dd3441ba 86 //pc.printf("sampling ...\n");
Akito914 0:3858dd3441ba 87
Akito914 0:3858dd3441ba 88 get_encoder_diff();
Akito914 0:3858dd3441ba 89 timer.start();
Akito914 0:3858dd3441ba 90 timer.reset();
Akito914 0:3858dd3441ba 91 while(button == 0) {
Akito914 0:3858dd3441ba 92
Akito914 0:3858dd3441ba 93 wait_us(sampling_period);
Akito914 0:3858dd3441ba 94
Akito914 0:3858dd3441ba 95 sample_time_us[sampling_count] = timer.read_us();
Akito914 0:3858dd3441ba 96 sample_data[sampling_count] = get_encoder_diff();
Akito914 0:3858dd3441ba 97
Akito914 0:3858dd3441ba 98 sampling_count += 1;
Akito914 0:3858dd3441ba 99 if(sampling_count >= sample_num_max - 1){
Akito914 0:3858dd3441ba 100 break;
Akito914 0:3858dd3441ba 101 }
Akito914 0:3858dd3441ba 102
Akito914 0:3858dd3441ba 103 }
Akito914 0:3858dd3441ba 104
Akito914 0:3858dd3441ba 105 //pc.printf("sampling finished\n");
Akito914 0:3858dd3441ba 106
Akito914 0:3858dd3441ba 107 for(uint32_t count = 0; count < sampling_count; count++){
Akito914 0:3858dd3441ba 108 interval_us = sample_time_us[count] - ex_time_us;
Akito914 0:3858dd3441ba 109 ex_time_us = sample_time_us[count];
Akito914 0:3858dd3441ba 110 revolPerSec = (double)sample_data[count] * 1000000.0 / countPerRevol / (double)interval_us;
Akito914 0:3858dd3441ba 111 speed = 2 * 3.14159265 * reel_radius / 1000.0 * revolPerSec;
Akito914 0:3858dd3441ba 112 pc.printf("%d, %f\n", sample_time_us[count], speed);
Akito914 0:3858dd3441ba 113 }
Akito914 0:3858dd3441ba 114
Akito914 0:3858dd3441ba 115 while(1);
Akito914 0:3858dd3441ba 116
Akito914 0:3858dd3441ba 117 }