File content as of revision 3:bb074b1d26fe:

#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);

/////////////////////////////////////////////
/////////////////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()
{
    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();
    }
}

/////////////////////////////////
////////////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;

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;
    }

    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()
{
    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()
{
    //tof i2c setting
    tof1.begin();
    tof1.setDistanceMode();
    
    thread.start(write_message);
    read_data();
}