fs
Dependents: ARAI45th 3servotest 1stcomp 3rdcompfixstart ... more
locate.cpp
- Committer:
- sakanakuuun
- Date:
- 2016-09-02
- Revision:
- 0:265d8ad19d2a
- Child:
- 1:3513a2fbd81f
File content as of revision 0:265d8ad19d2a:
#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; }