init

Dependencies:   VL53L1X Map

Files at this revision

API Documentation at this revision

Comitter:
leejoonwoo
Date:
Thu Jan 30 00:00:40 2020 +0000
Parent:
3:bb074b1d26fe
Commit message:
description

Changed in this revision

Map.lib Show annotated file Show diff for this revision Revisions of this file
Motordriver.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r bb074b1d26fe -r 4ea0f6305fa3 Map.lib
--- /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
diff -r bb074b1d26fe -r 4ea0f6305fa3 Motordriver.lib
--- 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
diff -r bb074b1d26fe -r 4ea0f6305fa3 main.cpp
--- 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();
 }