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)                  // エンコーダの１ステップあたりの距離(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(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