tetsuya yoshikawa / Mbed 2 deprecated Line_Trace_No_collision_Light_control_longer_dis

test

Dependencies:   S11059 VL6180X m3pi mbed

Fork of tc_agent by Ichiro Maruta

main.cpp@4:0e6997707f43, 2017-03-29 (annotated)

Committer:
tennisbaca
Date:
Wed Mar 29 03:40:18 2017 +0000
Revision:
4:0e6997707f43
Parent:
3:f1ddc26da601 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maruta 2:3ebca956fd36 1 #include "VL6180X.h"
sburg 0:2508f38e90fe 2 #include "mbed.h"
tennisbaca 1:6340d62d759f 3 #include "m3pi.h"
maruta 2:3ebca956fd36 4 #include "S11059.h"
sburg 0:2508f38e90fe 5
tennisbaca 4:0e6997707f43 6 //必要なオブジェクトを作る
maruta 2:3ebca956fd36 7 VL6180x rf(p28, p27); //I2C sda and scl
sburg 0:2508f38e90fe 8 Serial pc(USBTX, USBRX); //USB serial
maruta 2:3ebca956fd36 9 S11059 col(p28,p27);
tennisbaca 1:6340d62d759f 10 m3pi m3pi;
tennisbaca 1:6340d62d759f 11 Timer t;
tennisbaca 4:0e6997707f43 12
tennisbaca 3:f1ddc26da601 13 // Minimum and maximum motor speeds
tennisbaca 4:0e6997707f43 14 #define MAX 1 //速度の最大値
tennisbaca 4:0e6997707f43 15 #define MIN 0 //速度の最小値（バックはしません）
tennisbaca 3:f1ddc26da601 16
tennisbaca 4:0e6997707f43 17 // PID terms ライントレースに必要なPID項
tennisbaca 4:0e6997707f43 18 //このプログラムでは車両の下に存在する黒いテープの車両の中心からのズレを状態として、PIDで制御します。
tennisbaca 3:f1ddc26da601 19 #define P_TERM 1
tennisbaca 3:f1ddc26da601 20 #define I_TERM 0
tennisbaca 3:f1ddc26da601 21 #define D_TERM 20
tennisbaca 3:f1ddc26da601 22
tennisbaca 4:0e6997707f43 23 // PID terms 車両が衝突回避を行っているときのライントレースのPID項（速度がおそくなるから値を変えてます）
tennisbaca 3:f1ddc26da601 24 #define P_TERM_COL 1
tennisbaca 3:f1ddc26da601 25 #define I_TERM_COL 0
tennisbaca 3:f1ddc26da601 26 #define D_TERM_COL 20
tennisbaca 3:f1ddc26da601 27
tennisbaca 4:0e6997707f43 28 //リニア信号機，前方車両，ライントレースから計算される速度から車両の速度（左右のタイヤにかかる電圧）を決定するための最も遅い速度のインデックスを返す関数
tennisbaca 4:0e6997707f43 29 int min_index(float a,float b,float c,float d,float e,float f){
tennisbaca 4:0e6997707f43 30 float sum1 = a + b;
tennisbaca 4:0e6997707f43 31 float sum2 = c + d;
tennisbaca 4:0e6997707f43 32 float sum3 = e + f;
tennisbaca 4:0e6997707f43 33
tennisbaca 4:0e6997707f43 34 if(sum1<sum2 && sum1<sum3){
tennisbaca 4:0e6997707f43 35 return 1;
tennisbaca 4:0e6997707f43 36 }else if(sum1>sum2 && sum3>sum2){
tennisbaca 4:0e6997707f43 37 return 2;
tennisbaca 4:0e6997707f43 38 }else{
tennisbaca 4:0e6997707f43 39 return 3;
tennisbaca 4:0e6997707f43 40 }
tennisbaca 4:0e6997707f43 41 }
sburg 0:2508f38e90fe 42
sburg 0:2508f38e90fe 43 int main() {
tennisbaca 1:6340d62d759f 44 m3pi.cls();
tennisbaca 3:f1ddc26da601 45 //t_for_music.start();
tennisbaca 4:0e6997707f43 46 //一秒間スクリーンに文字を表示する
tennisbaca 4:0e6997707f43 47 m3pi.locate(0,0); //指定した位置（0,0）に
tennisbaca 4:0e6997707f43 48 m3pi.printf("Battery"); //指定した文字"Battery"を表示する
tennisbaca 1:6340d62d759f 49 m3pi.locate(0,1);
tennisbaca 3:f1ddc26da601 50 float battery = m3pi.battery();
tennisbaca 3:f1ddc26da601 51 m3pi.printf("%.0fmV",battery*1000);
tennisbaca 4:0e6997707f43 52 wait(1);
tennisbaca 3:f1ddc26da601 53
tennisbaca 3:f1ddc26da601 54
tennisbaca 3:f1ddc26da601 55 // distance sensor
tennisbaca 4:0e6997707f43 56 int reading; //前方車両との距離を格納する変数
tennisbaca 4:0e6997707f43 57 float time[2]; //サンプリング時間を一定にするための配列
tennisbaca 4:0e6997707f43 58 //おまじない
tennisbaca 4:0e6997707f43 59 m3pi.cls();
maruta 2:3ebca956fd36 60 rf.VL6180xInit();
maruta 2:3ebca956fd36 61 rf.VL6180xDefautSettings();
tennisbaca 4:0e6997707f43 62
tennisbaca 4:0e6997707f43 63 int check = 1; //サンプリリング時間が乱れた時にシステムを止めるための変数
tennisbaca 4:0e6997707f43 64 int Sensor_num = 0;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
tennisbaca 4:0e6997707f43 65 int reading_history[3]; //ノイズがあるため前方車両との距離は最新の3つの値の平均をとるのでそのための配列
tennisbaca 3:f1ddc26da601 66 int i;
tennisbaca 4:0e6997707f43 67 //
tennisbaca 3:f1ddc26da601 68 for(i = 0;i<3;i++){
tennisbaca 4:0e6997707f43 69 reading_history[i] = 255;//初期値はセンサの最大値
tennisbaca 3:f1ddc26da601 70 }
tennisbaca 4:0e6997707f43 71 float ave_reading = 255;
tennisbaca 3:f1ddc26da601 72
tennisbaca 4:0e6997707f43 73 //おまじない
tennisbaca 3:f1ddc26da601 74 rf.triggerDistance();
tennisbaca 4:0e6997707f43 75 m3pi.sensor_auto_calibrate();
tennisbaca 3:f1ddc26da601 76
tennisbaca 3:f1ddc26da601 77
tennisbaca 3:f1ddc26da601 78 // variables for PID
tennisbaca 4:0e6997707f43 79 //左右のタイヤにかかる電圧を格納する変数
tennisbaca 4:0e6997707f43 80 float right;//右のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
tennisbaca 4:0e6997707f43 81 float left;//左のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
tennisbaca 4:0e6997707f43 82
tennisbaca 4:0e6997707f43 83 float right1;//ライントレースからの右のタイヤの電圧
tennisbaca 4:0e6997707f43 84 float left1;//ライントレースからの左のタイヤの電圧
tennisbaca 4:0e6997707f43 85 float right2;//前方車両から計算される右のタイヤの電圧
tennisbaca 4:0e6997707f43 86 float left2;//前方車両から計算される左のタイヤの電圧
tennisbaca 4:0e6997707f43 87 float right3;//リニア信号機から受け取る右のタイヤの電圧
tennisbaca 4:0e6997707f43 88 float left3;//リニア信号機から受け取る左のタイヤの電圧
tennisbaca 4:0e6997707f43 89
tennisbaca 4:0e6997707f43 90 int mode = 1;
tennisbaca 4:0e6997707f43 91
tennisbaca 4:0e6997707f43 92
tennisbaca 4:0e6997707f43 93 //ライントレースに必要な変数
tennisbaca 4:0e6997707f43 94 float current_pos_of_line = 0.0;//現在の車両のライン上のポジション
tennisbaca 4:0e6997707f43 95 float previous_pos_of_line = 0.0;//過去の車両のライン上のポジション
tennisbaca 3:f1ddc26da601 96 float derivative,proportional,integral = 0;
tennisbaca 3:f1ddc26da601 97 float power;
tennisbaca 3:f1ddc26da601 98 float power_collision;
tennisbaca 3:f1ddc26da601 99 float speed = MAX;
tennisbaca 3:f1ddc26da601 100
tennisbaca 3:f1ddc26da601 101
tennisbaca 3:f1ddc26da601 102 //for color sensor
tennisbaca 3:f1ddc26da601 103 int bl=0;
tennisbaca 3:f1ddc26da601 104 int gr=0;
tennisbaca 4:0e6997707f43 105 int re=0;
tennisbaca 3:f1ddc26da601 106
tennisbaca 4:0e6997707f43 107 t.start();//サンプリング時間を一定に保つためタイマーを使い時間を計るので、タイマーをスタートさせる。
tennisbaca 4:0e6997707f43 108 time[0] = t.read();//スタート時間を格納する
tennisbaca 3:f1ddc26da601 109
sburg 0:2508f38e90fe 110 while(1) {
tennisbaca 4:0e6997707f43 111
tennisbaca 4:0e6997707f43 112 Sensor_num = Sensor_num + 1;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
tennisbaca 1:6340d62d759f 113 m3pi.cls();
tennisbaca 3:f1ddc26da601 114 //t.start();
tennisbaca 3:f1ddc26da601 115 //time[0] = t.read();
tennisbaca 3:f1ddc26da601 116 col.update();
tennisbaca 4:0e6997707f43 117 //カラーセンサからのRGB値をそれぞれの変数に格納する。
tennisbaca 3:f1ddc26da601 118 bl=col.b;
tennisbaca 3:f1ddc26da601 119 gr=col.g;
tennisbaca 3:f1ddc26da601 120 re=col.r;
tennisbaca 3:f1ddc26da601 121
tennisbaca 4:0e6997707f43 122 //左右のモータの電圧を初期化
tennisbaca 4:0e6997707f43 123 right1 = MAX;
tennisbaca 4:0e6997707f43 124 right2 = MAX;
tennisbaca 4:0e6997707f43 125 right3 = MAX;
tennisbaca 4:0e6997707f43 126 left1 = MAX;
tennisbaca 4:0e6997707f43 127 left2 = MAX;
tennisbaca 4:0e6997707f43 128 left3 = MAX;
tennisbaca 4:0e6997707f43 129
tennisbaca 4:0e6997707f43 130 //ライントレースするための左右のモータの電圧の計算
tennisbaca 3:f1ddc26da601 131 // Get the position of the line.
tennisbaca 3:f1ddc26da601 132 current_pos_of_line = m3pi.line_position();
tennisbaca 3:f1ddc26da601 133 proportional = current_pos_of_line;
tennisbaca 3:f1ddc26da601 134
tennisbaca 3:f1ddc26da601 135 // Compute the derivative
tennisbaca 3:f1ddc26da601 136 derivative = current_pos_of_line - previous_pos_of_line;
tennisbaca 3:f1ddc26da601 137
tennisbaca 3:f1ddc26da601 138 // Compute the integral
tennisbaca 3:f1ddc26da601 139 integral += proportional;
tennisbaca 3:f1ddc26da601 140
tennisbaca 3:f1ddc26da601 141 // Remember the last position.
tennisbaca 3:f1ddc26da601 142 previous_pos_of_line = current_pos_of_line;
tennisbaca 3:f1ddc26da601 143
tennisbaca 3:f1ddc26da601 144 // Compute the power
tennisbaca 3:f1ddc26da601 145 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
tennisbaca 3:f1ddc26da601 146 power_collision = (proportional * (P_TERM_COL) ) + (integral*(I_TERM_COL)) + (derivative*(D_TERM_COL)) ;
tennisbaca 3:f1ddc26da601 147
tennisbaca 3:f1ddc26da601 148 // Compute new speeds if there is nothing ahead
tennisbaca 4:0e6997707f43 149 if(ave_reading >= 190 && (gr+re) <= 10000){
tennisbaca 4:0e6997707f43 150 right1 = speed+power;
tennisbaca 4:0e6997707f43 151 left1 = speed-power;
tennisbaca 3:f1ddc26da601 152
tennisbaca 3:f1ddc26da601 153 // limit checks
tennisbaca 4:0e6997707f43 154 if (right1 < MIN)
tennisbaca 4:0e6997707f43 155 right1 = MIN;
tennisbaca 4:0e6997707f43 156 else if (right1 > MAX)
tennisbaca 4:0e6997707f43 157 right1 = MAX;
tennisbaca 4:0e6997707f43 158 if (left1 < MIN)
tennisbaca 4:0e6997707f43 159 left1 = MIN;
tennisbaca 4:0e6997707f43 160 else if (left1 > MAX)
tennisbaca 4:0e6997707f43 161 left1 = MAX;
tennisbaca 3:f1ddc26da601 162 }
tennisbaca 3:f1ddc26da601 163
tennisbaca 3:f1ddc26da601 164 // get distance once every three times because of the processing speed
tennisbaca 4:0e6997707f43 165 //3ステップに一回、車両は前方車両との距離を取得する。
tennisbaca 3:f1ddc26da601 166 if((Sensor_num %3)== 1){
tennisbaca 3:f1ddc26da601 167 reading = rf.pollDistance();
tennisbaca 3:f1ddc26da601 168 rf.triggerDistance();
tennisbaca 3:f1ddc26da601 169 if(reading < 90 && reading > 0.1){
tennisbaca 3:f1ddc26da601 170 /*m3pi.locate(0,0);
tennisbaca 3:f1ddc26da601 171 m3pi.printf("%dmm",reading);
tennisbaca 3:f1ddc26da601 172 wait(10);*/
tennisbaca 3:f1ddc26da601 173 reading = 90;
tennisbaca 4:0e6997707f43 174 }else if(reading < 0.1){//たまにセンサの不具合により距離0を返すときがあるのでそれを除外。
tennisbaca 3:f1ddc26da601 175 reading = 255;
tennisbaca 3:f1ddc26da601 176 }
tennisbaca 3:f1ddc26da601 177 reading_history[0] = reading_history[1];
tennisbaca 3:f1ddc26da601 178 reading_history[1] = reading_history[2];
tennisbaca 3:f1ddc26da601 179 reading_history[2] = reading;
tennisbaca 4:0e6997707f43 180 ave_reading = float(reading_history[0]+reading_history[1]+reading_history[2])/3;//三回取得した平均値を前方車両との距離とする
tennisbaca 3:f1ddc26da601 181 }
tennisbaca 3:f1ddc26da601 182
tennisbaca 3:f1ddc26da601 183 // if distance is too close, change the speed
tennisbaca 4:0e6997707f43 184 //前方車両との距離から左右のモータの電圧を計算
tennisbaca 3:f1ddc26da601 185 if(ave_reading < 190){
tennisbaca 4:0e6997707f43 186 right2 = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
tennisbaca 4:0e6997707f43 187 left2 = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
tennisbaca 3:f1ddc26da601 188 // limit checks
tennisbaca 4:0e6997707f43 189 if (right2 < MIN)
tennisbaca 4:0e6997707f43 190 right2 = MIN;
tennisbaca 4:0e6997707f43 191 else if (right2 > MAX)
tennisbaca 4:0e6997707f43 192 right2 = MAX;
tennisbaca 3:f1ddc26da601 193
tennisbaca 4:0e6997707f43 194 if (left2 < MIN)
tennisbaca 4:0e6997707f43 195 left2 = MIN;
tennisbaca 4:0e6997707f43 196 else if (left2 > MAX)
tennisbaca 4:0e6997707f43 197 left2 = MAX;
tennisbaca 3:f1ddc26da601 198 }
tennisbaca 3:f1ddc26da601 199
tennisbaca 3:f1ddc26da601 200 //if light is on its left side, use light to control
tennisbaca 4:0e6997707f43 201 //カラーセンサから得られるRGB値から左右のモータの電圧を計算する。
tennisbaca 4:0e6997707f43 202 if((gr+re) > 10000){
tennisbaca 3:f1ddc26da601 203 // if(Sensor_num%3 == 2){
tennisbaca 3:f1ddc26da601 204 double light_position = (((double)gr)/((double) (re+gr)));
tennisbaca 3:f1ddc26da601 205 double light_speed = light_position*(10.0/7.0)-(2.0/7.0);
tennisbaca 4:0e6997707f43 206 right3 = light_speed + light_speed*power_collision;
tennisbaca 4:0e6997707f43 207 left3 = light_speed - light_speed*power_collision;
tennisbaca 3:f1ddc26da601 208
tennisbaca 3:f1ddc26da601 209 // limit checks
tennisbaca 4:0e6997707f43 210 if (right3 < MIN)
tennisbaca 4:0e6997707f43 211 right3 = MIN;
tennisbaca 4:0e6997707f43 212 else if (right3 > MAX)
tennisbaca 4:0e6997707f43 213 right3 = MAX;
tennisbaca 3:f1ddc26da601 214
tennisbaca 4:0e6997707f43 215 if (left3 < MIN)
tennisbaca 4:0e6997707f43 216 left3 = MIN;
tennisbaca 4:0e6997707f43 217 else if (left3 > MAX)
tennisbaca 4:0e6997707f43 218 left3 = MAX;
tennisbaca 3:f1ddc26da601 219 // }
tennisbaca 3:f1ddc26da601 220 }
tennisbaca 4:0e6997707f43 221
tennisbaca 4:0e6997707f43 222 //上記で計算された左右のモータにかかる電圧（３つ）から最終的な電圧を計算する。
tennisbaca 4:0e6997707f43 223 mode = min_index(right1,left1,right2,left2,right3,left3);
tennisbaca 4:0e6997707f43 224
tennisbaca 4:0e6997707f43 225 if(mode == 1){//（ライントレースから求められた値）
tennisbaca 4:0e6997707f43 226 right = right1;
tennisbaca 4:0e6997707f43 227 left = left1;
tennisbaca 4:0e6997707f43 228 }else if(mode == 2){//（前方車両との距離から求められた値）
tennisbaca 4:0e6997707f43 229 right = right2;
tennisbaca 4:0e6997707f43 230 left = left2;
tennisbaca 4:0e6997707f43 231 }else{//（カラーセンサのRGB値より求められた値）
tennisbaca 4:0e6997707f43 232 right = right3;
tennisbaca 4:0e6997707f43 233 left = left3;
tennisbaca 4:0e6997707f43 234 }
tennisbaca 4:0e6997707f43 235
tennisbaca 4:0e6997707f43 236 //求められた左右のモータの電圧を入力
tennisbaca 3:f1ddc26da601 237 m3pi.left_motor(left);
tennisbaca 3:f1ddc26da601 238 m3pi.right_motor(right);
tennisbaca 3:f1ddc26da601 239 //wait_ms(1);
tennisbaca 4:0e6997707f43 240
tennisbaca 4:0e6997707f43 241 //サンプル時間が一定に保たれているかチェック
tennisbaca 3:f1ddc26da601 242 while(t.read() - time[0] < 0.005){
tennisbaca 3:f1ddc26da601 243 check = 0;
tennisbaca 3:f1ddc26da601 244 }
tennisbaca 4:0e6997707f43 245 if(check == 1){//保たれていなければストップ
tennisbaca 3:f1ddc26da601 246 m3pi.locate(0,0);
tennisbaca 3:f1ddc26da601 247 m3pi.printf("%dmm",reading);
tennisbaca 3:f1ddc26da601 248 wait(1);
tennisbaca 3:f1ddc26da601 249 m3pi.stop();
tennisbaca 3:f1ddc26da601 250 }
tennisbaca 3:f1ddc26da601 251 check = 1;
tennisbaca 4:0e6997707f43 252
tennisbaca 4:0e6997707f43 253 //タイマーストップ
tennisbaca 3:f1ddc26da601 254 t.stop();
tennisbaca 4:0e6997707f43 255 //タイマースタート（次のステップのサンプル時間をチェックするため）
tennisbaca 1:6340d62d759f 256 t.start();
tennisbaca 4:0e6997707f43 257 time[0] = t.read();//（スタート時間をチェック）
sburg 0:2508f38e90fe 258 }
sburg 0:2508f38e90fe 259 }