File content as of revision 6:27d0384052d2:

#include <math.h>
#include "mbed.h"
#include "Encoder.h"
#include "locate.h"


/********以下グローバル変数宣言**************/

TIM_Encoder_InitTypeDef encoder1, encoder2;     //Encoderライブラリで使う
TIM_HandleTypeDef  timer1,  timer2;             //Encoderライブラリで使う
uint16_t count1=0,  count2=0;                   //Encoderライブラリで使う
int8_t   dir1, dir2;                            //Encoderライブラリで使う

Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信
DigitalOut enc_v(PC_7);             //エンコーダの電源

int     r, l;                  //現在の回転数
int     pr = 0, pl = 0;        //前回のステップ数
int     v = 0;                 //ステップ速度
float   xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
float   theta = 0;             //機体角度、x軸正の向きを0とする
float   d_theta = 0;           //erase()関数を使って、そのときのthetaをコピー
float   virtual_xcount = 0 ,virtual_ycount = 0;
int     *virtual_v;
float   virtual_theta = 0;
float   virtual_ptheta  = 0;
/*************変数宣言終了******************/




void  setup()
{
    enc_v = 1;

    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
    //use PB6 PB7 = Nucleo D7 D8
    EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);

    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
    //use PA0 PA1 = Nucleo A0 A1
    EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
}

void  setup(int tx, int ty)
{
    enc_v = 1;

    xcount = tx / (LOCATE_STEP / 2);
    ycount = ty / (LOCATE_STEP / 2);

    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
    //use PB6 PB7 = Nucleo D7 D8
    EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);

    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
    //use PA0 PA1 = Nucleo A0 A1
    EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
}

void  update()
//位置情報を更新する。r,lはエンコーダから
{
    count1=__HAL_TIM_GET_COUNTER(&timer1);
    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
    count2=__HAL_TIM_GET_COUNTER(&timer2);
    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);

    r = -convert_enc_count(count1, dir1);
    l = -convert_enc_count(count2, dir2);
    
    theta = (r - l) * ROUND;
    //theta = (r - l) * ROUND - d_theta;
    v = (r - pr + l - pl);

    xcount += v * cos(theta);
    ycount += v * sin(theta);

    pr = r;
    pl = l;
    //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
    pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
}

void update_np()
{
    count1=__HAL_TIM_GET_COUNTER(&timer1);
    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
    count2=__HAL_TIM_GET_COUNTER(&timer2);
    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);

    r = -convert_enc_count(count1, dir1);
    l = -convert_enc_count(count2, dir2);

    theta = (r - l) * ROUND - d_theta;
    v = (r - pr + l - pl);

    xcount += v * cos(theta);
    ycount += v * sin(theta);

    pr = r;
    pl = l;
}

int coordinateX()
{
    return xcount * LOCATE_STEP / 2;
}

int coordinateY()
{
    return ycount * LOCATE_STEP / 2;
}

float coordinateTheta()
{
    return theta;
}

int convert_enc_count(int16_t pulse, int8_t direction)
{
    if(direction == 0)
        pulse = pulse - 0xffff -1;

    return pulse;
}

void erase()
{
    xcount = 0;
    ycount = 0;
    d_theta = theta;
}

void virtual_setup()
{
    virtual_xcount = 0;
    virtual_ycount = 0;
    
    virtual_v = &v;
    
    virtual_theta = 0;
    virtual_ptheta = coordinateTheta();
}

void virtual_update()
{
    virtual_theta = coordinateTheta() - virtual_ptheta;
    
    virtual_xcount += *virtual_v * cos(virtual_theta);
    virtual_ycount += *virtual_v * sin(virtual_theta);
}

int virtual_coordinateX()
{
    return virtual_xcount * LOCATE_STEP / 2;
}

int virtual_coordinateY()
{
    return virtual_ycount * LOCATE_STEP / 2;
}

float virtual_coordinateTheta()
{
    return virtual_theta;
}