Tk A / Locate

fs

Dependents:   ARAI45th 3servotest 1stcomp 3rdcompfixstart ... more

locate.cpp@0:265d8ad19d2a, 2016-09-02 (annotated)

Committer:
sakanakuuun
Date:
Fri Sep 02 12:40:36 2016 +0000
Revision:
0:265d8ad19d2a
Child:
1:3513a2fbd81f
fs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:265d8ad19d2a 1 #include <math.h>
sakanakuuun 0:265d8ad19d2a 2 #include "mbed.h"
sakanakuuun 0:265d8ad19d2a 3 #include "Encoder.h"
sakanakuuun 0:265d8ad19d2a 4 #include "locate.h"
sakanakuuun 0:265d8ad19d2a 5
sakanakuuun 0:265d8ad19d2a 6
sakanakuuun 0:265d8ad19d2a 7 /********以下グローバル変数宣言**************/
sakanakuuun 0:265d8ad19d2a 8
sakanakuuun 0:265d8ad19d2a 9 TIM_Encoder_InitTypeDef encoder1, encoder2; //Encoderライブラリで使う
sakanakuuun 0:265d8ad19d2a 10 TIM_HandleTypeDef timer1, timer2; //Encoderライブラリで使う
sakanakuuun 0:265d8ad19d2a 11 uint16_t count1=0, count2=0; //Encoderライブラリで使う
sakanakuuun 0:265d8ad19d2a 12 int8_t dir1, dir2; //Encoderライブラリで使う
sakanakuuun 0:265d8ad19d2a 13
sakanakuuun 0:265d8ad19d2a 14 Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信
sakanakuuun 0:265d8ad19d2a 15 DigitalOut enc_v(PC_7); //エンコーダの電源
sakanakuuun 0:265d8ad19d2a 16
sakanakuuun 0:265d8ad19d2a 17 int r, l; //現在の回転数
sakanakuuun 0:265d8ad19d2a 18 int pr = 0, pl = 0; //前回のステップ数
sakanakuuun 0:265d8ad19d2a 19 short v = 0; //ステップ速度
sakanakuuun 0:265d8ad19d2a 20 float x = 0, y = 0; //xy方向に進んだ距離(m換算なし)
sakanakuuun 0:265d8ad19d2a 21 float theta = 0; //機体角度、x軸正の向きを0とする
sakanakuuun 0:265d8ad19d2a 22 float d_theta = 0; //erase()関数を使って、そのときのthetaをコピー
sakanakuuun 0:265d8ad19d2a 23
sakanakuuun 0:265d8ad19d2a 24 /*************変数宣言終了******************/
sakanakuuun 0:265d8ad19d2a 25
sakanakuuun 0:265d8ad19d2a 26
sakanakuuun 0:265d8ad19d2a 27 void update ()
sakanakuuun 0:265d8ad19d2a 28 //位置情報を更新する。r,lはエンコーダから
sakanakuuun 0:265d8ad19d2a 29 {
sakanakuuun 0:265d8ad19d2a 30 count1=__HAL_TIM_GET_COUNTER(&timer1);
sakanakuuun 0:265d8ad19d2a 31 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
sakanakuuun 0:265d8ad19d2a 32 count2=__HAL_TIM_GET_COUNTER(&timer2);
sakanakuuun 0:265d8ad19d2a 33 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
sakanakuuun 0:265d8ad19d2a 34
sakanakuuun 0:265d8ad19d2a 35 r = -convert_enc_count(count1, dir1);
sakanakuuun 0:265d8ad19d2a 36 l = convert_enc_count(count2, dir2);
sakanakuuun 0:265d8ad19d2a 37
sakanakuuun 0:265d8ad19d2a 38 theta = (r - l) * ROUND - d_theta;
sakanakuuun 0:265d8ad19d2a 39 v = (r - pr + l - pl);
sakanakuuun 0:265d8ad19d2a 40
sakanakuuun 0:265d8ad19d2a 41 x += v * cos(theta);
sakanakuuun 0:265d8ad19d2a 42 y += v * sin(theta);
sakanakuuun 0:265d8ad19d2a 43
sakanakuuun 0:265d8ad19d2a 44 pr = r;
sakanakuuun 0:265d8ad19d2a 45 pl = l;
sakanakuuun 0:265d8ad19d2a 46 //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
sakanakuuun 0:265d8ad19d2a 47 pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
sakanakuuun 0:265d8ad19d2a 48 }
sakanakuuun 0:265d8ad19d2a 49
sakanakuuun 0:265d8ad19d2a 50 void setup()
sakanakuuun 0:265d8ad19d2a 51 {
sakanakuuun 0:265d8ad19d2a 52 enc_v = 1;
sakanakuuun 0:265d8ad19d2a 53
sakanakuuun 0:265d8ad19d2a 54 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 0:265d8ad19d2a 55 //use PB6 PB7 = Nucleo D7 D8
sakanakuuun 0:265d8ad19d2a 56 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 0:265d8ad19d2a 57
sakanakuuun 0:265d8ad19d2a 58 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 0:265d8ad19d2a 59 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 0:265d8ad19d2a 60 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 0:265d8ad19d2a 61 }
sakanakuuun 0:265d8ad19d2a 62
sakanakuuun 0:265d8ad19d2a 63 short coordinateX()
sakanakuuun 0:265d8ad19d2a 64 {
sakanakuuun 0:265d8ad19d2a 65 return x * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 66 }
sakanakuuun 0:265d8ad19d2a 67
sakanakuuun 0:265d8ad19d2a 68 short coordinateY()
sakanakuuun 0:265d8ad19d2a 69 {
sakanakuuun 0:265d8ad19d2a 70 return y * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 71 }
sakanakuuun 0:265d8ad19d2a 72
sakanakuuun 0:265d8ad19d2a 73 float coordinateTheta()
sakanakuuun 0:265d8ad19d2a 74 {
sakanakuuun 0:265d8ad19d2a 75 return theta;
sakanakuuun 0:265d8ad19d2a 76 }
sakanakuuun 0:265d8ad19d2a 77
sakanakuuun 0:265d8ad19d2a 78 int convert_enc_count(int16_t pulse, int8_t direction)
sakanakuuun 0:265d8ad19d2a 79 {
sakanakuuun 0:265d8ad19d2a 80 if(direction == 0)
sakanakuuun 0:265d8ad19d2a 81 pulse = pulse - 0xffff -1;
sakanakuuun 0:265d8ad19d2a 82
sakanakuuun 0:265d8ad19d2a 83 return pulse;
sakanakuuun 0:265d8ad19d2a 84 }
sakanakuuun 0:265d8ad19d2a 85
sakanakuuun 0:265d8ad19d2a 86 void erase()
sakanakuuun 0:265d8ad19d2a 87 {
sakanakuuun 0:265d8ad19d2a 88 x = 0;
sakanakuuun 0:265d8ad19d2a 89 y = 0;
sakanakuuun 0:265d8ad19d2a 90 d_theta = theta;
sakanakuuun 0:265d8ad19d2a 91 }