Tk A / Locate

fs

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

locate.cpp@6:27d0384052d2, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 12:16:57 2016 +0000
Revision:
6:27d0384052d2
Parent:
4:ecbb45e84c08 
aas

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 6:27d0384052d2 17 int r, l; //現在の回転数
sakanakuuun 6:27d0384052d2 18 int pr = 0, pl = 0; //前回のステップ数
sakanakuuun 6:27d0384052d2 19 int v = 0; //ステップ速度
sakanakuuun 6:27d0384052d2 20 float xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
sakanakuuun 6:27d0384052d2 21 float theta = 0; //機体角度、x軸正の向きを0とする
sakanakuuun 6:27d0384052d2 22 float d_theta = 0; //erase()関数を使って、そのときのthetaをコピー
sakanakuuun 6:27d0384052d2 23 float virtual_xcount = 0 ,virtual_ycount = 0;
sakanakuuun 6:27d0384052d2 24 int *virtual_v;
sakanakuuun 6:27d0384052d2 25 float virtual_theta = 0;
sakanakuuun 6:27d0384052d2 26 float virtual_ptheta = 0;
sakanakuuun 0:265d8ad19d2a 27 /*************変数宣言終了******************/
sakanakuuun 0:265d8ad19d2a 28
sakanakuuun 0:265d8ad19d2a 29
sakanakuuun 1:3513a2fbd81f 30
sakanakuuun 1:3513a2fbd81f 31
sakanakuuun 4:ecbb45e84c08 32 void setup()
sakanakuuun 4:ecbb45e84c08 33 {
sakanakuuun 4:ecbb45e84c08 34 enc_v = 1;
sakanakuuun 6:27d0384052d2 35
sakanakuuun 4:ecbb45e84c08 36 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 37 //use PB6 PB7 = Nucleo D7 D8
sakanakuuun 4:ecbb45e84c08 38 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 1:3513a2fbd81f 39
sakanakuuun 4:ecbb45e84c08 40 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 41 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 4:ecbb45e84c08 42 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 43 }
sakanakuuun 1:3513a2fbd81f 44
sakanakuuun 4:ecbb45e84c08 45 void setup(int tx, int ty)
sakanakuuun 4:ecbb45e84c08 46 {
sakanakuuun 4:ecbb45e84c08 47 enc_v = 1;
sakanakuuun 6:27d0384052d2 48
sakanakuuun 4:ecbb45e84c08 49 xcount = tx / (LOCATE_STEP / 2);
sakanakuuun 4:ecbb45e84c08 50 ycount = ty / (LOCATE_STEP / 2);
sakanakuuun 4:ecbb45e84c08 51
sakanakuuun 4:ecbb45e84c08 52 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 53 //use PB6 PB7 = Nucleo D7 D8
sakanakuuun 4:ecbb45e84c08 54 EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 55
sakanakuuun 4:ecbb45e84c08 56 //counting on both A&B inputs, 4 ticks per cycle, 16-bit count
sakanakuuun 4:ecbb45e84c08 57 //use PA0 PA1 = Nucleo A0 A1
sakanakuuun 4:ecbb45e84c08 58 EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
sakanakuuun 4:ecbb45e84c08 59 }
sakanakuuun 1:3513a2fbd81f 60
sakanakuuun 6:27d0384052d2 61 void update()
sakanakuuun 0:265d8ad19d2a 62 //位置情報を更新する。r,lはエンコーダから
sakanakuuun 0:265d8ad19d2a 63 {
sakanakuuun 0:265d8ad19d2a 64 count1=__HAL_TIM_GET_COUNTER(&timer1);
sakanakuuun 0:265d8ad19d2a 65 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
sakanakuuun 0:265d8ad19d2a 66 count2=__HAL_TIM_GET_COUNTER(&timer2);
sakanakuuun 6:27d0384052d2 67 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
sakanakuuun 6:27d0384052d2 68
sakanakuuun 0:265d8ad19d2a 69 r = -convert_enc_count(count1, dir1);
sakanakuuun 1:3513a2fbd81f 70 l = -convert_enc_count(count2, dir2);
sakanakuuun 6:27d0384052d2 71
sakanakuuun 6:27d0384052d2 72 theta = (r - l) * ROUND;
sakanakuuun 6:27d0384052d2 73 //theta = (r - l) * ROUND - d_theta;
sakanakuuun 0:265d8ad19d2a 74 v = (r - pr + l - pl);
sakanakuuun 0:265d8ad19d2a 75
sakanakuuun 4:ecbb45e84c08 76 xcount += v * cos(theta);
sakanakuuun 4:ecbb45e84c08 77 ycount += v * sin(theta);
sakanakuuun 0:265d8ad19d2a 78
sakanakuuun 0:265d8ad19d2a 79 pr = r;
sakanakuuun 0:265d8ad19d2a 80 pl = l;
sakanakuuun 0:265d8ad19d2a 81 //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
sakanakuuun 0:265d8ad19d2a 82 pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
sakanakuuun 0:265d8ad19d2a 83 }
sakanakuuun 0:265d8ad19d2a 84
sakanakuuun 4:ecbb45e84c08 85 void update_np()
sakanakuuun 0:265d8ad19d2a 86 {
sakanakuuun 4:ecbb45e84c08 87 count1=__HAL_TIM_GET_COUNTER(&timer1);
sakanakuuun 4:ecbb45e84c08 88 dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
sakanakuuun 4:ecbb45e84c08 89 count2=__HAL_TIM_GET_COUNTER(&timer2);
sakanakuuun 6:27d0384052d2 90 dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
sakanakuuun 6:27d0384052d2 91
sakanakuuun 4:ecbb45e84c08 92 r = -convert_enc_count(count1, dir1);
sakanakuuun 4:ecbb45e84c08 93 l = -convert_enc_count(count2, dir2);
sakanakuuun 6:27d0384052d2 94
sakanakuuun 4:ecbb45e84c08 95 theta = (r - l) * ROUND - d_theta;
sakanakuuun 4:ecbb45e84c08 96 v = (r - pr + l - pl);
sakanakuuun 0:265d8ad19d2a 97
sakanakuuun 4:ecbb45e84c08 98 xcount += v * cos(theta);
sakanakuuun 4:ecbb45e84c08 99 ycount += v * sin(theta);
sakanakuuun 4:ecbb45e84c08 100
sakanakuuun 4:ecbb45e84c08 101 pr = r;
sakanakuuun 4:ecbb45e84c08 102 pl = l;
sakanakuuun 0:265d8ad19d2a 103 }
sakanakuuun 0:265d8ad19d2a 104
sakanakuuun 1:3513a2fbd81f 105 int coordinateX()
sakanakuuun 0:265d8ad19d2a 106 {
sakanakuuun 4:ecbb45e84c08 107 return xcount * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 108 }
sakanakuuun 0:265d8ad19d2a 109
sakanakuuun 1:3513a2fbd81f 110 int coordinateY()
sakanakuuun 0:265d8ad19d2a 111 {
sakanakuuun 4:ecbb45e84c08 112 return ycount * LOCATE_STEP / 2;
sakanakuuun 0:265d8ad19d2a 113 }
sakanakuuun 0:265d8ad19d2a 114
sakanakuuun 0:265d8ad19d2a 115 float coordinateTheta()
sakanakuuun 0:265d8ad19d2a 116 {
sakanakuuun 0:265d8ad19d2a 117 return theta;
sakanakuuun 0:265d8ad19d2a 118 }
sakanakuuun 0:265d8ad19d2a 119
sakanakuuun 0:265d8ad19d2a 120 int convert_enc_count(int16_t pulse, int8_t direction)
sakanakuuun 0:265d8ad19d2a 121 {
sakanakuuun 0:265d8ad19d2a 122 if(direction == 0)
sakanakuuun 0:265d8ad19d2a 123 pulse = pulse - 0xffff -1;
sakanakuuun 6:27d0384052d2 124
sakanakuuun 0:265d8ad19d2a 125 return pulse;
sakanakuuun 0:265d8ad19d2a 126 }
sakanakuuun 0:265d8ad19d2a 127
sakanakuuun 0:265d8ad19d2a 128 void erase()
sakanakuuun 0:265d8ad19d2a 129 {
sakanakuuun 4:ecbb45e84c08 130 xcount = 0;
sakanakuuun 4:ecbb45e84c08 131 ycount = 0;
sakanakuuun 0:265d8ad19d2a 132 d_theta = theta;
sakanakuuun 0:265d8ad19d2a 133 }
sakanakuuun 6:27d0384052d2 134
sakanakuuun 6:27d0384052d2 135 void virtual_setup()
sakanakuuun 6:27d0384052d2 136 {
sakanakuuun 6:27d0384052d2 137 virtual_xcount = 0;
sakanakuuun 6:27d0384052d2 138 virtual_ycount = 0;
sakanakuuun 6:27d0384052d2 139
sakanakuuun 6:27d0384052d2 140 virtual_v = &v;
sakanakuuun 6:27d0384052d2 141
sakanakuuun 6:27d0384052d2 142 virtual_theta = 0;
sakanakuuun 6:27d0384052d2 143 virtual_ptheta = coordinateTheta();
sakanakuuun 6:27d0384052d2 144 }
sakanakuuun 6:27d0384052d2 145
sakanakuuun 6:27d0384052d2 146 void virtual_update()
sakanakuuun 6:27d0384052d2 147 {
sakanakuuun 6:27d0384052d2 148 virtual_theta = coordinateTheta() - virtual_ptheta;
sakanakuuun 6:27d0384052d2 149
sakanakuuun 6:27d0384052d2 150 virtual_xcount += *virtual_v * cos(virtual_theta);
sakanakuuun 6:27d0384052d2 151 virtual_ycount += *virtual_v * sin(virtual_theta);
sakanakuuun 6:27d0384052d2 152 }
sakanakuuun 6:27d0384052d2 153
sakanakuuun 6:27d0384052d2 154 int virtual_coordinateX()
sakanakuuun 6:27d0384052d2 155 {
sakanakuuun 6:27d0384052d2 156 return virtual_xcount * LOCATE_STEP / 2;
sakanakuuun 6:27d0384052d2 157 }
sakanakuuun 6:27d0384052d2 158
sakanakuuun 6:27d0384052d2 159 int virtual_coordinateY()
sakanakuuun 6:27d0384052d2 160 {
sakanakuuun 6:27d0384052d2 161 return virtual_ycount * LOCATE_STEP / 2;
sakanakuuun 6:27d0384052d2 162 }
sakanakuuun 6:27d0384052d2 163
sakanakuuun 6:27d0384052d2 164 float virtual_coordinateTheta()
sakanakuuun 6:27d0384052d2 165 {
sakanakuuun 6:27d0384052d2 166 return virtual_theta;
sakanakuuun 6:27d0384052d2 167 }