samco / Mbed OS Nucleo_rtos_basic_f103rb

init

Dependencies:   VL53L1X Map

main.cpp@3:bb074b1d26fe, 2020-01-29 (annotated)

Committer:
leejoonwoo
Date:
Wed Jan 29 03:02:47 2020 +0000
Revision:
3:bb074b1d26fe
Parent:
2:35f13b7f3659
Child:
4:4ea0f6305fa3
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bcostm 0:5701b41769fd 1 #include "mbed.h"
leejoonwoo 3:bb074b1d26fe 2 #include "mavlink.h"
leejoonwoo 3:bb074b1d26fe 3 #include "motordriver.h"
leejoonwoo 3:bb074b1d26fe 4 #include "Map.hpp"
leejoonwoo 3:bb074b1d26fe 5 #include "rtos.h"
leejoonwoo 3:bb074b1d26fe 6 #include "IncrementalEncoder.h"
leejoonwoo 3:bb074b1d26fe 7 #include "VL53L1X.h"
leejoonwoo 3:bb074b1d26fe 8
leejoonwoo 3:bb074b1d26fe 9 #define FRONT 1
leejoonwoo 3:bb074b1d26fe 10 #define NORMAL 0
leejoonwoo 3:bb074b1d26fe 11 #define BACK 2
leejoonwoo 3:bb074b1d26fe 12 #define LEFT 3
leejoonwoo 3:bb074b1d26fe 13 #define RIGHT 4
leejoonwoo 3:bb074b1d26fe 14
leejoonwoo 3:bb074b1d26fe 15 DigitalOut led (LED2);
leejoonwoo 3:bb074b1d26fe 16 //SERIAL
leejoonwoo 3:bb074b1d26fe 17 //Serial pc(USBTX, USBRX,115200);
leejoonwoo 3:bb074b1d26fe 18 RawSerial xbee(PC_10, PC_11, 115200);
leejoonwoo 3:bb074b1d26fe 19
leejoonwoo 3:bb074b1d26fe 20 //Thread
leejoonwoo 3:bb074b1d26fe 21 Thread thread;
leejoonwoo 3:bb074b1d26fe 22 Thread thread2;
leejoonwoo 3:bb074b1d26fe 23 Mutex mutex;
leejoonwoo 3:bb074b1d26fe 24
leejoonwoo 3:bb074b1d26fe 25 // OF MAVLINK
leejoonwoo 3:bb074b1d26fe 26 char buf[MAVLINK_MAX_PACKET_LEN];
leejoonwoo 3:bb074b1d26fe 27
leejoonwoo 3:bb074b1d26fe 28 mavlink_message_t message;
leejoonwoo 3:bb074b1d26fe 29 mavlink_message_t r_message;
leejoonwoo 3:bb074b1d26fe 30 mavlink_status_t status;
leejoonwoo 3:bb074b1d26fe 31 mavlink_status_t r_status;
leejoonwoo 3:bb074b1d26fe 32
leejoonwoo 3:bb074b1d26fe 33 mavlink_heartbeat_t heartbeat;
leejoonwoo 3:bb074b1d26fe 34 mavlink_joystick_t joystick;
leejoonwoo 3:bb074b1d26fe 35
leejoonwoo 3:bb074b1d26fe 36 unsigned len = 0;
leejoonwoo 3:bb074b1d26fe 37
leejoonwoo 3:bb074b1d26fe 38 uint8_t system_id = 10;
leejoonwoo 3:bb074b1d26fe 39 uint8_t component_id = 15;
leejoonwoo 3:bb074b1d26fe 40
leejoonwoo 3:bb074b1d26fe 41 //Motordriver , RC MAP
leejoonwoo 3:bb074b1d26fe 42 Motor motor_left(PA_9,PA_8,PB_15,1);
leejoonwoo 3:bb074b1d26fe 43 Motor motor_right(PC_9,PC_6,PC_8,1);
leejoonwoo 3:bb074b1d26fe 44 //pwm
leejoonwoo 3:bb074b1d26fe 45 float joystick_percent_x = 0;
leejoonwoo 3:bb074b1d26fe 46 float joystick_percent_y = 0;
leejoonwoo 3:bb074b1d26fe 47
leejoonwoo 3:bb074b1d26fe 48 Map map_x (0, 1024, -0.3, 0.3);
leejoonwoo 3:bb074b1d26fe 49 Map map_y (0, 1024, -0.3, 0.3);
leejoonwoo 3:bb074b1d26fe 50
leejoonwoo 3:bb074b1d26fe 51 //Encoder
leejoonwoo 3:bb074b1d26fe 52 IncrementalEncoder right_encoder(PD_2);
leejoonwoo 3:bb074b1d26fe 53 IncrementalEncoder left_encoder(PC_4);
leejoonwoo 3:bb074b1d26fe 54
leejoonwoo 3:bb074b1d26fe 55 //tof
leejoonwoo 3:bb074b1d26fe 56 VL53L1X tof1 (PB_9, PB_8);
bcostm 0:5701b41769fd 57
leejoonwoo 3:bb074b1d26fe 58 /////////////////////////////////////////////
leejoonwoo 3:bb074b1d26fe 59 /////////////////write///////////////////////
leejoonwoo 3:bb074b1d26fe 60 /////////////////////////////////////////////
leejoonwoo 3:bb074b1d26fe 61 int write_data (char *data)
leejoonwoo 3:bb074b1d26fe 62 {
leejoonwoo 3:bb074b1d26fe 63 }
leejoonwoo 3:bb074b1d26fe 64 void write_data(char* buf, unsigned int len)
leejoonwoo 3:bb074b1d26fe 65 {
leejoonwoo 3:bb074b1d26fe 66 for(int i = 0; i<len; i++) {
leejoonwoo 3:bb074b1d26fe 67 xbee.putc(buf[i]);
leejoonwoo 3:bb074b1d26fe 68 }
leejoonwoo 3:bb074b1d26fe 69 }
leejoonwoo 3:bb074b1d26fe 70
leejoonwoo 3:bb074b1d26fe 71 void heartbeat_info()
leejoonwoo 3:bb074b1d26fe 72 {
leejoonwoo 3:bb074b1d26fe 73 Thread::wait(2000);
leejoonwoo 3:bb074b1d26fe 74 heartbeat.type = 0;
leejoonwoo 3:bb074b1d26fe 75 heartbeat.mode = 1;
leejoonwoo 3:bb074b1d26fe 76
leejoonwoo 3:bb074b1d26fe 77 mavlink_msg_heartbeat_encode (system_id, component_id, &message, &heartbeat);
leejoonwoo 3:bb074b1d26fe 78 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 3:bb074b1d26fe 79
leejoonwoo 3:bb074b1d26fe 80 write_data(buf, len);
leejoonwoo 3:bb074b1d26fe 81 }
leejoonwoo 3:bb074b1d26fe 82
leejoonwoo 3:bb074b1d26fe 83 mavlink_motor_t motor;
leejoonwoo 3:bb074b1d26fe 84 void Motor_message_send()
bcostm 0:5701b41769fd 85 {
leejoonwoo 3:bb074b1d26fe 86 Thread::wait(500);
leejoonwoo 3:bb074b1d26fe 87 motor.encoder_left = (uint16_t)left_encoder.readTotal();
leejoonwoo 3:bb074b1d26fe 88 motor.encoder_right = (uint16_t)right_encoder.readTotal();
leejoonwoo 3:bb074b1d26fe 89
leejoonwoo 3:bb074b1d26fe 90 mavlink_msg_motor_encode (system_id, component_id, &message, &motor);
leejoonwoo 3:bb074b1d26fe 91 len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 3:bb074b1d26fe 92
leejoonwoo 3:bb074b1d26fe 93 write_data(buf, len);
leejoonwoo 3:bb074b1d26fe 94 }
leejoonwoo 3:bb074b1d26fe 95 mavlink_pwm_t pwm;
leejoonwoo 3:bb074b1d26fe 96 void pwm_message_send()
leejoonwoo 3:bb074b1d26fe 97 {
leejoonwoo 3:bb074b1d26fe 98 Thread::wait(500);
leejoonwoo 3:bb074b1d26fe 99
leejoonwoo 3:bb074b1d26fe 100 pwm.motor_left_pwm = (uint16_t)joystick_percent_x;
leejoonwoo 3:bb074b1d26fe 101 pwm.motor_right_pwm = (uint16_t)joystick_percent_y;
leejoonwoo 3:bb074b1d26fe 102
leejoonwoo 3:bb074b1d26fe 103 mavlink_msg_pwm_encode (system_id, component_id, &message, &pwm);
leejoonwoo 3:bb074b1d26fe 104 len=mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
leejoonwoo 3:bb074b1d26fe 105 write_data(buf,len);
leejoonwoo 3:bb074b1d26fe 106 }
leejoonwoo 3:bb074b1d26fe 107
leejoonwoo 3:bb074b1d26fe 108 void write_message()
leejoonwoo 3:bb074b1d26fe 109 {
leejoonwoo 3:bb074b1d26fe 110 while(1) {
leejoonwoo 3:bb074b1d26fe 111 heartbeat_info();
leejoonwoo 3:bb074b1d26fe 112 Motor_message_send();
leejoonwoo 3:bb074b1d26fe 113 pwm_message_send();
leejoonwoo 3:bb074b1d26fe 114 }
bcostm 0:5701b41769fd 115 }
bcostm 0:5701b41769fd 116
leejoonwoo 3:bb074b1d26fe 117 /////////////////////////////////
leejoonwoo 3:bb074b1d26fe 118 ////////////read/////////////////
leejoonwoo 3:bb074b1d26fe 119 /////////////////////////////////
leejoonwoo 3:bb074b1d26fe 120 void object_detection()
leejoonwoo 3:bb074b1d26fe 121 {
leejoonwoo 3:bb074b1d26fe 122 uint16_t distance = tof1.getDistance();
leejoonwoo 3:bb074b1d26fe 123
leejoonwoo 3:bb074b1d26fe 124 if(distance < 400)
leejoonwoo 3:bb074b1d26fe 125 {
leejoonwoo 3:bb074b1d26fe 126 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 127 motor_right.speed(0);
leejoonwoo 3:bb074b1d26fe 128 }
leejoonwoo 3:bb074b1d26fe 129 }
leejoonwoo 3:bb074b1d26fe 130 int state = NORMAL;
leejoonwoo 3:bb074b1d26fe 131 float sens = 1;
leejoonwoo 3:bb074b1d26fe 132 void speedGap()
leejoonwoo 3:bb074b1d26fe 133 {
leejoonwoo 3:bb074b1d26fe 134 int gap = (right_encoder.readTotal()) - (left_encoder.readTotal());
leejoonwoo 3:bb074b1d26fe 135
leejoonwoo 3:bb074b1d26fe 136 //left over
leejoonwoo 3:bb074b1d26fe 137 if (gap > 0 && sens < 1.5)
leejoonwoo 3:bb074b1d26fe 138 {
leejoonwoo 3:bb074b1d26fe 139 sens -= 0.01;
leejoonwoo 3:bb074b1d26fe 140 }
leejoonwoo 3:bb074b1d26fe 141 if (gap < 0 && sens > 0.5)
leejoonwoo 3:bb074b1d26fe 142 {
leejoonwoo 3:bb074b1d26fe 143 sens += 0.01;
leejoonwoo 3:bb074b1d26fe 144 }
leejoonwoo 3:bb074b1d26fe 145
leejoonwoo 3:bb074b1d26fe 146 }
leejoonwoo 3:bb074b1d26fe 147 mavlink_joystick_t local_joystick;
bcostm 2:35f13b7f3659 148
leejoonwoo 3:bb074b1d26fe 149 void rc_move()
leejoonwoo 3:bb074b1d26fe 150 {
leejoonwoo 3:bb074b1d26fe 151 local_joystick = joystick;
leejoonwoo 3:bb074b1d26fe 152 joystick_percent_x = map_x.Calculate(local_joystick.joystick_x);
leejoonwoo 3:bb074b1d26fe 153 joystick_percent_y = map_y.Calculate(local_joystick.joystick_y);
leejoonwoo 3:bb074b1d26fe 154
leejoonwoo 3:bb074b1d26fe 155 if ( local_joystick.joystick_click == 0 ) {
leejoonwoo 3:bb074b1d26fe 156 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 157 motor_right.speed (0);
leejoonwoo 3:bb074b1d26fe 158 }
leejoonwoo 3:bb074b1d26fe 159 //move forward/back
leejoonwoo 3:bb074b1d26fe 160 if( joystick_percent_y >= -0.1 && joystick_percent_y <= 0.1 && joystick_percent_x >= -0.1 && joystick_percent_x <=0.1) {
leejoonwoo 3:bb074b1d26fe 161 motor_left.speed (0);
leejoonwoo 3:bb074b1d26fe 162 motor_right.speed (0);
leejoonwoo 3:bb074b1d26fe 163 state = NORMAL;
leejoonwoo 3:bb074b1d26fe 164 }
bcostm 0:5701b41769fd 165
leejoonwoo 3:bb074b1d26fe 166 else if ( joystick_percent_x > 0.1 && joystick_percent_y > 0.1) {
leejoonwoo 3:bb074b1d26fe 167 motor_left.speed (joystick_percent_y+joystick_percent_x);
leejoonwoo 3:bb074b1d26fe 168 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 169 }
leejoonwoo 3:bb074b1d26fe 170 else if (joystick_percent_x < -0.1 && joystick_percent_y >0.1) {
leejoonwoo 3:bb074b1d26fe 171 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 172 motor_right.speed ((joystick_percent_y +(-1*joystick_percent_x))*sens);
leejoonwoo 3:bb074b1d26fe 173 }
leejoonwoo 3:bb074b1d26fe 174 else if (joystick_percent_x < -0.1 && joystick_percent_y < -0.1) {
leejoonwoo 3:bb074b1d26fe 175 motor_left.speed (joystick_percent_y );
leejoonwoo 3:bb074b1d26fe 176 motor_right.speed ((joystick_percent_y + joystick_percent_x)*sens);
leejoonwoo 3:bb074b1d26fe 177 }
leejoonwoo 3:bb074b1d26fe 178 else if (joystick_percent_x > 0.1 && joystick_percent_y < - 0.1) {
leejoonwoo 3:bb074b1d26fe 179 motor_left.speed ((joystick_percent_y - joystick_percent_x));
leejoonwoo 3:bb074b1d26fe 180 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 181 }
leejoonwoo 3:bb074b1d26fe 182 //forward,backward
leejoonwoo 3:bb074b1d26fe 183 else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y>0.1) {
leejoonwoo 3:bb074b1d26fe 184 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 185 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 186 state = BACK;
leejoonwoo 3:bb074b1d26fe 187 }
leejoonwoo 3:bb074b1d26fe 188 else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y < -0.1) {
leejoonwoo 3:bb074b1d26fe 189 motor_left.speed (joystick_percent_y);
leejoonwoo 3:bb074b1d26fe 190 motor_right.speed (joystick_percent_y*sens);
leejoonwoo 3:bb074b1d26fe 191 state = FRONT;
leejoonwoo 3:bb074b1d26fe 192 }
leejoonwoo 3:bb074b1d26fe 193 else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x < -0.1) {
leejoonwoo 3:bb074b1d26fe 194 motor_right.speed(-joystick_percent_x*sens);
leejoonwoo 3:bb074b1d26fe 195 state = LEFT;
leejoonwoo 3:bb074b1d26fe 196 }
leejoonwoo 3:bb074b1d26fe 197 else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x > 0.1) {
leejoonwoo 3:bb074b1d26fe 198 motor_left.speed(joystick_percent_x);
leejoonwoo 3:bb074b1d26fe 199 state = RIGHT;
leejoonwoo 3:bb074b1d26fe 200 }
leejoonwoo 3:bb074b1d26fe 201
leejoonwoo 3:bb074b1d26fe 202 if(state == FRONT)
leejoonwoo 3:bb074b1d26fe 203 {
leejoonwoo 3:bb074b1d26fe 204 speedGap();
leejoonwoo 3:bb074b1d26fe 205 object_detection();
leejoonwoo 3:bb074b1d26fe 206 }
leejoonwoo 3:bb074b1d26fe 207 else if(state == BACK || state == LEFT || state == RIGHT)
leejoonwoo 3:bb074b1d26fe 208 {
leejoonwoo 3:bb074b1d26fe 209 left_encoder.reset();
leejoonwoo 3:bb074b1d26fe 210 right_encoder.reset();
leejoonwoo 3:bb074b1d26fe 211 }
leejoonwoo 3:bb074b1d26fe 212
leejoonwoo 3:bb074b1d26fe 213
leejoonwoo 3:bb074b1d26fe 214 }
leejoonwoo 3:bb074b1d26fe 215
leejoonwoo 3:bb074b1d26fe 216 void read_data()
bcostm 0:5701b41769fd 217 {
leejoonwoo 3:bb074b1d26fe 218 led = 1;
leejoonwoo 3:bb074b1d26fe 219 uint8_t msgReceived = 0;
leejoonwoo 3:bb074b1d26fe 220
leejoonwoo 3:bb074b1d26fe 221 while(1) {
leejoonwoo 3:bb074b1d26fe 222 if(xbee.readable()) {
leejoonwoo 3:bb074b1d26fe 223
leejoonwoo 3:bb074b1d26fe 224 uint8_t c = xbee.getc();
leejoonwoo 3:bb074b1d26fe 225
leejoonwoo 3:bb074b1d26fe 226 msgReceived = mavlink_parse_char(MAVLINK_COMM_1, c, &r_message, &r_status);
leejoonwoo 3:bb074b1d26fe 227
leejoonwoo 3:bb074b1d26fe 228 if(msgReceived > 0) {
leejoonwoo 3:bb074b1d26fe 229 led = 0;
leejoonwoo 3:bb074b1d26fe 230 if (r_message.sysid == 60|| r_message.sysid == 62) {
leejoonwoo 3:bb074b1d26fe 231 switch (r_message.msgid) {
leejoonwoo 3:bb074b1d26fe 232 case 3: { //JOYSTICK
leejoonwoo 3:bb074b1d26fe 233 mavlink_msg_joystick_decode(&r_message, &joystick);
leejoonwoo 3:bb074b1d26fe 234 rc_move();
leejoonwoo 3:bb074b1d26fe 235 break;
leejoonwoo 3:bb074b1d26fe 236 }
leejoonwoo 3:bb074b1d26fe 237 }
leejoonwoo 3:bb074b1d26fe 238 }
leejoonwoo 3:bb074b1d26fe 239 }
leejoonwoo 3:bb074b1d26fe 240 } else {
leejoonwoo 3:bb074b1d26fe 241 led = 1;
leejoonwoo 3:bb074b1d26fe 242 }
bcostm 0:5701b41769fd 243 }
bcostm 0:5701b41769fd 244 }
bcostm 0:5701b41769fd 245
bcostm 0:5701b41769fd 246 int main()
bcostm 0:5701b41769fd 247 {
leejoonwoo 3:bb074b1d26fe 248 //tof i2c setting
leejoonwoo 3:bb074b1d26fe 249 tof1.begin();
leejoonwoo 3:bb074b1d26fe 250 tof1.setDistanceMode();
leejoonwoo 3:bb074b1d26fe 251
leejoonwoo 3:bb074b1d26fe 252 thread.start(write_message);
leejoonwoo 3:bb074b1d26fe 253 read_data();
bcostm 0:5701b41769fd 254 }