Tk A
/
locate_by_hello_enc
aaaaaaaaaa
Diff: locate.h
- Revision:
- 0:f12d257b587e
- Child:
- 1:10cc86cabdce
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/locate.h Thu Sep 01 05:13:31 2016 +0000 @@ -0,0 +1,109 @@ +#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) // エンコーダの1ステップあたりの距離(mm) +#define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm) +#define ROUND_HOSEI 1 //角度のズレを補正 +#define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) //機体が1回転するために必要なステップ数の”逆数” + +//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(PB_10); + +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とする +//宣言終わり + + + +void setup() //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う +{ + enc_v = 1; + + //counting on both A&B inputs, 4 ticks per cycle, 16-bit count + //use PA8 PA9 = Nucleo D7 D8 + EncoderInit(&encoder1, &timer1, TIM1, 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; +} + +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; + 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", r, l); +} + +short coordinateX() +//xをmm換算して整数値として返す +{ + return x * LOCATE_STEP / 2; +} + +short coordinateY() +//yをmm換算して整数値として返す +{ + return y * LOCATE_STEP / 2; +} + +float coordinateTheta() +//thetaを返す +{ + return theta; +} + + +#endif