a
Fork of ARAI45th by
Embed:
(wiki syntax)
Show/hide line numbers
locate.h
00001 #ifndef Locate_H 00002 #define Locate_H 00003 00004 #include <math.h> 00005 #include"mbed.h" 00006 #include "Encoder.h" 00007 00008 00009 #define OUTERRING_D 140 //外輪間距離(mm) 00010 #define INNERRING_D 136 //内輪間距離(mm) 00011 #define PI 3.14159 //π 00012 #define RESOLUSION 400 //P/R(分解能) 00013 #define DIAMETER 31.8 //タイヤの直径(mm) 00014 #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの1ステップあたりの距離(mm) 00015 #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm) 00016 #define ROUND_HOSEI 1 //角度のズレを補正 00017 #define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) //機体が1回転するために必要なステップ数の”逆数” 00018 00019 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h 00020 #ifdef TARGET_STM32F3 00021 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT) 00022 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR)) 00023 #endif 00024 00025 00026 //エンコーダから、現在のステップ数(=タイヤがどれだけ回ったか)を得られる 00027 00028 00029 //グローバル変数宣言 00030 Serial pc(SERIAL_TX, SERIAL_RX); 00031 TIM_Encoder_InitTypeDef encoder1, encoder2; 00032 TIM_HandleTypeDef timer1, timer2; 00033 DigitalOut enc_v(PC_7); 00034 00035 uint16_t count1=0, count2=0; 00036 int8_t dir1, dir2; 00037 int r, l; 00038 int pr = 0, pl = 0; //前回のステップ数 00039 short v = 0; //ステップ速度 00040 float x = 0, y = 0; //xy方向に進んだ距離(m換算なし) 00041 float theta = 0; //機体角度、x軸正の向きを0とする 00042 float erase_theta = 0; 00043 //宣言終わり 00044 00045 00046 00047 void setup() //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う 00048 { 00049 enc_v = 1; 00050 00051 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count 00052 //use PB6 PB7 = Nucleo D7 D8 00053 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12); 00054 00055 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count 00056 //use PA0 PA1 = Nucleo A0 A1 00057 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12); 00058 } 00059 00060 int convert_enc_count(int16_t pulse, int8_t direction) 00061 { 00062 if(direction == 0) 00063 pulse = pulse - 0xffff -1; 00064 00065 return pulse; 00066 } 00067 00068 00069 short coordinateX() 00070 //xをmm換算して整数値として返す 00071 { 00072 return x * LOCATE_STEP / 2; 00073 } 00074 00075 short coordinateY() 00076 //yをmm換算して整数値として返す 00077 { 00078 return y * LOCATE_STEP / 2; 00079 } 00080 00081 float coordinateTheta() 00082 //thetaを返す 00083 { 00084 return theta; 00085 } 00086 00087 00088 void update () 00089 //位置情報を更新する。r,lはエンコーダから 00090 { 00091 count1=__HAL_TIM_GET_COUNTER(&timer1); 00092 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1); 00093 count2=__HAL_TIM_GET_COUNTER(&timer2); 00094 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); 00095 00096 r = -convert_enc_count(count1, dir1); 00097 l = -convert_enc_count(count2, dir2); 00098 00099 theta = (r - l) * ROUND - erase_theta; 00100 v = (r - pr + l - pl); 00101 00102 x += v * cos(theta); 00103 y += v * sin(theta); 00104 00105 pr = r; 00106 pl = l; 00107 //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-"); 00108 pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta()); 00109 } 00110 00111 void erase() 00112 { 00113 x = 0; 00114 y = 0; 00115 erase_theta = theta; 00116 } 00117 00118 #endif
Generated on Fri Jul 15 2022 14:59:36 by
1.7.2
