STM32 ] 자율주행 코드 짜기 - 프로젝트 준비과정 (6)

 

 

먼저 GPS모듈을 통해 자동차의 위도, 경도를 확인하였고, 어플과의 블루투스 통신을 통해 사용자가 찍은 Waypoint(목적지)의 위도, 경도까지 확인할 수 있었다.

 

 

 

그리고 Waypoint의 위치에서 자동차의 위치를 빼주면 두 포인트 사이의 위도차, 경도차를 알 수 있고 직선거리로의 차이 c는 피타고라스의 정리에 의해 구할 수 있다.

 

다만 이건 평면 위에서의 거리이고 구 위에서의 거리는 하버사인공식(Haversine Formular)로 구할 수 있는데 어차피 우리는 블루투스 모듈의 통신거리 한계로 테스트 환경이 좁은 지역 안에서 해야하기 때문에 그냥 피타고라스의 정리를 사용하기로 하였다.

 

 

 

 

다음은 방위각. 지자기센서를 통해 자동차의 방위각은 알아낼 수 있다. Waypoint의 방위각이 문제인데 문과생으로서 조금 쉬운 접근방법을 생각했다.

먼저 경도차 위도차가 +/+, -/+, -/-, +/- 냐에 따라 자동차의 위치를 원점으로 봤을 때 Waypoint가 몇사분면에 위치했는지를 알아낼 수 있다.

 

그리고 경도차와 직선거리로 삼각비인 역코사인 함수 acos를 사용하면 경도축에 맞닿아 있는 방위각 θ를 알아낼 수 있고, 이후에 Waypoint가 몇사분면에 위치했느냐에 따라 보정하면 북측(0°,360°)을 기준으로 자동차의 위치에서 바라본 Waypoint의 방위각을 알아낼 수 있다.

 

 

 

 

 

 

 

selfDriving.c

#include "selfDriving.h"
#include "gps.h"
#include "bluetooth.h"
#include "HMC5883L.h"
#include <math.h>
#include "stdlib.h"
#define _USE_MATH_DEFINES

extern float headingDegrees;

int distance_long;
int distance_lat;
int distance_c;	

double angle;			
double degree_angle;	
double target_angle;	

int diffAngle;

CONTROLLER_SIGNAL moveSignal;
uint8_t rotate_flag=0;

extern GPS_t GPS;
extern _DestinationGPS waypointGPS;
_DestinationGPS waypointBefore;

extern uint16_t Distance[3];
_Quadrant quadrant;

void SelfDriving(){

	  distance_long = (waypointGPS.longitude - GPS.dec_longitude) * 1000000;
	  distance_lat = (waypointGPS.latitude - GPS.dec_latitude) * 1000000;
	  distance_c = sqrt((distance_lat * distance_lat) + (distance_long * distance_long));

	  angle = acos((double)distance_long / distance_c);
	  degree_angle = (double)angle * 180 / M_PI;

	  if (distance_long > 0 && distance_lat > 0) {
		  quadrant = QUADRANT_ONE;
	  }
	  else if (distance_long < 0 && distance_lat > 0) {
		  quadrant = QUADRANT_TWO;
	  }
	  else if (distance_long < 0 && distance_lat < 0) {
		  quadrant = QUADRANT_THREE;
	  }
	  else if (distance_long > 0 && distance_lat < 0) {
		  quadrant = QUADRANT_FOUR;
	  }

	  switch (quadrant) {
	  case QUADRANT_ONE:
		  target_angle = 90 - degree_angle;
		  break;
	  case QUADRANT_TWO:
		  degree_angle = 180-degree_angle;
		  target_angle = 270 + degree_angle;
		  break;
	  case QUADRANT_THREE:
		  degree_angle = 180-degree_angle;
		  target_angle = 270 - degree_angle;
		  break;
	  case QUADRANT_FOUR:
		  target_angle = 90 + degree_angle;
		  break;
	  default:
		  break;
	  }

	  diffAngle = ((int)target_angle - (int)headingDegrees + 360) % 360;

	  if(distance_c < 50){
		  moveSignal = STOP;

	  }else {
		  if ((diffAngle >= 345) || (diffAngle < 15)){
			  moveSignal = FORWARD;

		  }else {

			  if ((diffAngle >= 180) && (diffAngle < 345)){
				  moveSignal = CCW;
			  }else if ((diffAngle >= 15) && (diffAngle < 180)){
				  moveSignal = CW;
			  }
		  }
	  }

	  Move(moveSignal);

}

 

 

먼저 위도차 distance_lat, 경도차 distance_long 은 float 형태라 소숫점 6자리까지 표시되는데 값을 읽기 힘들어서 눈에 익은 수로 만들어주기 위해 백만을 곱했다.

 

acos 함수로 라디안 단위 각도를 구하고 degree 단위로 변환한다. Waypoint가 자동차의 위치를 원점으로 봤을 때 몇사분면에 위치했느냐를 알아내어 Waypoint의 방위각을 알아낸다.

 

 

다음 방위각 차이를 구한다. 이렇게 구한 이유는 절대적 각도차이가 아닌 CW방향 기준으로의 차이를 알아내기 위해서이다.

예를들어 자동차가 350°이고 Waypoint가 10°인 경우 각도차이는 20°이며, 자동차가 350°이고 Waypoint가 330°인 경우 각도차이는 340°이다.

diffAngle = ((int)target_angle - (int)headingDegrees + 360) % 360;

 

 

직선거리차이 distance_c 가 50미만이라면 정지하고 아니라면 방위각 차이를 확인한다.

방위각이 345 이상 15미만이라면 방위각차이가 거의 없다고 보고 직진한다.

아니라면 15-180 사이일 때 CW방향으로 회전하고 180-345 사이일 때 CCW방향으로 회전한다.

 

그리고 그 아래 Move 함수가 while 루프안에서 계속 호출되면서 목적지까지 거리차이가 있는 경우, 계속 각도차이를 실시간 보정하면서 다가가게끔 한다.