Tk A / Locate

fs

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

locate.cpp@4:ecbb45e84c08, 2016-09-07 (annotated)

Committer:
sakanakuuun
Date:
Wed Sep 07 04:57:39 2016 +0000
Revision:
4:ecbb45e84c08
Parent:
1:3513a2fbd81f
Child:
6:27d0384052d2
aaaaaaaaa

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 4:ecbb45e84c08 20 float xcount = 0, ycount = 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 1:3513a2fbd81f 27
sakanakuuun 1:3513a2fbd81f 28
sakanakuuun 4:ecbb45e84c08 29 void setup()
sakanakuuun 4:ecbb45e84c08 30 {
sakanakuuun 4:ecbb45e84c08 31 enc_v = 1;
sakanakuuun 4:ecbb45e84c08 32
sakanakuuun 4:ecbb45e84c08 33 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 34 //use PB6 PB7 = Nucleo D7 D8
sakanakuuun 4:ecbb45e84c08 35 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 1:3513a2fbd81f 36
sakanakuuun 4:ecbb45e84c08 37 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 38 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 4:ecbb45e84c08 39 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 40 }
sakanakuuun 1:3513a2fbd81f 41
sakanakuuun 4:ecbb45e84c08 42 void setup(int tx, int ty)
sakanakuuun 4:ecbb45e84c08 43 {
sakanakuuun 4:ecbb45e84c08 44 enc_v = 1;
sakanakuuun 4:ecbb45e84c08 45
sakanakuuun 4:ecbb45e84c08 46 xcount = tx / (LOCATE_STEP / 2);
sakanakuuun 4:ecbb45e84c08 47 ycount = ty / (LOCATE_STEP / 2);
sakanakuuun 4:ecbb45e84c08 48
sakanakuuun 4:ecbb45e84c08 49 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 50 //use PB6 PB7 = Nucleo D7 D8
sakanakuuun 4:ecbb45e84c08 51 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 52
sakanakuuun 4:ecbb45e84c08 53 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 54 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 4:ecbb45e84c08 55 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 56 }
sakanakuuun 1:3513a2fbd81f 57
sakanakuuun 0:265d8ad19d2a 58 void update ()
sakanakuuun 0:265d8ad19d2a 59 //位置情報を更新する。r,lはエンコーダから
sakanakuuun 0:265d8ad19d2a 60 {
sakanakuuun 0:265d8ad19d2a 61 count1=__HAL_TIM_GET_COUNTER(&timer1);
sakanakuuun 0:265d8ad19d2a 62 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
sakanakuuun 0:265d8ad19d2a 63 count2=__HAL_TIM_GET_COUNTER(&timer2);
sakanakuuun 0:265d8ad19d2a 64 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
sakanakuuun 0:265d8ad19d2a 65
sakanakuuun 0:265d8ad19d2a 66 r = -convert_enc_count(count1, dir1);
sakanakuuun 1:3513a2fbd81f 67 l = -convert_enc_count(count2, dir2);
sakanakuuun 0:265d8ad19d2a 68
sakanakuuun 0:265d8ad19d2a 69 theta = (r - l) * ROUND - d_theta;
sakanakuuun 0:265d8ad19d2a 70 v = (r - pr + l - pl);
sakanakuuun 0:265d8ad19d2a 71
sakanakuuun 4:ecbb45e84c08 72 xcount += v * cos(theta);
sakanakuuun 4:ecbb45e84c08 73 ycount += v * sin(theta);
sakanakuuun 0:265d8ad19d2a 74
sakanakuuun 0:265d8ad19d2a 75 pr = r;
sakanakuuun 0:265d8ad19d2a 76 pl = l;
sakanakuuun 0:265d8ad19d2a 77 //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
sakanakuuun 0:265d8ad19d2a 78 pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
sakanakuuun 0:265d8ad19d2a 79 }
sakanakuuun 0:265d8ad19d2a 80
sakanakuuun 4:ecbb45e84c08 81 void update_np()
sakanakuuun 0:265d8ad19d2a 82 {
sakanakuuun 4:ecbb45e84c08 83 count1=__HAL_TIM_GET_COUNTER(&timer1);
sakanakuuun 4:ecbb45e84c08 84 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
sakanakuuun 4:ecbb45e84c08 85 count2=__HAL_TIM_GET_COUNTER(&timer2);
sakanakuuun 4:ecbb45e84c08 86 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
sakanakuuun 0:265d8ad19d2a 87
sakanakuuun 4:ecbb45e84c08 88 r = -convert_enc_count(count1, dir1);
sakanakuuun 4:ecbb45e84c08 89 l = -convert_enc_count(count2, dir2);
sakanakuuun 4:ecbb45e84c08 90
sakanakuuun 4:ecbb45e84c08 91 theta = (r - l) * ROUND - d_theta;
sakanakuuun 4:ecbb45e84c08 92 v = (r - pr + l - pl);
sakanakuuun 0:265d8ad19d2a 93
sakanakuuun 4:ecbb45e84c08 94 xcount += v * cos(theta);
sakanakuuun 4:ecbb45e84c08 95 ycount += v * sin(theta);
sakanakuuun 4:ecbb45e84c08 96
sakanakuuun 4:ecbb45e84c08 97 pr = r;
sakanakuuun 4:ecbb45e84c08 98 pl = l;
sakanakuuun 0:265d8ad19d2a 99 }
sakanakuuun 0:265d8ad19d2a 100
sakanakuuun 1:3513a2fbd81f 101 int coordinateX()
sakanakuuun 0:265d8ad19d2a 102 {
sakanakuuun 4:ecbb45e84c08 103 return xcount * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 104 }
sakanakuuun 0:265d8ad19d2a 105
sakanakuuun 1:3513a2fbd81f 106 int coordinateY()
sakanakuuun 0:265d8ad19d2a 107 {
sakanakuuun 4:ecbb45e84c08 108 return ycount * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 109 }
sakanakuuun 0:265d8ad19d2a 110
sakanakuuun 0:265d8ad19d2a 111 float coordinateTheta()
sakanakuuun 0:265d8ad19d2a 112 {
sakanakuuun 0:265d8ad19d2a 113 return theta;
sakanakuuun 0:265d8ad19d2a 114 }
sakanakuuun 0:265d8ad19d2a 115
sakanakuuun 0:265d8ad19d2a 116 int convert_enc_count(int16_t pulse, int8_t direction)
sakanakuuun 0:265d8ad19d2a 117 {
sakanakuuun 0:265d8ad19d2a 118 if(direction == 0)
sakanakuuun 0:265d8ad19d2a 119 pulse = pulse - 0xffff -1;
sakanakuuun 0:265d8ad19d2a 120
sakanakuuun 0:265d8ad19d2a 121 return pulse;
sakanakuuun 0:265d8ad19d2a 122 }
sakanakuuun 0:265d8ad19d2a 123
sakanakuuun 0:265d8ad19d2a 124 void erase()
sakanakuuun 0:265d8ad19d2a 125 {
sakanakuuun 4:ecbb45e84c08 126 xcount = 0;
sakanakuuun 4:ecbb45e84c08 127 ycount = 0;
sakanakuuun 0:265d8ad19d2a 128 d_theta = theta;
sakanakuuun 0:265d8ad19d2a 129 }