File content as of revision 9:f4dbffb78eb8:

#ifndef Locate_H
#define Locate_H

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


#define OUTERRING_D     140             //外輪間距離(mm)
#define INNERRING_D     136             //内輪間距離(mm)
#define PI              3.14159         //π
#define RESOLUSION      400             //P/R(分解能)
#define DIAMETER        31.8            //タイヤの直径(mm)
#define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                  // エンコーダの１ステップあたりの距離(mm)
#define TIRE_DISTANCE   ((OUTERRING_D + INNERRING_D) / 2)           //タイヤ間距離(mm)
#define ROUND_HOSEI     1               //角度のズレを補正
#define ROUND           ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI)   //機体が１回転するために必要なステップ数の”逆数”

//STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
#ifdef TARGET_STM32F3
#define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
#define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__)            (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
#endif


//エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる


//グローバル変数宣言
Serial pc(SERIAL_TX, SERIAL_RX);
TIM_Encoder_InitTypeDef encoder1, encoder2;
TIM_HandleTypeDef  timer1,  timer2;
DigitalOut enc_v(PC_7);

uint16_t count1=0,  count2=0;
int8_t dir1, dir2;
int r, l;
int pr = 0, pl = 0;     //前回のステップ数
short v = 0;        //ステップ速度
float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
float theta = 0;    //機体角度、x軸正の向きを0とする
float erase_theta = 0;
//宣言終わり



void  setup()      //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
{
    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);
}

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


short coordinateX()
//xをmm換算して整数値として返す
{
    return x * LOCATE_STEP / 2;
}

short coordinateY()
//yをmm換算して整数値として返す
{
    return y * LOCATE_STEP / 2;
}

float coordinateTheta()
//thetaを返す
{
    return theta;
}


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 - erase_theta;
    v = (r - pr + l - pl);

    x += v * cos(theta);
    y += 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 erase()
{
    x = 0;
    y = 0;
    erase_theta = theta;
}

#endif