File content as of revision 6:4e03a22f2abb:

#include "mbed.h"
#include "uagv.h"

#define LEFT 0
#define RIGHT 1


/*******************************************************************************
  ROS NodeHandle
*******************************************************************************/
ros::NodeHandle nh;

/*******************************************************************************
  Publisher
*******************************************************************************/
std_msgs::UInt8 lift_status;                                      // 0 = unexpected result or error, 1 = lift up, 2 = lift down, 3 = halt, 4 = reserved for UAGV onload status
ros::Publisher lift_status_pub("liftStatus", &lift_status);

/*******************************************************************************
  Subscriber
*******************************************************************************/
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", commandVelocityCallback);
ros::Subscriber<std_msgs::UInt8> lift_cmd_sub("liftCmd", liftCmdCallback);

/*******************************************************************************
  Declaration for motor & BT command
*******************************************************************************/
float mDesiredDirection, mDesiredVelocity; //從BT來的command：角度-1~1, 速度 -1~1
float mMaxVelocity = 0.5; //最高速 = 0.5m/s
float mCurrentVelocity, mCurrentDirection; //目前的速度和角度
float safeVelocity = 1;

float goal_linear_velocity  = 0.0;
float goal_angular_velocity = 0.0;

short VELOCITY_CONSTANT_VAULE = 30000;    //cmd:16585~=1(m/s)
short LIMIT_X_MAX_VELOCITY = 18000;      //max cmd:8293~=0.5m/s
short LIMIT_X_MIN_VELOCITY = 1640;       //min value

int lin_vel1;  //左輪
int lin_vel2;  //右輪
int lin_vel1_t;  //左輪
int lin_vel2_t;  //右輪
long LencV_0;  //左輪
long RencV_0;  //右輪
long LencV_t;  //左輪
long RencV_t;  //右輪

/*******************************************************************************
  Declaration for LED
*******************************************************************************/
int brightness = 0;
int fadeAmount = 5;

/*******************************************************************************
  Declaration for control flags & other parameters
*******************************************************************************/
bool i2cinitial = true;
uint32_t delaytime;
 
DigitalOut LIFT_UP_PIN(D4);
DigitalOut LIFT_DOWN_PIN(D3);
//InterruptIn sw2(B1);
//I2C i2c( PTB1, PTB0);
I2C i2c( D14, D15);
 
//Serial serialPort(USBTX, USBRX);
//Serial serialPort( PTE0, PTE1);

char Rmotobuffer[3];
char Lmotobuffer[3];

int Right_ADDR = 0x14<<1;
int Left_ADDR  = 0x15<<1;
Timer t;
long last_debounce_time=0;

int main()
{
    // Initializing Mbed and public parameters
    t.start();
    i2c.frequency(100000);
    wait_ms(100);
    
    // Initializing ROS connection
    nh.getHardware()->setBaud(57600);
    nh.initNode();
    nh.subscribe(cmd_vel_sub);
    nh.advertise(lift_status_pub);
    nh.subscribe(lift_cmd_sub);
    nh.loginfo("Initialized done.");
    while(true){
        if(t.read_ms() - last_debounce_time>(1000/CMD_VEL_PUBLISH_PERIOD)){
            Driving_Left(lin_vel1);
            wait_ms(10);
            Driving_Right(lin_vel2);
            last_debounce_time = t.read_ms();
        }
        nh.spinOnce();
    }
}

/*******************************************************************************
  Callback function for cmd_vel msg
*******************************************************************************/
void commandVelocityCallback(const geometry_msgs::Twist& cmd_vel_msg)
{
  goal_linear_velocity  = cmd_vel_msg.linear.x;
  goal_angular_velocity = cmd_vel_msg.angular.z;
  controlMotorSpeed(goal_linear_velocity, goal_angular_velocity);
}

/*******************************************************************************
  Converter of motor command
*******************************************************************************/
void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
{
    float wheel_speed_cmd[2];

    wheel_speed_cmd[LEFT]  = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2);
    wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2);

    lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE;
    wait_ms(2);

    if (lin_vel1 > LIMIT_X_MAX_VELOCITY)       lin_vel1 =  LIMIT_X_MAX_VELOCITY;
    else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;
    else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
    else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;
    lin_vel1 = lin_vel1_t * 0.3 + lin_vel1 * 0.7;

    lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
    wait_ms(2);

    if (lin_vel2 > LIMIT_X_MAX_VELOCITY)       lin_vel2 =  LIMIT_X_MAX_VELOCITY;
    else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;
    else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
    else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;
    lin_vel2 = lin_vel2_t * 0.3 + lin_vel2 * 0.7;
}

/*******************************************************************************
  Callback function for lift_cmd msg
*******************************************************************************/
void liftCmdCallback(const std_msgs::UInt8& lift_cmd_msg)
{
    wait_ms(100);
    LIFT_UP_PIN=0;
    LIFT_DOWN_PIN=0;
    wait_ms(2000);
    switch (lift_cmd_msg.data) {
        case 1:         //up
            LIFT_UP_PIN=1;
            LIFT_DOWN_PIN=0;
 
            break;
        case 2:         //down
            LIFT_UP_PIN=0;
            LIFT_DOWN_PIN=1;
 
            break;
        case 3:         //stop
            LIFT_UP_PIN=0;
            LIFT_DOWN_PIN=0;
            
            break;
        default:         //stop
            LIFT_UP_PIN=0;
            LIFT_DOWN_PIN=0;
            
            break; 
    }
 
}

void Driving_Right(int rightOutPut)
{
    Rmotobuffer[0]= 0x00;
    Rmotobuffer[1]= rightOutPut>>8;
    Rmotobuffer[2]= rightOutPut;
    i2c.start();
    i2c.write(Right_ADDR);
    i2c.write(0);
    i2c.write(Rmotobuffer[1]);
    i2c.write(Rmotobuffer[2]);
    i2c.stop();
}
 
void Driving_Left(int leftOutPut)
{
    Lmotobuffer[0]= 0x00;
    Lmotobuffer[1]= leftOutPut>>8;
    Lmotobuffer[2]= leftOutPut;
    i2c.start();
    i2c.write(Left_ADDR);
    i2c.write(0);
    i2c.write(Lmotobuffer[1]);
    i2c.write(Lmotobuffer[2]);
    i2c.stop();
 
}