fs
Dependents: ARAI45th 3servotest 1stcomp 3rdcompfixstart ... more
Diff: locate.cpp
- Revision:
- 0:265d8ad19d2a
- Child:
- 1:3513a2fbd81f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/locate.cpp Fri Sep 02 12:40:36 2016 +0000 @@ -0,0 +1,91 @@ +#include <math.h> +#include "mbed.h" +#include "Encoder.h" +#include "locate.h" + + +/********以下グローバル変数宣言**************/ + +TIM_Encoder_InitTypeDef encoder1, encoder2; //Encoderライブラリで使う +TIM_HandleTypeDef timer1, timer2; //Encoderライブラリで使う +uint16_t count1=0, count2=0; //Encoderライブラリで使う +int8_t dir1, dir2; //Encoderライブラリで使う + +Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信 +DigitalOut enc_v(PC_7); //エンコーダの電源 + +int r, l; //現在の回転数 +int pr = 0, pl = 0; //前回のステップ数 +short v = 0; //ステップ速度 +float x = 0, y = 0; //xy方向に進んだ距離(m換算なし) +float theta = 0; //機体角度、x軸正の向きを0とする +float d_theta = 0; //erase()関数を使って、そのときのthetaをコピー + +/*************変数宣言終了******************/ + + +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 - d_theta; + 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 x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta()); +} + +void setup() +{ + enc_v = 1; + + //counting on both A&B inputs, 4 ticks per cycle, 16-bit count + //use PB6 PB7 = Nucleo D7 D8 + EncoderInit(&encoder1, &timer1, TIM4, 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); +} + +short coordinateX() +{ + return x * LOCATE_STEP / 2; +} + +short coordinateY() +{ + return y * LOCATE_STEP / 2; +} + +float coordinateTheta() +{ + return theta; +} + +int convert_enc_count(int16_t pulse, int8_t direction) +{ + if(direction == 0) + pulse = pulse - 0xffff -1; + + return pulse; +} + +void erase() +{ + x = 0; + y = 0; + d_theta = theta; +}