a
Dependents: 4thcomp 6th33222_copy
Fork of Locate by
locate.h@5:d9857c701310, 2016-09-07 (annotated)
- Committer:
- sakanakuuun
- Date:
- Wed Sep 07 07:36:42 2016 +0000
- Revision:
- 5:d9857c701310
- Parent:
- 4:ecbb45e84c08
- Child:
- 6:ba3ef4656f7a
s
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakanakuuun | 0:265d8ad19d2a | 1 | #ifndef Locate_H |
sakanakuuun | 0:265d8ad19d2a | 2 | #define Locate_H |
sakanakuuun | 0:265d8ad19d2a | 3 | |
sakanakuuun | 0:265d8ad19d2a | 4 | #include <math.h> |
sakanakuuun | 0:265d8ad19d2a | 5 | #include "Encoder.h" |
sakanakuuun | 0:265d8ad19d2a | 6 | |
sakanakuuun | 0:265d8ad19d2a | 7 | /****************定数定義***************************/ |
sakanakuuun | 0:265d8ad19d2a | 8 | |
sakanakuuun | 0:265d8ad19d2a | 9 | #define OUTERRING_D 140 //外輪間距離(mm) |
sakanakuuun | 0:265d8ad19d2a | 10 | #define INNERRING_D 136 //内輪間距離(mm) |
sakanakuuun | 0:265d8ad19d2a | 11 | #define PI 3.14159 //π |
sakanakuuun | 0:265d8ad19d2a | 12 | #define RESOLUSION 400 //P/R(分解能) |
sakanakuuun | 0:265d8ad19d2a | 13 | #define DIAMETER 31.8 //タイヤの直径(mm) |
sakanakuuun | 5:d9857c701310 | 14 | #define ROUND_HOSEI 1.04 //角度のズレを補正 |
sakanakuuun | 0:265d8ad19d2a | 15 | |
sakanakuuun | 0:265d8ad19d2a | 16 | // エンコーダの1ステップあたりの距離(mm) |
sakanakuuun | 0:265d8ad19d2a | 17 | #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) |
sakanakuuun | 0:265d8ad19d2a | 18 | //タイヤ間距離(mm) |
sakanakuuun | 0:265d8ad19d2a | 19 | #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) |
sakanakuuun | 0:265d8ad19d2a | 20 | //機体が1回転するために必要なステップ数の”逆数” |
sakanakuuun | 0:265d8ad19d2a | 21 | #define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) |
sakanakuuun | 0:265d8ad19d2a | 22 | |
sakanakuuun | 0:265d8ad19d2a | 23 | /****************************************************************/ |
sakanakuuun | 0:265d8ad19d2a | 24 | |
sakanakuuun | 0:265d8ad19d2a | 25 | |
sakanakuuun | 0:265d8ad19d2a | 26 | |
sakanakuuun | 0:265d8ad19d2a | 27 | |
sakanakuuun | 0:265d8ad19d2a | 28 | /********以下関数宣言*****************/ |
sakanakuuun | 0:265d8ad19d2a | 29 | |
sakanakuuun | 0:265d8ad19d2a | 30 | //最初に一回だけ行う |
sakanakuuun | 0:265d8ad19d2a | 31 | void setup(); |
sakanakuuun | 0:265d8ad19d2a | 32 | |
sakanakuuun | 4:ecbb45e84c08 | 33 | //setupの初期位置指定版 |
sakanakuuun | 4:ecbb45e84c08 | 34 | void setup(int tx, int ty); |
sakanakuuun | 4:ecbb45e84c08 | 35 | |
sakanakuuun | 0:265d8ad19d2a | 36 | //位置情報を更新する。r,lはエンコーダから |
sakanakuuun | 4:ecbb45e84c08 | 37 | void update(); |
sakanakuuun | 4:ecbb45e84c08 | 38 | |
sakanakuuun | 4:ecbb45e84c08 | 39 | //update()のprintfなし版 |
sakanakuuun | 4:ecbb45e84c08 | 40 | void update_np(); |
sakanakuuun | 0:265d8ad19d2a | 41 | |
sakanakuuun | 0:265d8ad19d2a | 42 | //xをmm換算して整数値として返す |
sakanakuuun | 3:0afbd3f61151 | 43 | int coordinateX(); |
sakanakuuun | 0:265d8ad19d2a | 44 | |
sakanakuuun | 0:265d8ad19d2a | 45 | //yをmm換算して整数値として返す |
sakanakuuun | 1:3513a2fbd81f | 46 | int coordinateY(); |
sakanakuuun | 0:265d8ad19d2a | 47 | |
sakanakuuun | 0:265d8ad19d2a | 48 | //thetaを返す |
sakanakuuun | 0:265d8ad19d2a | 49 | float coordinateTheta(); |
sakanakuuun | 0:265d8ad19d2a | 50 | |
sakanakuuun | 0:265d8ad19d2a | 51 | //encorderの値を回転数に変換。update内部で使われる |
sakanakuuun | 0:265d8ad19d2a | 52 | int convert_enc_count(int16_t pulse, int8_t direction); |
sakanakuuun | 0:265d8ad19d2a | 53 | |
sakanakuuun | 0:265d8ad19d2a | 54 | //x, y, thetaを0にする |
sakanakuuun | 0:265d8ad19d2a | 55 | void erase(); |
sakanakuuun | 0:265d8ad19d2a | 56 | |
sakanakuuun | 0:265d8ad19d2a | 57 | /**********************************/ |
sakanakuuun | 0:265d8ad19d2a | 58 | #endif |
sakanakuuun | 0:265d8ad19d2a | 59 | |
sakanakuuun | 0:265d8ad19d2a | 60 | |
sakanakuuun | 0:265d8ad19d2a | 61 | |
sakanakuuun | 0:265d8ad19d2a | 62 | |
sakanakuuun | 0:265d8ad19d2a | 63 | |
sakanakuuun | 0:265d8ad19d2a | 64 | /**************Encoderライブラリで使う定義**************************/ |
sakanakuuun | 0:265d8ad19d2a | 65 | |
sakanakuuun | 0:265d8ad19d2a | 66 | #ifdef TARGET_STM32F3 |
sakanakuuun | 0:265d8ad19d2a | 67 | #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT) |
sakanakuuun | 0:265d8ad19d2a | 68 | #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR)) |
sakanakuuun | 0:265d8ad19d2a | 69 | #endif |
sakanakuuun | 0:265d8ad19d2a | 70 | |
sakanakuuun | 0:265d8ad19d2a | 71 | /***************************************************************/ |