Final project report(TSP智慧型自走車) - YYBS/ee240500 GitHub Wiki

TITLE

TSP智慧型自走車

Introduction

TSP的目的find a minimum TSP (Travelling Salesman Problem) tour，就是圖上有幾個城鎮，而商人要搭著車車跑過所有城鎮的最短路徑。 實現方法，舉例:用紅色圓形當作起始城鎮和終點城鎮，而藍色圓形當過境城市，兩個城鎮間用綠色線條代表路徑。 #如何實現 這邊分成四個部份，第一個部份是影像處理，第二個部份是演算法，第三個部份是Xbee的傳輸，第四個部份則是車子接受訊號並產生反應，我的部分主要負責演算法和Xbee和車子的控制。

  1. 首先，取得圖片的方式是用webcam空拍要走的圖，在把圖片丟去影像處理，影像處理使用RGB過濾，因為總共有三種顏色，我們可以先濾紅色在濾藍色最後濾綠色，一開始濾紅色，會得到一個白色圓形，這時候我們還沒得到座標，所以使用判斷圓形的function去定位圓心座標，這就是起始座標，接來濾藍色，在做如上步驟，剩下來比較麻煩的就是綠色線段的判斷，綠色的線段判斷非常重要，因為這是兩個點之間有沒有連線的依據。 綠色線段先用綠色濾圖片，可以在得到一堆白線，但我們不可能只憑白線的座標就判斷兩點之間有無連線，我們想了一個方法，當我們有了圓心座標時，我們從圓心座標畫一個範圍，以方形為範圍也可以，然後沿著方形範圍去尋找切到白線的座標，然後把她存起來，這樣有個好處，假設一個圓形有四條線連接，那我們用這個找法，起碼能找到四個點，但實際上會找到更多的點，因為綠色線條是有寬度的，不過對我們後面要說的方法是不會產生影響的，找完全部圓心周圍方形的點後，那我們可以開始判斷兩個圓形之間是否有連線，這時候就很輕鬆的可以判斷了，假設現在有兩個圓形，每個圓形的方形周圍又有好多點，那我們各找每個圓形方形周圍的點，然後把這兩點之間做search，search的方式，是從一個點的座標，用兩點之間的斜率慢慢search，假設要40次移動才能到另外一個點，那我們判斷search的40個點裡有幾個點有包含到綠色線段，當達到一個界限後，我們就判斷這兩點之間有連線，找完所有兩個點之間的組合，那我們就能找出所有點之間的連結。接下來是要把座標存到Txt檔中，txt檔中存的資訊有所有點的座標，和所有點的連結。
  2. 接下來是演算法，演算法我使用的是branch and bound，延著每條路去走下一點，一直走到完，如果走得完，則代表找到一條能夠走完所有點的路，這時候就去判斷是否為最短路徑，如果比先前走完的路徑還短的話，則把這條路徑存起來，就這樣不斷把所有可能的組合走完，講到這裡，找路的方法跟窮舉法是一樣的，不過我多加了一個限制，如果當下在找的路徑長度已經比最短路徑還長的話，則代表這條路不需要繼續走完，而轉而backtrack其他路徑，這會讓找到最佳路徑的時間更短，演算法是在板子上跑。
  3. 接下來是Xbee傳輸的過程，我參考之前lab用到的code，但是那個code的Xbee是用char這個型態傳資料，所以我之後的處理都是用char來做傳輸，這造成之後我們的成果在傳輸的速度上慢了許多，這算是一大敗筆，傳輸的方式是板子傳送一個char給車子，而車子在回傳一個char給板子，這樣板子就會在傳輸下一個char，用這種方式建立穩定的溝通，而車子一次的動作，需要傳四個char，第一個char代表動作，後面三個char是數字，這代表需要做大的動作，格式為'r','l','f'，分別為右轉左轉前進，車子會先轉到需要轉的角度後，在接收需要前進的距離。
  4. 而車子的掌控，因為複雜的運算都在板子上做完了，車子只要使用servo_speed做出相應的動作就行，這邊轉的角度是用車子的角速度和車子輪圍去計算的，所以準確度算滿高的，而車子前進的距離，則是需要調整比例尺，因為空拍的圖片，座標不代表可以找到正確的距離，這邊就需要當下的微調。

影片demo

https://www.youtube.com/watch?v=YKK04VTwRe0&feature=youtu.be

#code

版子上跑的code #include <stdio.h> #include <string.h> #include <stdlib.h> #include <math.h> #include <fcntl.h> #include <termios.h> #include <string.h> #include <unistd.h> #include <errno.h> #include <stdbool.h> #include <limits.h> #include <sys/select.h>

#define BUFLEN (255) #define ZDEBUG (1)

#define PI 3.141591 int now_angle = 0; int road[20] = {1}; //debug int road_s[20] = {1}; int v[50][50] = {0}; //road weight int min_length = 9999999;

int main (int argc, char *argv[]) { int serial_d; speed_t speed; struct termios soptions, soptions_org; char command; unsigned char send_buf[BUFLEN]; unsigned char recv_buf[BUFLEN]; int sent_c, recv_c; //no port specified if (argc == 1) { printf("Parameter: /dev/ttyUSB0 [command]\n"); printf("\"/dev/ttyUSB0\" is a USB serial device to support serial control.\n"); return 1; }

`serial_d = openserial(argv[1]);`
`if (serial_d == -1) return 1;`

`// ----- Begin of setup serial ports`
`tcgetattr(serial_d, &soptions_org);`
`tcgetattr(serial_d, &soptions);`

`speed = B9600; // Speed options: B19200, B38400, B57600, B115200`
`cfsetispeed(&soptions, speed);`
`cfsetospeed(&soptions, speed);`

`// Enable the reciver and set local mode...`
`soptions.c_cflag |= ( CLOCAL | CREAD );`
`// Setting Parity Checking (8N1)`
`soptions.c_cflag &= ~PARENB;`
`soptions.c_cflag &= ~CSTOPB;`
`soptions.c_cflag &= ~CSIZE;`
`soptions.c_cflag |= CS8;`

`// Local setting`
`//soptions.c_lflag = (ICANON | ECHO | ECHOE); //canonical`
`soptions.c_lflag =  ~(ICANON | ECHO | ECHOE | ISIG); //noncanonical`

`// Input setting`
`//soptions.c_iflag |= (IXON | IXOFF | IXANY); //software flow control`
`soptions.c_iflag |= (INPCK | ISTRIP);`
`soptions.c_iflag = IGNPAR;`

`// Output setting`
`soptions.c_oflag = 0;`
`soptions.c_oflag &= ~OPOST;`

`// Read options`
`soptions.c_cc[VTIME] = 0;`
`soptions.c_cc[VMIN] = 1; //transfer at least 1 character or block`

`// Apply setting`
`tcsetattr(serial_d, TCSANOW, &soptions);`
`// ----- End of setup serial ports`

//////////////////////////////////////////////////////////////////////////////////////// int angle_out; int *have_gone; int *x_addr; int *y_addr; int Q; int ith,jth,go_length; char tag[4]={'0'}; int n; int c1,c2,c3; int j,i; FILE *in_file_1 = fopen("citydist.txt", "r"); // read only if(!in_file_1){ printf("error input!\n"); return -1; } FILE *in_file_2 = fopen("citynum.txt", "r"); // read only if(!in_file_2){ printf("error input!\n"); return -1; } //read file_1 fscanf(in_file_1, "%d", &c1); n = c1; have_gone = (int*)malloc((n+1)*sizeof(int)); while (1){ fscanf(in_file_1, "%d%d%d", &c1, &c2, &c3); if(feof(in_file_1))break; v[c1][c2] = c3; v[c2][c1] = c3; } fclose(in_file_1); trace(0,have_gone,0,1,n); if(road[n]!=1){ printf("fail\n"); return 0; } fscanf(in_file_2, "%d", &c1); n = c1; x_addr = (int*)malloc((n+1)*sizeof(int)); y_addr = (int*)malloc((n+1)*sizeof(int)); while (1){ fscanf(in_file_2, "%d%d%d", &c1, &c2, &c3); if(feof(in_file_2))break; x_addr[c1] = c2; y_addr[c1] = c3; } fclose(in_file_2); for(i = 1;i <=n;i++){ //debug printf("%d (%d,%d)\n",i,x_addr[i],y_addr[i]); }

`for(i = 0;i <(n+1);i++){  //debug`
	`printf("%d-->",road[i]);// 0~n`
`}`
`printf("the minium path is %d \n",min_length);`
	
`for(i=0;i<n;i++){`
	`ith = road[i];`
	`jth = road[i+1];`
	`angle_out = angle_turn(x_addr[ith],y_addr[ith], x_addr[jth],y_addr[jth]);`
	`go_length = v[ith][jth];`
	`printf("(%d,%d)-->(%d,%d) ,",x_addr[ith],y_addr[ith], x_addr[jth],y_addr[jth]);`
	`printf("turn angle: %d ,",angle_out);`
	`printf("go length : %d \n",go_length);`
	`//angle out  angle ==> buffer`
	`if(angle_out>=0){`
		`// turn right`
		`command  = 'r';`
		`sent_c = write(serial_d, &command, 1); // Send command`
		`tcdrain(serial_d);`
		`usleep(2000000); // Wait for response`
		`memset(recv_buf, '\0', BUFLEN);`
		`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
		`tcdrain(serial_d);`
		`int_to_char(tag,angle_out);`
		`printf("right %s degree\n",tag);`
		`for(j=0;j<3;j++){`
			`command = tag[j];`
			`sent_c = write(serial_d, &command, 1); // Send command`
			`tcdrain(serial_d);`
			`usleep(2000000); // Wait for response`
			`memset(recv_buf, '\0', BUFLEN);`
			`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
			`tcdrain(serial_d);`
			`printf("%s\n\n",recv_buf);`
		`}`
	`}`
	`else{`
		`// turn left`
		`command  = 'l';`
		`sent_c = write(serial_d, &command, 1); // Send command`
		`tcdrain(serial_d);`
		`usleep(2000000); // Wait for response`
		`memset(recv_buf, '\0', BUFLEN);`
		`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
		`tcdrain(serial_d);`
		`int_to_char(tag,(-1)*angle_out);`
		`printf("left %s degree\n",tag);`
		`for(j=0;j<3;j++){`
			`command = tag[j];`
			`sent_c = write(serial_d, &command, 1); // Send command`
			`tcdrain(serial_d);`
			`usleep(2000000); // Wait for response`
			`memset(recv_buf, '\0', BUFLEN);`
			`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
			`tcdrain(serial_d);`
			`printf("%s\n\n",recv_buf);`
		`}`
	`}`
	`//`
	`//length out `
	`int_to_char(tag,go_length);`
	`printf("forward %s\n",tag);`
	`command  = 'f';`
	`sent_c = write(serial_d, &command, 1); // Send command`
	`tcdrain(serial_d);`
	`usleep(2000000); // Wait for response`
	`memset(recv_buf, '\0', BUFLEN);`
	`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
	`tcdrain(serial_d);`
	`for(j=0;j<3;j++){`
		`command = tag[j];`
		`sent_c = write(serial_d, &command, 1); // Send command`
		`tcdrain(serial_d);`
		`usleep(2000000); // Wait for response`
		`memset(recv_buf, '\0', BUFLEN);`
		`recv_c = read(serial_d, recv_buf, BUFLEN); // Get response message`
		`tcdrain(serial_d);`
		`printf("%s\n\n",recv_buf);`
	`}`
`}`

// restore setting and close tcsetattr(serial_d, TCSANOW, &soptions_org); close(serial_d); return 0; }

int openserial (char *sdevfile) { int _serial_d = open(sdevfile, O_RDWR | O_NOCTTY); if (_serial_d == -1) perror("Unable to open device\n"); return _serial_d; } int angle_turn(int x1,int y1,int x2,int y2){ int Q; int tmp; Q =180*(atan2(x2-x1,y2-y1)/PI); tmp = Q-now_angle; now_angle = Q; if(tmp >=180) tmp = tmp -360; else if(tmp<= -180) tmp = tmp +360; return tmp; }

int int_to_char(char *in,int a){ int tmp; tmp = a/100; a = a%100; in[0] = tmp + '0'; tmp = a/10; a = a%10; in[1] = tmp + '0'; in[2] = a + '0'; return 0; }

int trace(int gone,int* have_gone,int length,int now_way,int n){ int i=0,j=0; if(gone == 0){ //reset all way gone = 1; length = 0; for(i = 0;i<=n;i++){ have_gone[i]=0; //0 mean haven't enter } } if(gone == n){ //to check min path if(v[now_way][1] != 0){ road[gone] = 1; length = length + v[now_way][1]; if(min_length > length){ min_length = length; for(i = 0;i <20;i++){ //debug road[i]=road_s[i]; printf("%d-->",road[i]); } } } return 0; } for(i=2;i<=n;i++){ //1 is origin place if(v[now_way][i]!=0 && have_gone[i]==0){ have_gone[i] = 1; // let's go i'th place road_s[gone] = i; if((length + v[now_way][i]) < min_length){ trace(gone+1,have_gone,length + v[now_way][i],i,n); } have_gone[i] = 0; // let's back way } } return 0; }

2.車子上跑的code clude "simpletools.h" #include "servo.h" #include "servodiffdrive.h" #include "fdserial.h" #include "talk.h"

#define PIN_SOUND 26 // pin for the speaker, default=26 #define PIN_XBEE_RX 0 #define PIN_XBEE_TX 1 #define PIN_SERVO_LEFT 16 #define PIN_SERVO_RIGHT 17 #define Map_ratio 1 //100 :10cm

int main (void) { int i =0; int length; char number[4]; int ptime; fdserial *xbee; xbee = fdserial_open(PIN_XBEE_RX, PIN_XBEE_TX, 0, 9600); drive_pins(PIN_SERVO_LEFT, PIN_SERVO_RIGHT);

`pause(1000);`
`putchar(CLS);`
`fdserial_rxFlush(xbee);`

`while (1) {`
	`char ch = fdserial_rxChar(xbee);`
	`fdserial_rxFlush(xbee);`
	`switch (ch) {`
		`case 'f':`
			`print("Forward\n");`
			`pause(100);`
			`fdserial_txChar(xbee, ch);`
			`fdserial_txFlush(xbee);`
			`i =0;`
			`while(1){`
				`char ch = fdserial_rxChar(xbee);`
				`fdserial_rxFlush(xbee);`
				`if('0'<=ch && ch<='9'){`
					`number[i]=ch;`
					`i++;`
				`}`
        `if(i==3){`
          `////////////////function`
          `drive_speeds(100,100);`
          `printf("number = %d\n",char_to_int(number));`
          `ptime  = (1000*Map_ratio*char_to_int(number)*0.667)/104;`
          `pause(ptime);`
          `drive_speeds(0,0);`
          `///////////////////////////////////`
          
        `}`
		`//		print("%c\n", (char) ch);`
				`pause(100);`
				`fdserial_txChar(xbee, ch);`
				`fdserial_txFlush(xbee);`
				`if(i==3)break;`
			`}`
			`break;`
		`case 'l':`
			`print("turn left\n");`
			`pause(100);`
			`fdserial_txChar(xbee, ch);`
			`fdserial_txFlush(xbee);`
			`i =0;`
			`while(1){`
				`char ch = fdserial_rxChar(xbee);`
				`fdserial_rxFlush(xbee);`
				`if('0'<=ch && ch<='9'){`
					`number[i]=ch;`
					`i++;`
				`}`
        `if(i==3){`
          `////////////////function`
          `drive_speeds(-50,50);`
          `ptime = 1000*665*char_to_int(number)/360/104;`
          `if(ptime != 0)pause(ptime/2);`
          `drive_speeds(0,0);`
          `printf("number = %d\n",char_to_int(number));`
          `///////////////////////////////////`
          
        `}`
			`//	print("%c\n", (char) ch);`
				`pause(100);`
				`fdserial_txChar(xbee, ch);`
				`fdserial_txFlush(xbee);`
				`if(i==3)break;`
			`}`
			`break;`
		`case 'r':`
			`print("turn right\n");`
			`pause(100);`
			`fdserial_txChar(xbee, ch);`
			`fdserial_txFlush(xbee);`
			`i =0;`
			`while(1){`
				`char ch = fdserial_rxChar(xbee);`
				`fdserial_rxFlush(xbee);`
				`if('0'<=ch && ch<='9'){`
					`number[i]=ch;`
					`i++;`
				`}`
        `if(i==3){`
          `////////////////function`
          `drive_speeds(50,-50);`
          `ptime = 1000*665*char_to_int(number)/360/104;`
          `print("ptime = %d",ptime);`
          `if(ptime != 0)pause(ptime/2);`
          `drive_speeds(0,0);`
          `printf("number = %d\n",char_to_int(number));`
          `///////////////////////////////////`
          
        `}`
			`//	print("%c\n", (char) ch);`
				`pause(100);`
				`fdserial_txChar(xbee, ch);`
				`fdserial_txFlush(xbee);`
				`if(i==3)break;`
			`}`
			`break;`

		`default:`
			`print("Invalid Command\n");`
			`pause(700);`
			`print("%c\n", (char) ch);`
			`pause(100);`
			`fdserial_txChar(xbee, ch);`
			`fdserial_txFlush(xbee);`
			`break;`
	`}`
	
`}`

}

int char_to_int(char * a){ int tmp,tmp1,tmp2,tmp3; tmp1 = a[0] - '0'; tmp2 = a[1] - '0'; tmp3 = a[2] - '0'; tmp = tmp1*100 + tmp2*10 +tmp3; return tmp; } `

⚠️ **GitHub.com Fallback** ⚠️