Revision 4:4ea0f6305fa3, committed 2020-01-30

Comitter:

leejoonwoo

Date:

Thu Jan 30 00:00:40 2020 +0000

Parent:

3:bb074b1d26fe

Commit message:

description

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map.lib	Thu Jan 30 00:00:40 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Generic/code/Map/#dad975e2e150

--- a/Motordriver.lib	Wed Jan 29 03:02:47 2020 +0000
+++ b/Motordriver.lib	Thu Jan 30 00:00:40 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/samco/code/Motordriver/#5d6d2ae22b13
+https://os.mbed.com/teams/samco/code/Motordriver/#14aa47762fc3

--- a/main.cpp	Wed Jan 29 03:02:47 2020 +0000
+++ b/main.cpp	Thu Jan 30 00:00:40 2020 +0000
@@ -1,3 +1,4 @@
+//헤더파일 선언
 #include "mbed.h"
 #include "mavlink.h"
 #include "motordriver.h"
@@ -6,6 +7,7 @@
 #include "IncrementalEncoder.h"
 #include "VL53L1X.h"
 
+//모빌리티 상태 정의 (정지,앞,백,왼쪽,오른쪽)
 #define FRONT 1
 #define NORMAL 0
 #define BACK 2
@@ -13,16 +15,16 @@
 #define RIGHT 4
 
 DigitalOut led (LED2);
-//SERIAL
-//Serial pc(USBTX, USBRX,115200);
+
+//Xbee (Serial)선언
 RawSerial xbee(PC_10, PC_11, 115200);
 
-//Thread
+//Thread 선언 (read, write)
 Thread thread;
 Thread thread2;
-Mutex mutex;
+//Mutex mutex;
 
-// OF MAVLINK
+// MAVLINK 관련 함수 선언
 char buf[MAVLINK_MAX_PACKET_LEN];
 
 mavlink_message_t message;
@@ -34,25 +36,27 @@
 mavlink_joystick_t joystick;
 
 unsigned len = 0;
-
+//MAVLINK ID 선언
 uint8_t system_id = 10;
 uint8_t component_id = 15;
 
-//Motordriver , RC MAP
+//Motor 선언 (Motordriver)
 Motor motor_left(PA_9,PA_8,PB_15,1);
 Motor motor_right(PC_9,PC_6,PC_8,1);
-//pwm
+
+//joystick 값 선언
 float joystick_percent_x = 0;
 float joystick_percent_y = 0;
 
+//Input (0~1024) Output (-0.3~0.3)
 Map map_x (0, 1024, -0.3, 0.3);
 Map map_y (0, 1024, -0.3, 0.3);
 
-//Encoder
+//Encoder 선언(IncrementalEncoder)
 IncrementalEncoder right_encoder(PD_2);
 IncrementalEncoder left_encoder(PC_4);
 
-//tof
+//tof 선언
 VL53L1X tof1 (PB_9, PB_8);
 
 /////////////////////////////////////////////
@@ -61,6 +65,8 @@
 int write_data (char *data)
 {
 }
+
+//Xbee(Serial)로 값을 내보내기
 void write_data(char* buf, unsigned int len)
 {
     for(int i = 0; i<len; i++) {
@@ -68,9 +74,12 @@
     }
 }
 
+//heartbeat(Mavlik) 속성 설정
 void heartbeat_info()
 {
+    //Thread 주기 설정
     Thread::wait(2000);
+
     heartbeat.type = 0;
     heartbeat.mode = 1;
 
@@ -80,31 +89,36 @@
     write_data(buf, len);
 }
 
+//motor (Mavlink) 속성 설정
 mavlink_motor_t motor;
 void Motor_message_send()
 {
     Thread::wait(500);
     motor.encoder_left = (uint16_t)left_encoder.readTotal();
     motor.encoder_right = (uint16_t)right_encoder.readTotal();
-    
+
     mavlink_msg_motor_encode (system_id, component_id, &message, &motor);
     len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
-    
+
     write_data(buf, len);
 }
+
+//pwm (Mavlink) 속성 설정
 mavlink_pwm_t pwm;
 void pwm_message_send()
 {
     Thread::wait(500);
-    
+
     pwm.motor_left_pwm = (uint16_t)joystick_percent_x;
     pwm.motor_right_pwm = (uint16_t)joystick_percent_y;
-    
+
     mavlink_msg_pwm_encode (system_id, component_id, &message, &pwm);
     len=mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
-    write_data(buf,len);    
+
+    write_data(buf,len);
 }
 
+//write보낼 (heartbeat,motor,pwm) message send (loop)
 void write_message()
 {
     while(1) {
@@ -117,102 +131,123 @@
 /////////////////////////////////
 ////////////read/////////////////
 /////////////////////////////////
+
+//물체감지 함수
 void object_detection()
 {
+    //Tof 거리값 read
     uint16_t distance = tof1.getDistance();
-    
-    if(distance < 400)
-    {
+
+    //물체감지 후 정지
+    if(distance < 400) {
         motor_left.speed (0);
         motor_right.speed(0);
     }
 }
+
+//Encoder (왼쪽,오른쪽) 계산 : 초기상태 정지
 int state = NORMAL;
 float sens = 1;
 void speedGap()
 {
     int gap = (right_encoder.readTotal()) - (left_encoder.readTotal());
-    
+
     //left over
-    if (gap > 0 && sens < 1.5)
-    {
+    if (gap > 0 && sens < 1.5) {
         sens -= 0.01;
     }
-    if (gap < 0 && sens > 0.5)
-    {
+    if (gap < 0 && sens > 0.5) {
         sens += 0.01;
     }
-    
+
 }
+
+//GCS로 부터 joystick값 (0~1024)을 받은후 (-0.3,0.3)의 pwm로 변환 후 motordriver 전송 -> 'mobility move'
 mavlink_joystick_t local_joystick;
-
 void rc_move()
 {
     local_joystick = joystick;
+
     joystick_percent_x = map_x.Calculate(local_joystick.joystick_x);
     joystick_percent_y = map_y.Calculate(local_joystick.joystick_y);
-    
+
+    //button 클릭 시 모빌리티 정지
     if ( local_joystick.joystick_click == 0 ) {
         motor_left.speed (0);
         motor_right.speed (0);
     }
-    //move forward/back
+    //joystick 중립상태
     if( joystick_percent_y >= -0.1 && joystick_percent_y <= 0.1 && joystick_percent_x >= -0.1 && joystick_percent_x <=0.1) {
         motor_left.speed (0);
         motor_right.speed (0);
         state = NORMAL;
     }
 
+    //forward and right
     else if ( joystick_percent_x > 0.1 && joystick_percent_y > 0.1) {
         motor_left.speed (joystick_percent_y+joystick_percent_x);
         motor_right.speed (joystick_percent_y*sens);
-    } 
+    }
+
+    //forward and left
     else if (joystick_percent_x < -0.1 && joystick_percent_y >0.1) {
         motor_left.speed (joystick_percent_y);
         motor_right.speed ((joystick_percent_y +(-1*joystick_percent_x))*sens);
-    } 
+    }
+
+    //left and back
     else if (joystick_percent_x < -0.1 && joystick_percent_y < -0.1) {
         motor_left.speed (joystick_percent_y );
         motor_right.speed ((joystick_percent_y + joystick_percent_x)*sens);
-    } 
+    }
+
+    //forward and back
     else if (joystick_percent_x > 0.1 && joystick_percent_y < - 0.1) {
         motor_left.speed ((joystick_percent_y - joystick_percent_x));
         motor_right.speed (joystick_percent_y*sens);
-    } 
-    //forward,backward
+    }
+
+    //forward
     else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y>0.1) {
         motor_left.speed (joystick_percent_y);
         motor_right.speed (joystick_percent_y*sens);
         state = BACK;
-    } 
+    }
+
+    //back
     else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y < -0.1) {
         motor_left.speed (joystick_percent_y);
         motor_right.speed (joystick_percent_y*sens);
         state = FRONT;
-    } 
+    }
+
+    //left
     else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x < -0.1) {
         motor_right.speed(-joystick_percent_x*sens);
         state = LEFT;
-    } 
+    }
+
+    //right
     else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x > 0.1) {
         motor_left.speed(joystick_percent_x);
         state = RIGHT;
     }
-        
-    if(state == FRONT)
-    {
+
+    //앞으로 움직일 때 encoder 차이값 계산, 물체감지 작동
+    if(state == FRONT) {
         speedGap();
         object_detection();
     }
-    else if(state == BACK || state == LEFT || state == RIGHT)
-    {
+
+    //앞으로 움직이지 않을 때 encoder 값 실시간 reset
+    else if(state == BACK || state == LEFT || state == RIGHT) {
         left_encoder.reset();
         right_encoder.reset();
     }
-    
-    
 }
 
+
+//Xbee (Serial)로 받은 mavlink 데이터 처리
 void read_data()
 {
     led = 1;
@@ -220,13 +255,16 @@
 
     while(1) {
         if(xbee.readable()) {
-
+            
+            //xbee 데이터 read
             uint8_t c = xbee.getc();
 
             msgReceived = mavlink_parse_char(MAVLINK_COMM_1, c, &r_message, &r_status);
 
             if(msgReceived > 0) {
                 led = 0;
+
+                //id 검사
                 if (r_message.sysid == 60|| r_message.sysid == 62) {
                     switch (r_message.msgid) {
                         case 3: { //JOYSTICK
@@ -235,7 +273,7 @@
                             break;
                         }
                     }
-                } 
+                }
             }
         } else {
             led = 1;
@@ -248,7 +286,9 @@
     //tof i2c setting
     tof1.begin();
     tof1.setDistanceMode();
-    
+
+    //write part thread start
     thread.start(write_message);
+    //read part (while)
     read_data();
 }