samco / Mbed OS Nucleo_rtos_basic_f103rb

init

Dependencies:   VL53L1X Map

main.cpp@4:4ea0f6305fa3, 2020-01-30 (annotated)

Committer:
leejoonwoo
Date:
Thu Jan 30 00:00:40 2020 +0000
Revision:
4:4ea0f6305fa3
Parent:
3:bb074b1d26fe 
description

Who changed what in which revision?

UserRevisionLine numberNew contents of line
leejoonwoo 4:4ea0f6305fa3 1 //헤더파일 선언
bcostm 0:5701b41769fd 2 #include "mbed.h"
leejoonwoo 3:bb074b1d26fe 3 #include "mavlink.h"
leejoonwoo 3:bb074b1d26fe 4 #include "motordriver.h"
leejoonwoo 3:bb074b1d26fe 5 #include "Map.hpp"
leejoonwoo 3:bb074b1d26fe 6 #include "rtos.h"
leejoonwoo 3:bb074b1d26fe 7 #include "IncrementalEncoder.h"
leejoonwoo 3:bb074b1d26fe 8 #include "VL53L1X.h"
leejoonwoo 3:bb074b1d26fe 9
leejoonwoo 4:4ea0f6305fa3 10 //모빌리티 상태 정의 (정지,앞,백,왼쪽,오른쪽)
leejoonwoo 3:bb074b1d26fe 11 #define FRONT 1
leejoonwoo 3:bb074b1d26fe 12 #define NORMAL 0
leejoonwoo 3:bb074b1d26fe 13 #define BACK 2
leejoonwoo 3:bb074b1d26fe 14 #define LEFT 3
leejoonwoo 3:bb074b1d26fe 15 #define RIGHT 4
leejoonwoo 3:bb074b1d26fe 16
leejoonwoo 3:bb074b1d26fe 17 DigitalOut led (LED2);
leejoonwoo 4:4ea0f6305fa3 18
leejoonwoo 4:4ea0f6305fa3 19 //Xbee (Serial)선언
leejoonwoo 3:bb074b1d26fe 20 RawSerial xbee(PC_10, PC_11, 115200);
leejoonwoo 3:bb074b1d26fe 21
leejoonwoo 4:4ea0f6305fa3 22 //Thread 선언 (read, write)
leejoonwoo 3:bb074b1d26fe 23 Thread thread;
leejoonwoo 3:bb074b1d26fe 24 Thread thread2;
leejoonwoo 4:4ea0f6305fa3 25 //Mutex mutex;
leejoonwoo 3:bb074b1d26fe 26
leejoonwoo 4:4ea0f6305fa3 27 // MAVLINK 관련 함수 선언
leejoonwoo 3:bb074b1d26fe 28 char buf[MAVLINK_MAX_PACKET_LEN];
leejoonwoo 3:bb074b1d26fe 29
leejoonwoo 3:bb074b1d26fe 30 mavlink_message_t message;
leejoonwoo 3:bb074b1d26fe 31 mavlink_message_t r_message;
leejoonwoo 3:bb074b1d26fe 32 mavlink_status_t status;
leejoonwoo 3:bb074b1d26fe 33 mavlink_status_t r_status;
leejoonwoo 3:bb074b1d26fe 34
leejoonwoo 3:bb074b1d26fe 35 mavlink_heartbeat_t heartbeat;
leejoonwoo 3:bb074b1d26fe 36 mavlink_joystick_t joystick;
leejoonwoo 3:bb074b1d26fe 37
leejoonwoo 3:bb074b1d26fe 38 unsigned len = 0;
leejoonwoo 4:4ea0f6305fa3 39 //MAVLINK ID 선언
leejoonwoo 3:bb074b1d26fe 40 uint8_t system_id = 10;
leejoonwoo 3:bb074b1d26fe 41 uint8_t component_id = 15;
leejoonwoo 3:bb074b1d26fe 42
leejoonwoo 4:4ea0f6305fa3 43 //Motor 선언 (Motordriver)
leejoonwoo 3:bb074b1d26fe 44 Motor motor_left(PA_9,PA_8,PB_15,1);
leejoonwoo 3:bb074b1d26fe 45 Motor motor_right(PC_9,PC_6,PC_8,1);
leejoonwoo 4:4ea0f6305fa3 46
leejoonwoo 4:4ea0f6305fa3 47 //joystick 값 선언
leejoonwoo 3:bb074b1d26fe 48 float joystick_percent_x = 0;
leejoonwoo 3:bb074b1d26fe 49 float joystick_percent_y = 0;
leejoonwoo 3:bb074b1d26fe 50
leejoonwoo 4:4ea0f6305fa3 51 //Input (0~1024) Output (-0.3~0.3)
leejoonwoo 3:bb074b1d26fe 52 Map map_x (0, 1024, -0.3, 0.3);
leejoonwoo 3:bb074b1d26fe 53 Map map_y (0, 1024, -0.3, 0.3);
leejoonwoo 3:bb074b1d26fe 54
leejoonwoo 4:4ea0f6305fa3 55 //Encoder 선언(IncrementalEncoder)
leejoonwoo 3:bb074b1d26fe 56 IncrementalEncoder right_encoder(PD_2);
leejoonwoo 3:bb074b1d26fe 57 IncrementalEncoder left_encoder(PC_4);
leejoonwoo 3:bb074b1d26fe 58
leejoonwoo 4:4ea0f6305fa3 59 //tof 선언
leejoonwoo 3:bb074b1d26fe 60 VL53L1X tof1 (PB_9, PB_8);
bcostm 0:5701b41769fd 61
leejoonwoo 3:bb074b1d26fe 62 /////////////////////////////////////////////
leejoonwoo 3:bb074b1d26fe 63 /////////////////write///////////////////////
leejoonwoo 3:bb074b1d26fe 64 /////////////////////////////////////////////
leejoonwoo 3:bb074b1d26fe 65 int write_data (char *data)
leejoonwoo 3:bb074b1d26fe 66 {
leejoonwoo 3:bb074b1d26fe 67 }
leejoonwoo 4:4ea0f6305fa3 68
leejoonwoo 4:4ea0f6305fa3 69 //Xbee(Serial)로 값을 내보내기
leejoonwoo 3:bb074b1d26fe 70 void write_data(char* buf, unsigned int len)
leejoonwoo 3:bb074b1d26fe 71 {
leejoonwoo 3:bb074b1d26fe 72 for(int i = 0; i<len; i++) {
leejoonwoo 3:bb074b1d26fe 73 xbee.putc(buf[i]);
leejoonwoo 3:bb074b1d26fe 74 }
leejoonwoo 3:bb074b1d26fe 75 }
leejoonwoo 3:bb074b1d26fe 76
leejoonwoo 4:4ea0f6305fa3 77 //heartbeat(Mavlik) 속성 설정
leejoonwoo 3:bb074b1d26fe 78 void heartbeat_info()
leejoonwoo 3:bb074b1d26fe 79 {
leejoonwoo 4:4ea0f6305fa3 80 //Thread 주기 설정
leejoonwoo 3:bb074b1d26fe 81 Thread::wait(2000);
leejoonwoo 4:4ea0f6305fa3 82
leejoonwoo 3:bb074b1d26fe 83 heartbeat.type = 0;
leejoonwoo 3:bb074b1d26fe 84 heartbeat.mode = 1;
leejoonwoo 3:bb074b1d26fe 85
leejoonwoo 3:bb074b1d26fe 86 mavlink_msg_heartbeat_encode (system_id, component_id, &message, &heartbeat);
leejoonwoo 3:bb074b1d26fe 87 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 3:bb074b1d26fe 88
leejoonwoo 3:bb074b1d26fe 89 write_data(buf, len);
leejoonwoo 3:bb074b1d26fe 90 }
leejoonwoo 3:bb074b1d26fe 91
leejoonwoo 4:4ea0f6305fa3 92 //motor (Mavlink) 속성 설정
leejoonwoo 3:bb074b1d26fe 93 mavlink_motor_t motor;
leejoonwoo 3:bb074b1d26fe 94 void Motor_message_send()
bcostm 0:5701b41769fd 95 {
leejoonwoo 3:bb074b1d26fe 96 Thread::wait(500);
leejoonwoo 3:bb074b1d26fe 97 motor.encoder_left = (uint16_t)left_encoder.readTotal();
leejoonwoo 3:bb074b1d26fe 98 motor.encoder_right = (uint16_t)right_encoder.readTotal();
leejoonwoo 4:4ea0f6305fa3 99
leejoonwoo 3:bb074b1d26fe 100 mavlink_msg_motor_encode (system_id, component_id, &message, &motor);
leejoonwoo 3:bb074b1d26fe 101 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 4:4ea0f6305fa3 102
leejoonwoo 3:bb074b1d26fe 103 write_data(buf, len);
leejoonwoo 3:bb074b1d26fe 104 }
leejoonwoo 4:4ea0f6305fa3 105
leejoonwoo 4:4ea0f6305fa3 106 //pwm (Mavlink) 속성 설정
leejoonwoo 3:bb074b1d26fe 107 mavlink_pwm_t pwm;
leejoonwoo 3:bb074b1d26fe 108 void pwm_message_send()
leejoonwoo 3:bb074b1d26fe 109 {
leejoonwoo 3:bb074b1d26fe 110 Thread::wait(500);
leejoonwoo 4:4ea0f6305fa3 111
leejoonwoo 3:bb074b1d26fe 112 pwm.motor_left_pwm = (uint16_t)joystick_percent_x;
leejoonwoo 3:bb074b1d26fe 113 pwm.motor_right_pwm = (uint16_t)joystick_percent_y;
leejoonwoo 4:4ea0f6305fa3 114
leejoonwoo 3:bb074b1d26fe 115 mavlink_msg_pwm_encode (system_id, component_id, &message, &pwm);
leejoonwoo 3:bb074b1d26fe 116 len=mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 4:4ea0f6305fa3 117
leejoonwoo 4:4ea0f6305fa3 118 write_data(buf,len);
leejoonwoo 3:bb074b1d26fe 119 }
leejoonwoo 3:bb074b1d26fe 120
leejoonwoo 4:4ea0f6305fa3 121 //write보낼 (heartbeat,motor,pwm) message send (loop)
leejoonwoo 3:bb074b1d26fe 122 void write_message()
leejoonwoo 3:bb074b1d26fe 123 {
leejoonwoo 3:bb074b1d26fe 124 while(1) {
leejoonwoo 3:bb074b1d26fe 125 heartbeat_info();
leejoonwoo 3:bb074b1d26fe 126 Motor_message_send();
leejoonwoo 3:bb074b1d26fe 127 pwm_message_send();
leejoonwoo 3:bb074b1d26fe 128 }
bcostm 0:5701b41769fd 129 }
bcostm 0:5701b41769fd 130
leejoonwoo 3:bb074b1d26fe 131 /////////////////////////////////
leejoonwoo 3:bb074b1d26fe 132 ////////////read/////////////////
leejoonwoo 3:bb074b1d26fe 133 /////////////////////////////////
leejoonwoo 4:4ea0f6305fa3 134
leejoonwoo 4:4ea0f6305fa3 135 //물체감지 함수
leejoonwoo 3:bb074b1d26fe 136 void object_detection()
leejoonwoo 3:bb074b1d26fe 137 {
leejoonwoo 4:4ea0f6305fa3 138 //Tof 거리값 read
leejoonwoo 3:bb074b1d26fe 139 uint16_t distance = tof1.getDistance();
leejoonwoo 4:4ea0f6305fa3 140
leejoonwoo 4:4ea0f6305fa3 141 //물체감지 후 정지
leejoonwoo 4:4ea0f6305fa3 142 if(distance < 400) {
leejoonwoo 3:bb074b1d26fe 143 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 144 motor_right.speed(0);
leejoonwoo 3:bb074b1d26fe 145 }
leejoonwoo 3:bb074b1d26fe 146 }
leejoonwoo 4:4ea0f6305fa3 147
leejoonwoo 4:4ea0f6305fa3 148 //Encoder (왼쪽,오른쪽) 계산 : 초기상태 정지
leejoonwoo 3:bb074b1d26fe 149 int state = NORMAL;
leejoonwoo 3:bb074b1d26fe 150 float sens = 1;
leejoonwoo 3:bb074b1d26fe 151 void speedGap()
leejoonwoo 3:bb074b1d26fe 152 {
leejoonwoo 3:bb074b1d26fe 153 int gap = (right_encoder.readTotal()) - (left_encoder.readTotal());
leejoonwoo 4:4ea0f6305fa3 154
leejoonwoo 3:bb074b1d26fe 155 //left over
leejoonwoo 4:4ea0f6305fa3 156 if (gap > 0 && sens < 1.5) {
leejoonwoo 3:bb074b1d26fe 157 sens -= 0.01;
leejoonwoo 3:bb074b1d26fe 158 }
leejoonwoo 4:4ea0f6305fa3 159 if (gap < 0 && sens > 0.5) {
leejoonwoo 3:bb074b1d26fe 160 sens += 0.01;
leejoonwoo 3:bb074b1d26fe 161 }
leejoonwoo 4:4ea0f6305fa3 162
leejoonwoo 3:bb074b1d26fe 163 }
leejoonwoo 4:4ea0f6305fa3 164
leejoonwoo 4:4ea0f6305fa3 165 //GCS로 부터 joystick값 (0~1024)을 받은후 (-0.3,0.3)의 pwm로 변환 후 motordriver 전송 -> 'mobility move'
leejoonwoo 3:bb074b1d26fe 166 mavlink_joystick_t local_joystick;
leejoonwoo 3:bb074b1d26fe 167 void rc_move()
leejoonwoo 3:bb074b1d26fe 168 {
leejoonwoo 3:bb074b1d26fe 169 local_joystick = joystick;
leejoonwoo 4:4ea0f6305fa3 170
leejoonwoo 3:bb074b1d26fe 171 joystick_percent_x = map_x.Calculate(local_joystick.joystick_x);
leejoonwoo 3:bb074b1d26fe 172 joystick_percent_y = map_y.Calculate(local_joystick.joystick_y);
leejoonwoo 4:4ea0f6305fa3 173
leejoonwoo 4:4ea0f6305fa3 174 //button 클릭 시 모빌리티 정지
leejoonwoo 3:bb074b1d26fe 175 if ( local_joystick.joystick_click == 0 ) {
leejoonwoo 3:bb074b1d26fe 176 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 177 motor_right.speed (0);
leejoonwoo 3:bb074b1d26fe 178 }
leejoonwoo 4:4ea0f6305fa3 179 //joystick 중립상태
leejoonwoo 3:bb074b1d26fe 180 if( joystick_percent_y >= -0.1 && joystick_percent_y <= 0.1 && joystick_percent_x >= -0.1 && joystick_percent_x <=0.1) {
leejoonwoo 3:bb074b1d26fe 181 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 182 motor_right.speed (0);
leejoonwoo 3:bb074b1d26fe 183 state = NORMAL;
leejoonwoo 3:bb074b1d26fe 184 }
bcostm 0:5701b41769fd 185
leejoonwoo 4:4ea0f6305fa3 186 //forward and right
leejoonwoo 3:bb074b1d26fe 187 else if ( joystick_percent_x > 0.1 && joystick_percent_y > 0.1) {
leejoonwoo 3:bb074b1d26fe 188 motor_left.speed (joystick_percent_y+joystick_percent_x);
leejoonwoo 3:bb074b1d26fe 189 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 4:4ea0f6305fa3 190 }
leejoonwoo 4:4ea0f6305fa3 191
leejoonwoo 4:4ea0f6305fa3 192 //forward and left
leejoonwoo 3:bb074b1d26fe 193 else if (joystick_percent_x < -0.1 && joystick_percent_y >0.1) {
leejoonwoo 3:bb074b1d26fe 194 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 195 motor_right.speed ((joystick_percent_y +(-1*joystick_percent_x))*sens);
leejoonwoo 4:4ea0f6305fa3 196 }
leejoonwoo 4:4ea0f6305fa3 197
leejoonwoo 4:4ea0f6305fa3 198 //left and back
leejoonwoo 3:bb074b1d26fe 199 else if (joystick_percent_x < -0.1 && joystick_percent_y < -0.1) {
leejoonwoo 3:bb074b1d26fe 200 motor_left.speed (joystick_percent_y );
leejoonwoo 3:bb074b1d26fe 201 motor_right.speed ((joystick_percent_y + joystick_percent_x)*sens);
leejoonwoo 4:4ea0f6305fa3 202 }
leejoonwoo 4:4ea0f6305fa3 203
leejoonwoo 4:4ea0f6305fa3 204 //forward and back
leejoonwoo 3:bb074b1d26fe 205 else if (joystick_percent_x > 0.1 && joystick_percent_y < - 0.1) {
leejoonwoo 3:bb074b1d26fe 206 motor_left.speed ((joystick_percent_y - joystick_percent_x));
leejoonwoo 3:bb074b1d26fe 207 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 4:4ea0f6305fa3 208 }
leejoonwoo 4:4ea0f6305fa3 209
leejoonwoo 4:4ea0f6305fa3 210 //forward
leejoonwoo 3:bb074b1d26fe 211 else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y>0.1) {
leejoonwoo 3:bb074b1d26fe 212 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 213 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 214 state = BACK;
leejoonwoo 4:4ea0f6305fa3 215 }
leejoonwoo 4:4ea0f6305fa3 216
leejoonwoo 4:4ea0f6305fa3 217 //back
leejoonwoo 3:bb074b1d26fe 218 else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y < -0.1) {
leejoonwoo 3:bb074b1d26fe 219 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 220 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 221 state = FRONT;
leejoonwoo 4:4ea0f6305fa3 222 }
leejoonwoo 4:4ea0f6305fa3 223
leejoonwoo 4:4ea0f6305fa3 224 //left
leejoonwoo 3:bb074b1d26fe 225 else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x < -0.1) {
leejoonwoo 3:bb074b1d26fe 226 motor_right.speed(-joystick_percent_x*sens);
leejoonwoo 3:bb074b1d26fe 227 state = LEFT;
leejoonwoo 4:4ea0f6305fa3 228 }
leejoonwoo 4:4ea0f6305fa3 229
leejoonwoo 4:4ea0f6305fa3 230 //right
leejoonwoo 3:bb074b1d26fe 231 else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x > 0.1) {
leejoonwoo 3:bb074b1d26fe 232 motor_left.speed(joystick_percent_x);
leejoonwoo 3:bb074b1d26fe 233 state = RIGHT;
leejoonwoo 3:bb074b1d26fe 234 }
leejoonwoo 4:4ea0f6305fa3 235
leejoonwoo 4:4ea0f6305fa3 236 //앞으로 움직일 때 encoder 차이값 계산, 물체감지 작동
leejoonwoo 4:4ea0f6305fa3 237 if(state == FRONT) {
leejoonwoo 3:bb074b1d26fe 238 speedGap();
leejoonwoo 3:bb074b1d26fe 239 object_detection();
leejoonwoo 3:bb074b1d26fe 240 }
leejoonwoo 4:4ea0f6305fa3 241
leejoonwoo 4:4ea0f6305fa3 242 //앞으로 움직이지 않을 때 encoder 값 실시간 reset
leejoonwoo 4:4ea0f6305fa3 243 else if(state == BACK || state == LEFT || state == RIGHT) {
leejoonwoo 3:bb074b1d26fe 244 left_encoder.reset();
leejoonwoo 3:bb074b1d26fe 245 right_encoder.reset();
leejoonwoo 3:bb074b1d26fe 246 }
leejoonwoo 3:bb074b1d26fe 247 }
leejoonwoo 3:bb074b1d26fe 248
leejoonwoo 4:4ea0f6305fa3 249
leejoonwoo 4:4ea0f6305fa3 250 //Xbee (Serial)로 받은 mavlink 데이터 처리
leejoonwoo 3:bb074b1d26fe 251 void read_data()
bcostm 0:5701b41769fd 252 {
leejoonwoo 3:bb074b1d26fe 253 led = 1;
leejoonwoo 3:bb074b1d26fe 254 uint8_t msgReceived = 0;
leejoonwoo 3:bb074b1d26fe 255
leejoonwoo 3:bb074b1d26fe 256 while(1) {
leejoonwoo 3:bb074b1d26fe 257 if(xbee.readable()) {
leejoonwoo 4:4ea0f6305fa3 258
leejoonwoo 4:4ea0f6305fa3 259 //xbee 데이터 read
leejoonwoo 3:bb074b1d26fe 260 uint8_t c = xbee.getc();
leejoonwoo 3:bb074b1d26fe 261
leejoonwoo 3:bb074b1d26fe 262 msgReceived = mavlink_parse_char(MAVLINK_COMM_1, c, &r_message, &r_status);
leejoonwoo 3:bb074b1d26fe 263
leejoonwoo 3:bb074b1d26fe 264 if(msgReceived > 0) {
leejoonwoo 3:bb074b1d26fe 265 led = 0;
leejoonwoo 4:4ea0f6305fa3 266
leejoonwoo 4:4ea0f6305fa3 267 //id 검사
leejoonwoo 3:bb074b1d26fe 268 if (r_message.sysid == 60|| r_message.sysid == 62) {
leejoonwoo 3:bb074b1d26fe 269 switch (r_message.msgid) {
leejoonwoo 3:bb074b1d26fe 270 case 3: { //JOYSTICK
leejoonwoo 3:bb074b1d26fe 271 mavlink_msg_joystick_decode(&r_message, &joystick);
leejoonwoo 3:bb074b1d26fe 272 rc_move();
leejoonwoo 3:bb074b1d26fe 273 break;
leejoonwoo 3:bb074b1d26fe 274 }
leejoonwoo 3:bb074b1d26fe 275 }
leejoonwoo 4:4ea0f6305fa3 276 }
leejoonwoo 3:bb074b1d26fe 277 }
leejoonwoo 3:bb074b1d26fe 278 } else {
leejoonwoo 3:bb074b1d26fe 279 led = 1;
leejoonwoo 3:bb074b1d26fe 280 }
bcostm 0:5701b41769fd 281 }
bcostm 0:5701b41769fd 282 }
bcostm 0:5701b41769fd 283
bcostm 0:5701b41769fd 284 int main()
bcostm 0:5701b41769fd 285 {
leejoonwoo 3:bb074b1d26fe 286 //tof i2c setting
leejoonwoo 3:bb074b1d26fe 287 tof1.begin();
leejoonwoo 3:bb074b1d26fe 288 tof1.setDistanceMode();
leejoonwoo 4:4ea0f6305fa3 289
leejoonwoo 4:4ea0f6305fa3 290 //write part thread start
leejoonwoo 3:bb074b1d26fe 291 thread.start(write_message);
leejoonwoo 4:4ea0f6305fa3 292 //read part (while)
leejoonwoo 3:bb074b1d26fe 293 read_data();
bcostm 0:5701b41769fd 294 }