samco
/
Nucleo_rtos_basic_f103rb
init
main.cpp
- Committer:
- leejoonwoo
- Date:
- 2020-01-30
- Revision:
- 4:4ea0f6305fa3
- Parent:
- 3:bb074b1d26fe
File content as of revision 4:4ea0f6305fa3:
//헤더파일 선언 #include "mbed.h" #include "mavlink.h" #include "motordriver.h" #include "Map.hpp" #include "rtos.h" #include "IncrementalEncoder.h" #include "VL53L1X.h" //모빌리티 상태 정의 (정지,앞,백,왼쪽,오른쪽) #define FRONT 1 #define NORMAL 0 #define BACK 2 #define LEFT 3 #define RIGHT 4 DigitalOut led (LED2); //Xbee (Serial)선언 RawSerial xbee(PC_10, PC_11, 115200); //Thread 선언 (read, write) Thread thread; Thread thread2; //Mutex mutex; // MAVLINK 관련 함수 선언 char buf[MAVLINK_MAX_PACKET_LEN]; mavlink_message_t message; mavlink_message_t r_message; mavlink_status_t status; mavlink_status_t r_status; mavlink_heartbeat_t heartbeat; mavlink_joystick_t joystick; unsigned len = 0; //MAVLINK ID 선언 uint8_t system_id = 10; uint8_t component_id = 15; //Motor 선언 (Motordriver) Motor motor_left(PA_9,PA_8,PB_15,1); Motor motor_right(PC_9,PC_6,PC_8,1); //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 선언(IncrementalEncoder) IncrementalEncoder right_encoder(PD_2); IncrementalEncoder left_encoder(PC_4); //tof 선언 VL53L1X tof1 (PB_9, PB_8); ///////////////////////////////////////////// /////////////////write/////////////////////// ///////////////////////////////////////////// int write_data (char *data) { } //Xbee(Serial)로 값을 내보내기 void write_data(char* buf, unsigned int len) { for(int i = 0; i<len; i++) { xbee.putc(buf[i]); } } //heartbeat(Mavlik) 속성 설정 void heartbeat_info() { //Thread 주기 설정 Thread::wait(2000); heartbeat.type = 0; heartbeat.mode = 1; mavlink_msg_heartbeat_encode (system_id, component_id, &message, &heartbeat); len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message); 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보낼 (heartbeat,motor,pwm) message send (loop) void write_message() { while(1) { heartbeat_info(); Motor_message_send(); pwm_message_send(); } } ///////////////////////////////// ////////////read///////////////// ///////////////////////////////// //물체감지 함수 void object_detection() { //Tof 거리값 read uint16_t distance = tof1.getDistance(); //물체감지 후 정지 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) { sens -= 0.01; } 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); } //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 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; } //앞으로 움직일 때 encoder 차이값 계산, 물체감지 작동 if(state == FRONT) { speedGap(); object_detection(); } //앞으로 움직이지 않을 때 encoder 값 실시간 reset else if(state == BACK || state == LEFT || state == RIGHT) { left_encoder.reset(); right_encoder.reset(); } } //Xbee (Serial)로 받은 mavlink 데이터 처리 void read_data() { led = 1; uint8_t msgReceived = 0; 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 mavlink_msg_joystick_decode(&r_message, &joystick); rc_move(); break; } } } } } else { led = 1; } } } int main() { //tof i2c setting tof1.begin(); tof1.setDistanceMode(); //write part thread start thread.start(write_message); //read part (while) read_data(); }