File content as of revision 7:4cf73c284530:

/*
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */
 
#include "mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float32.h>
#include "Servo.h"
#include "MPU6050.h"
#include <geometry_msgs/Twist.h>
//#include "esc.h"
//#include "MbedJSONValue.h"

#define spd 60
int flag_key=1;

//Serial uart(PA_11, PA_12, 57600);
ros::NodeHandle nh;
DigitalOut myled(LED1);
//PWM
PwmOut sv(D5);
PwmOut esc1(D9);

geometry_msgs::Twist cmd;

std_msgs::Float32 mpu6050_yaw;
std_msgs::Int8 commandRead;
ros::Publisher chatter("chatter", &commandRead);
ros::Publisher yaw_value("yaw_value", &mpu6050_yaw);

int SPEED=0;
int ANG=90;
int DIR1='S';
int Num = 0;
char inData[80];
float dt,Yaw, lgyroZ=0;
int past_ANG = 90;
int gyroZ_raw= 0;
float default_PWM = 0.15;

MPU6050 mpu6050;
Timer t;
 
void MPU6050_calculation()
{
    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) 
    {  // check if data ready interrupt
        mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
        mpu6050.getGres();
        gyroZ_raw = (int)gyroCount[2]*gRes; // - gyroBias[2];
    }
    Now = t.read_us();
    dt = (float)((Now - lastUpdate)/1000000.0f) ;
    lastUpdate = Now;
    Yaw += ((lgyroZ+gyroZ_raw)/2)*dt;
    lgyroZ = gyroZ_raw;
    mpu6050_yaw.data=Yaw;    
}

int spd_msg;
int ang_msg;
       
void dc_motor(int DIR1, float SPEED)
{
        float getPWM = SPEED/5000;
        if(DIR1=='F')
            {
                for(int i=0; i<=SPEED; i++)
                {
                    esc1 = (default_PWM + getPWM*i/SPEED);
                    //wait_ms(20);
                }
            }
        else if(DIR1=='S')
            {
                esc1 = default_PWM;
                // wait_ms(20); 
            }
        else if(DIR1=='B')
            {
                for(int i=0; i<=SPEED; i++)
                {
                    esc1 = (default_PWM - getPWM*i/SPEED);
                    //wait_ms(20);                
                }
            }            
}

void handlerFunction_key(const geometry_msgs::Twist& commandSend){
        flag_key=0;
        cmd= commandSend;
        spd_msg=cmd.linear.x;
        ang_msg=cmd.angular.z;
        flag_key=cmd.linear.y;
        
        if(ang_msg>=150)ang_msg=150;
        if(ang_msg<=30)ang_msg=30;
        if(spd_msg>=200)spd_msg=200;
        if(spd_msg<=-200)spd_msg=-200;
     
        if(spd_msg==0){
            SPEED = spd_msg;
            DIR1  ='S';
            ANG   = ang_msg;
            }
        else if(spd_msg>0){
            SPEED = spd_msg;
            DIR1  ='F';
            ANG   = ang_msg;
            }
        else{
            SPEED = -spd_msg;
            DIR1  ='B';
            ANG   = ang_msg;
            }
        
       // float getPWM = 0.063 + ((SPEED+200)/400.0) * 0.065 ; 
        //float getPWM = SPEED/5000.0;
        dc_motor(DIR1, SPEED);
 
        if (ANG!=0)
        {   //assign ANG value to the servo motor angle
            float Angle_Value = 0;
            Angle_Value = 0.06 + (ANG-30)/120.0 * (0.06);
            sv.write(Angle_Value);
            /*
            if(ANG>=past_ANG)
            {
                for(int i=past_ANG; i<=ANG; i++)
                {   
                    Angle_Value = 0.05+i/90.0*0.1;
                    sv.write(Angle_Value);
                    //wait(0.02);    
                }
            }
            else if(past_ANG >= ANG)
            {
                for(int i=past_ANG; i>=ANG; i--)
                {
                    Angle_Value = 0.05+i/90.0*0.1;
                    sv.write(Angle_Value);
                    //wait(0.02);
                }
            }
            */
            past_ANG = ANG;
            //wait(0.02);
        }   
    
    }
    
void handlerFunction_Cam(const geometry_msgs::Twist& commandSend){
    if(flag_key==true){
        cmd= commandSend;
        spd_msg=cmd.linear.x;
        ang_msg=cmd.angular.z;
        if(ang_msg>=150)ang_msg=150;
        if(ang_msg<=30)ang_msg=30;
        if(spd_msg>=200)spd_msg=200;
        if(spd_msg<=-200)spd_msg=-200;
     
        if(spd_msg==0){
            SPEED = spd_msg;
            DIR1  ='S';
            ANG   = ang_msg;
            }
        else if(spd_msg>0){
            SPEED = spd_msg;
            DIR1  ='F';
            ANG   = ang_msg;
            }
        else{
            SPEED = -spd_msg;
            DIR1  ='B';
            ANG   = ang_msg;
            } 
        
    
        //float getPWM = SPEED/5000.0;
        dc_motor(DIR1, SPEED); 
           
        if (ANG != 0)
        {   //assign ANG value to the servo motor angle
            float Angle_Value;
            
            Angle_Value = 0.06 + 0.06 * (ANG - 30.0) / 120.0;
            sv.write(Angle_Value);
           
            past_ANG = ANG;
            //wait(0.02);
        }   
    }
}

//ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);
ros::Subscriber<geometry_msgs::Twist> sub("/data_msg", &handlerFunction_Cam);
ros::Subscriber<geometry_msgs::Twist> sub_key("/data_msg_key", &handlerFunction_key);
int main() {
    
    nh.initNode();
    nh.subscribe(sub);
    nh.subscribe(sub_key);
    
    nh.advertise(chatter);
    nh.advertise(yaw_value);
    
    esc1.period(0.01f);    
    esc1.write(default_PWM);
    sv.period(0.0164f);
    //sv.write(0.09);
    sv.write(0.09);
    i2c.frequency(400000);
    t.start();
    
    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // read WHO_AM_I register
    
    if (whoami == 0x68)
    {
        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values

        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
        {
            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
            mpu6050.initMPU6050();
        } 
    } 
    else {
        while(1) ; // Loop forever if communication doesn't happen
    }
    wait_ms(100);
    
    while (1) {
        //Drive_Contrl(); 
        MPU6050_calculation();
        
        yaw_value.publish(&mpu6050_yaw);       
        chatter.publish(&commandRead);
        
        nh.spinOnce();
        wait_ms(100);
    }
}