samco
/
Nucleo_rtos_basic_f103rb
init
Diff: main.cpp
- Revision:
- 3:bb074b1d26fe
- Parent:
- 2:35f13b7f3659
- Child:
- 4:4ea0f6305fa3
diff -r 35f13b7f3659 -r bb074b1d26fe main.cpp --- a/main.cpp Thu Nov 23 13:09:25 2017 +0000 +++ b/main.cpp Wed Jan 29 03:02:47 2020 +0000 @@ -1,31 +1,254 @@ #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); +//SERIAL +//Serial pc(USBTX, USBRX,115200); +RawSerial xbee(PC_10, PC_11, 115200); + +//Thread +Thread thread; +Thread thread2; +Mutex mutex; + +// OF 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; + +uint8_t system_id = 10; +uint8_t component_id = 15; + +//Motordriver , RC MAP +Motor motor_left(PA_9,PA_8,PB_15,1); +Motor motor_right(PC_9,PC_6,PC_8,1); +//pwm +float joystick_percent_x = 0; +float joystick_percent_y = 0; + +Map map_x (0, 1024, -0.3, 0.3); +Map map_y (0, 1024, -0.3, 0.3); + +//Encoder +IncrementalEncoder right_encoder(PD_2); +IncrementalEncoder left_encoder(PC_4); + +//tof +VL53L1X tof1 (PB_9, PB_8); -void print_char(char c = '*') +///////////////////////////////////////////// +/////////////////write/////////////////////// +///////////////////////////////////////////// +int write_data (char *data) +{ +} +void write_data(char* buf, unsigned int len) +{ + for(int i = 0; i<len; i++) { + xbee.putc(buf[i]); + } +} + +void heartbeat_info() +{ + 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); +} + +mavlink_motor_t motor; +void Motor_message_send() { - printf("%c", c); - fflush(stdout); + 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); +} +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); +} + +void write_message() +{ + while(1) { + heartbeat_info(); + Motor_message_send(); + pwm_message_send(); + } } -Thread thread; +///////////////////////////////// +////////////read///////////////// +///////////////////////////////// +void object_detection() +{ + uint16_t distance = tof1.getDistance(); + + if(distance < 400) + { + motor_left.speed (0); + motor_right.speed(0); + } +} +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; + } + +} +mavlink_joystick_t local_joystick; -DigitalOut led1(LED1); +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); + + if ( local_joystick.joystick_click == 0 ) { + motor_left.speed (0); + motor_right.speed (0); + } + //move forward/back + 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; + } -void print_thread() + 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); + } + 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); + } + 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); + } + 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 + 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; + } + 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; + } + 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; + } + 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) + { + speedGap(); + object_detection(); + } + else if(state == BACK || state == LEFT || state == RIGHT) + { + left_encoder.reset(); + right_encoder.reset(); + } + + +} + +void read_data() { - while (true) { - wait(1); - print_char(); + led = 1; + uint8_t msgReceived = 0; + + while(1) { + if(xbee.readable()) { + + uint8_t c = xbee.getc(); + + msgReceived = mavlink_parse_char(MAVLINK_COMM_1, c, &r_message, &r_status); + + if(msgReceived > 0) { + led = 0; + 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() { - printf("\n\n*** RTOS basic example ***\n"); - - thread.start(print_thread); - - while (true) { - led1 = !led1; - wait(0.5); - } + //tof i2c setting + tof1.begin(); + tof1.setDistanceMode(); + + thread.start(write_message); + read_data(); }