涼太郎 中村 / Mbed 2 deprecated 9_6test

a

Dependencies:   mbed

Fork of ARAI45th by 涼太郎 中村

locate.h@9:f4dbffb78eb8, 2016-09-06 (annotated)

Committer:
choutin
Date:
Tue Sep 06 08:46:22 2016 +0000
Revision:
9:f4dbffb78eb8
Parent:
7:2b63f0a1b679 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:f12d257b587e 1 #ifndef Locate_H
sakanakuuun 0:f12d257b587e 2 #define Locate_H
sakanakuuun 0:f12d257b587e 3
sakanakuuun 0:f12d257b587e 4 #include <math.h>
sakanakuuun 0:f12d257b587e 5 #include"mbed.h"
sakanakuuun 0:f12d257b587e 6 #include "Encoder.h"
sakanakuuun 0:f12d257b587e 7
sakanakuuun 0:f12d257b587e 8
sakanakuuun 0:f12d257b587e 9 #define OUTERRING_D 140 //外輪間距離(mm)
sakanakuuun 0:f12d257b587e 10 #define INNERRING_D 136 //内輪間距離(mm)
sakanakuuun 0:f12d257b587e 11 #define PI 3.14159 //π
sakanakuuun 0:f12d257b587e 12 #define RESOLUSION 400 //P/R(分解能)
sakanakuuun 0:f12d257b587e 13 #define DIAMETER 31.8 //タイヤの直径(mm)
sakanakuuun 0:f12d257b587e 14 #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの１ステップあたりの距離(mm)
sakanakuuun 0:f12d257b587e 15 #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm)
sakanakuuun 0:f12d257b587e 16 #define ROUND_HOSEI 1 //角度のズレを補正
sakanakuuun 0:f12d257b587e 17 #define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) //機体が１回転するために必要なステップ数の”逆数”
sakanakuuun 0:f12d257b587e 18
sakanakuuun 0:f12d257b587e 19 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
sakanakuuun 0:f12d257b587e 20 #ifdef TARGET_STM32F3
sakanakuuun 0:f12d257b587e 21 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
sakanakuuun 0:f12d257b587e 22 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
sakanakuuun 0:f12d257b587e 23 #endif
sakanakuuun 0:f12d257b587e 24
sakanakuuun 0:f12d257b587e 25
sakanakuuun 0:f12d257b587e 26 //エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
sakanakuuun 0:f12d257b587e 27
sakanakuuun 0:f12d257b587e 28
sakanakuuun 0:f12d257b587e 29 //グローバル変数宣言
sakanakuuun 0:f12d257b587e 30 Serial pc(SERIAL_TX, SERIAL_RX);
sakanakuuun 0:f12d257b587e 31 TIM_Encoder_InitTypeDef encoder1, encoder2;
sakanakuuun 0:f12d257b587e 32 TIM_HandleTypeDef timer1, timer2;
sakanakuuun 1:10cc86cabdce 33 DigitalOut enc_v(PC_7);
sakanakuuun 0:f12d257b587e 34
sakanakuuun 0:f12d257b587e 35 uint16_t count1=0, count2=0;
sakanakuuun 0:f12d257b587e 36 int8_t dir1, dir2;
sakanakuuun 0:f12d257b587e 37 int r, l;
sakanakuuun 0:f12d257b587e 38 int pr = 0, pl = 0; //前回のステップ数
sakanakuuun 0:f12d257b587e 39 short v = 0; //ステップ速度
sakanakuuun 0:f12d257b587e 40 float x = 0, y = 0; //xy方向に進んだ距離(m換算なし)
sakanakuuun 0:f12d257b587e 41 float theta = 0; //機体角度、x軸正の向きを0とする
choutin 3:56b034c41dc5 42 float erase_theta = 0;
sakanakuuun 0:f12d257b587e 43 //宣言終わり
sakanakuuun 0:f12d257b587e 44
sakanakuuun 0:f12d257b587e 45
sakanakuuun 0:f12d257b587e 46
sakanakuuun 0:f12d257b587e 47 void setup() //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
sakanakuuun 0:f12d257b587e 48 {
sakanakuuun 0:f12d257b587e 49 enc_v = 1;
sakanakuuun 0:f12d257b587e 50
sakanakuuun 0:f12d257b587e 51 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
choutin 2:4b2594dd86be 52 //use PB6 PB7 = Nucleo D7 D8
choutin 2:4b2594dd86be 53 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 0:f12d257b587e 54
sakanakuuun 0:f12d257b587e 55 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 0:f12d257b587e 56 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 0:f12d257b587e 57 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 0:f12d257b587e 58 }
sakanakuuun 0:f12d257b587e 59
sakanakuuun 0:f12d257b587e 60 int convert_enc_count(int16_t pulse, int8_t direction)
sakanakuuun 0:f12d257b587e 61 {
sakanakuuun 0:f12d257b587e 62 if(direction == 0)
sakanakuuun 0:f12d257b587e 63 pulse = pulse - 0xffff -1;
sakanakuuun 0:f12d257b587e 64
sakanakuuun 0:f12d257b587e 65 return pulse;
sakanakuuun 0:f12d257b587e 66 }
sakanakuuun 0:f12d257b587e 67
sakanakuuun 0:f12d257b587e 68
sakanakuuun 0:f12d257b587e 69 short coordinateX()
sakanakuuun 0:f12d257b587e 70 //xをmm換算して整数値として返す
sakanakuuun 0:f12d257b587e 71 {
sakanakuuun 0:f12d257b587e 72 return x * LOCATE_STEP / 2;
sakanakuuun 0:f12d257b587e 73 }
sakanakuuun 0:f12d257b587e 74
sakanakuuun 0:f12d257b587e 75 short coordinateY()
sakanakuuun 0:f12d257b587e 76 //yをmm換算して整数値として返す
sakanakuuun 0:f12d257b587e 77 {
sakanakuuun 0:f12d257b587e 78 return y * LOCATE_STEP / 2;
sakanakuuun 0:f12d257b587e 79 }
sakanakuuun 0:f12d257b587e 80
sakanakuuun 0:f12d257b587e 81 float coordinateTheta()
sakanakuuun 0:f12d257b587e 82 //thetaを返す
sakanakuuun 0:f12d257b587e 83 {
sakanakuuun 0:f12d257b587e 84 return theta;
sakanakuuun 0:f12d257b587e 85 }
sakanakuuun 0:f12d257b587e 86
sakanakuuun 0:f12d257b587e 87
choutin 3:56b034c41dc5 88 void update ()
choutin 3:56b034c41dc5 89 //位置情報を更新する。r,lはエンコーダから
choutin 3:56b034c41dc5 90 {
choutin 3:56b034c41dc5 91 count1=__HAL_TIM_GET_COUNTER(&timer1);
choutin 3:56b034c41dc5 92 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
choutin 3:56b034c41dc5 93 count2=__HAL_TIM_GET_COUNTER(&timer2);
choutin 3:56b034c41dc5 94 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
choutin 3:56b034c41dc5 95
choutin 3:56b034c41dc5 96 r = -convert_enc_count(count1, dir1);
choutin 7:2b63f0a1b679 97 l = -convert_enc_count(count2, dir2);
choutin 3:56b034c41dc5 98
choutin 3:56b034c41dc5 99 theta = (r - l) * ROUND - erase_theta;
choutin 3:56b034c41dc5 100 v = (r - pr + l - pl);
choutin 3:56b034c41dc5 101
choutin 3:56b034c41dc5 102 x += v * cos(theta);
choutin 3:56b034c41dc5 103 y += v * sin(theta);
choutin 3:56b034c41dc5 104
choutin 3:56b034c41dc5 105 pr = r;
choutin 3:56b034c41dc5 106 pl = l;
choutin 3:56b034c41dc5 107 //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
choutin 3:56b034c41dc5 108 pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
choutin 3:56b034c41dc5 109 }
choutin 3:56b034c41dc5 110
choutin 3:56b034c41dc5 111 void erase()
choutin 3:56b034c41dc5 112 {
choutin 3:56b034c41dc5 113 x = 0;
choutin 3:56b034c41dc5 114 y = 0;
choutin 3:56b034c41dc5 115 erase_theta = theta;
choutin 3:56b034c41dc5 116 }
choutin 3:56b034c41dc5 117
sakanakuuun 0:f12d257b587e 118 #endif