File content as of revision 5:daa981fdd231:

/*
 * 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 "esc.h"
//#include "MbedJSONValue.h"

#define spd 60


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

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=0;
char 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;    
}

void handlerFunction(const std_msgs::Int8& commandSend){
    
    commandRead = commandSend;
    switch (commandSend.data)
    {
        case 1: //forward
            SPEED =spd;
            DIR1 = 'F';
            ANG = 90;  
            break;
        case 2: //strong left
            SPEED =spd;
            DIR1 = 'F';
            ANG = 45;
            break;            
        case 3:  //strong right
            SPEED =spd;
            DIR1 = 'F';
            ANG = 135;
            break;    
        case 4: // weak left
            SPEED=spd;
            ANG = 75;
            DIR1 = 'F';
            break;
        case 5: //weak right 
            SPEED = spd;
            DIR1 = 'F';
            ANG =105;
            break;
        case 6: // mid left
            SPEED = spd;
            DIR1 = 'F';
            ANG =60;
            break;
        case 7: //mid right 
            SPEED = spd;
            DIR1 = 'F';
            ANG =120;
            break;
        case 8: //stop 
            SPEED = 1;
            DIR1  ='S';
            ANG   = 90;
            break;
        default :
            SPEED = 1;
            DIR1  ='S';
            ANG   = 90;
            break;
    }
    if (SPEED) 
    { //assign speed value to Motor_PWM
        float getPWM = SPEED/5000.0;
            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);                
                }
            }
    }
    
    if (ANG)
    {   //assign ANG value to the servo motor angle
        float Angle_Value = 0;
        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);
    }   
    
}

ros::Subscriber<std_msgs::Int8> sub("/data_msg", &handlerFunction);

int main() {
    
    nh.initNode();
    nh.subscribe(sub);
    
    nh.advertise(chatter);
    nh.advertise(yaw_value);
    
    esc1.period(0.01f);    
    esc1.write(default_PWM);
    sv.period(0.01f);
    sv.write(0.15);
    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);
    }
}