STM32L010とGP2Y0A21YK(距離センサー)で6cmから80cmを求める。 シリアル出力

//電圧

#include "mbed.h"

//10の割り算 0から1028までは、正しい。主に0から999
#define DVI10(n) ((n*205)>>11)

//アナログ入力の設定
AnalogIn adc_vbat(A3); //PA_4
//AnalogIn adc_vbat(A0); //767

//Serial pc(USBTX, USBRX); // tx, rx
//Serial pc(SERIAL_TX, SERIAL_RX); //767
//Serial pc(PA_2, PA_3); //010
//Serial pc(PA_9, PA_10); //010
RawSerial pc(PA_2, PA_3); //010

char ch_hex_a_b[5];
char *ch_hex_a(int l_num)
{
    int a,b,c;

    b=DVI10(l_num);
    c=l_num-(b*10);
    a=DVI10(b);
    b=b-(a*10);

    ch_hex_a_b[0] = '0' + a;
    ch_hex_a_b[1] = '0' + b;
    ch_hex_a_b[2] = '0' + c;
    ch_hex_a_b[3] = 0;

    return(ch_hex_a_b);
} //ch_hex_a

//文字列の表示
int pc_printf(char *str1) {

    //文字の中身がゼロか
    while(*str1){

        //一文字出力
        pc.putc(*str1 ++);

    } //while

    //戻り値
    return(0);
}//pc_printf

//赤外線距離センサー(GP2Y0A21YK)の電圧から距離を求める。
//Voltage //電圧
float ir_len(float Voltage)
{

    float ir_length;   //長さ

    if (Voltage >= 0.0f  && Voltage <= 0.4f) {Voltage = 0.4f;}
    if (Voltage >= 3.12f && Voltage <= 5.0f) {Voltage = 3.12f;}

    if        (Voltage >= 0.4f && Voltage <= 1.3f ) { //80-20

        ir_length = 1.0f/ ( 0.0125f + ( ( Voltage - 0.4f) *  (0.0375f/0.9f) ) );

    } else if (Voltage >= 1.3f && Voltage <= 2.3f ) { //20-10

        ir_length = 1.0f/ ( 0.05f   + ( ( Voltage - 1.3f) * 0.05f ) );

    } else if (Voltage >= 2.3f && Voltage <= 2.75f)  { //10-8

        ir_length = 1.0f/ ( 0.1f + ( ( Voltage - 2.3f) * (0.025f/0.45f) ) );

    } else if (Voltage >= 2.75f && Voltage <= 2.98f) { //8-7

        ir_length = 1.0f/ ( 0.125f + ( ( Voltage - 2.75f) * (0.0179f/0.23f) ) );

    } else if (Voltage >= 2.98f && Voltage <= 3.12f) { //7-6

        ir_length = 1.0f/ ( 0.1429f + ( ( Voltage - 2.98f) * (0.0238f/0.14f) ) );

    }//end if

    //戻り値
    return(ir_length);

} //ir_len


int main() {

    float Voltage;      //電圧
    float ir_length;    //長さ
    int ir_length_i;    //長さ
    int ir_length_s;    //長さ 小数点以下

    //無限ループ
    while(1) {

        //adcの読み込み 0から4096
        int s = (adc_vbat.read_u16()>>4);

//        pc.printf("\r\n -S=%d",s); //767

        //電圧に変換 ex 3.300 -> 3300 mVを出力
        s=(s*6600)>>13;

        Voltage = (float)s / 1000.0f;
        //赤外線距離センサー(GP2Y0A21YK)の電圧から距離を求める。
        ir_length = ir_len(Voltage);

        ir_length_i = (int)ir_length;
        ir_length_s = (int)(ir_length*1000);
        ir_length_s = ir_length_s - (ir_length_i * 1000);

        //距離の小数点以上の表示
        pc_printf(  ch_hex_a(ir_length_i)  );
        //pc_printf(  "\r\n"  );

        pc_printf(  "."  );

        pc_printf(  ch_hex_a(ir_length_s)  );
        pc_printf(  "\r\n"  );


//        //s = 3300;
//        pc.printf("\t c=%d",s); //767

//小数点以上と小数点以下を分ける iiは1の桁 sは小数点以上の桁
//v int ii = s;
//v if     ( ii >= 3000 ) { ii = (ii - 3000); s = 3;}
//v else if( ii >= 2000 ) { ii = (ii - 2000); s = 2;}
//v else if( ii >= 1000 ) { ii = (ii - 1000); s = 1;}
//v else                  {                 ; s = 0;}

//        pc.printf("\t t=%d %d",s,ii); //767
//        pc.printf("\r\n"); //767 リターン


        //電圧の小数点以上の表示
//v        ch_hex_a( s*10 );
//v        ch_hex_a_b[0] = '+';
//v      //ch_hex_a_b[1] = '0' + s;
//v        ch_hex_a_b[2] = '.';      
//v        pc_printf(  ch_hex_a_b  );

        //電圧の小数点以下の表示
//v        pc_printf(  ch_hex_a(ii)  );

        //リターン
//v        pc_printf(  "\r\n"  );

        //1秒の待ち
        wait_ms(100);

    } //while

} //main

//容量削減
void error(const char* format, ...){}