File content as of revision 0:4a5e692e02bf:

#include "mbed.h"
#include "PID.h"
#include <ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Vector3.h>
PwmOut mypwm_left(D7);

DigitalOut myledg(D13); // IO used by pwm_io function
DigitalOut myledy(D14); // IO used by pwm_io function
DigitalOut myledr(D15); // IO used by pwm_io function

AnalogIn leftFSR(A0);
AnalogIn rightFSR(A3);
//gitalOut myled(LED1);
//DigitalOut myled(LED2);
Ticker timer1;  
//Serial pc(SERIAL_TX, SERIAL_RX);



//int right_count = 1900;
//int init__right_PW = 1900;
int left_count = 1250;
int init__left_PW = 1250;

int count = 1250;
int init_PW=1250;

void init_TIMER(void);
void timer1_interrupt(void);

float meas_left;
float meas_right;


float T_read = 0;
float Td = 500.0;
float err = 0;
float err_old = 0;
float derivative = 0;
float dt = 0.002;
PID servo_pid(1.5, 1.0, 0.1, dt);
float PI_out = 0;
float PIDout = 0;
float Kp = 1.0;
float Kd = 1.0;
float Scale = 0.005;
int det = 0;
int count_light = 0;
int cmd_g = 0; 
// ROS NODE
ros:: NodeHandle nh;

geometry_msgs::Vector3 fsr_msg;
ros::Publisher fsr("stm_msg", &fsr_msg);

void grip_callback(const std_msgs::Int32& msg)  
{   
    //myled = 0;
    std_msgs::Int32 command = msg;

    det = command.data;
    
    
    if (det==1400)
    {
        cmd_g = 1250;
        //myled = 1;
        while(cmd_g < 1400)
        {
            cmd_g = cmd_g + 5;
            mypwm_left.pulsewidth_us(cmd_g);  
        } 
     
    }
    else
    {
        mypwm_left.pulsewidth_us(init__left_PW);  
    
    }
    
}


ros::Subscriber<std_msgs::Int32> sub("gripper_command",grip_callback);


int main() {
//    pc.printf("GOGOGO\n"); 
   
//    pc.baud(9600);
    mypwm_left.period_ms(20);
    //mypwm_right.period_ms(20);
    mypwm_left.pulsewidth_us(init__left_PW);
    //mypwm_right.pulsewidth_us(init__right_PW );
//    
//    
//   printf("pwm set to %.2f %%\n", mypwm.read() * 100);
//   
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(fsr);
    init_TIMER();
   
    while(1) {
        
        
        if(det==1250)
        {
            fsr_msg.x = 0.0;
            fsr_msg.y = T_read;
            fsr_msg.z = left_count;
        }
        else if ( T_read > 500.0 || left_count <= 1400)
        {
            fsr_msg.x = 3;
            fsr_msg.y = T_read;
            fsr_msg.z = left_count;
        }
        
        count_light = count_light + 1;
        if(T_read > 180.0)
        {
            myledg = 1;
            count_light = 0;
        }
        else if(count_light<5)
        {
            myledg = 1;
        }
        else
        {
            myledg = 0;
        }
        
        fsr.publish( &fsr_msg);
        
        nh.spinOnce();
        wait_ms(1000);
        
    
    //pc.printf("pwm set to %d %%\n", init_PW);

   
    }
}

void init_TIMER(){
 
    timer1.attach_us(&timer1_interrupt, 10000.0);
}

void timer1_interrupt(){
    
    meas_left = leftFSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    meas_left = meas_left * 1000; // Change the value to be in the 0 to 3300 range
    
    meas_right = rightFSR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
    meas_right = meas_right * 1000; // Change the value to be in the 0 to 3300 range
    //printf("%.0f\t%.0f\r", meas_left, meas_right);
    
    if(meas_right>meas_left)
    {
        T_read = meas_right;
    }
    else
    {
        T_read = meas_left;
    }
    //printf("%.0f\t%.0f\t%.0f\r", meas_left, meas_right,T_read);
    
    err = Td - T_read;
    derivative = ( err - err_old ) / dt;
    err_old = err;
    
    

    //PI_out = Kp * err + Kd * derivative;
    //right_count = right_count - int(Scale * PI_out);
   

   
   
   
    servo_pid.Compute(Td, T_read);
    PIDout = servo_pid.output;
    left_count = left_count + int(0.002 * PIDout);
    
//    printf("%d \n", left_count);
    
    //printf("%.0f \r", meas_right);
    ///// left_count = left_count - 1;
    //right_count = right_count - 10;  
    
    
    if(left_count > 1400)        //1700
    {
        left_count = 1400;       //1850
    }
    
    /*if(right_count < 1750)      //1700
    {
        right_count = 2000;     //1850
    }
    */
    
    
    //mypwm_right.pulsewidth_us(left_count);
    //pc.printf("pwm set to %d %%\n", left_count);
    //pc.printf("pwm set to %d %%\n", right_count);
    
}