/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */

/*
*  left_forward    PC_0  
*  left_backward   PC_3  
*  right_forward   PC_2
*  right_backward  PC_1
*  left_pwm        PA_6
*  right_pwm       PA_7
*  encoder_left_a_add_int PC_10
*  encoder_left_b_add_int PC_11
*  encoder_right_a_add_int PC_12
*  encoder_right_b_add_int PD_2
*
*/

#include"mbed.h"
#include <ros.h>
#include <string>
#include <std_msgs/String.h>
#include <iomanip>
#include <locale>
#include <sstream>
#define PWM_STEP 0.001f

DigitalOut myled(LED1);
ros::NodeHandle  nh;

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

DigitalOut left_forward(PC_0);
DigitalOut left_backward(PC_3);

DigitalOut right_forward(PC_2);
DigitalOut right_backward(PC_1);

DigitalOut led(LED1);
PwmOut left_pwm(PA_6);
PwmOut right_pwm(PA_7);

InterruptIn button(USER_BUTTON);

InterruptIn encoder_left_a_add_int(PC_10);
InterruptIn encoder_left_b_add_int(PC_11);

InterruptIn encoder_right_a_add_int(PC_12);
InterruptIn encoder_right_b_add_int(PD_2);

float leftPwmOut=0.0f;
float rightPwmOut=0.0f;

int pwmDirection=1;

long long left_encoder_counter = 0;
long long right_encoder_counter = 0;

void left_encoder(void)
{
    left_encoder_counter++;
}

void right_encoder(void)
{
    right_encoder_counter++;
}

void setup(void)
{
    left_forward=1;
    left_backward=0;
    right_forward=1;
    right_backward=0;

    left_pwm.period_us(100);
    right_pwm.period_us(100);
    //left_pwm.write(0.50f);
    //moto_pwm.write(1.0f);
    encoder_left_a_add_int.rise(&left_encoder);
    encoder_left_b_add_int.rise(&left_encoder);

    encoder_right_a_add_int.rise(&right_encoder);
    encoder_right_b_add_int.rise(&right_encoder);
}

float getPwm(float pwmout)
{
    float result=0.0;

    if(pwmDirection) {
        result = pwmout + PWM_STEP;
        if(result >= 1.0f) {
            result = 1.0f;
            pwmDirection = 0;
        }
    } else {
        result = pwmout - PWM_STEP;
        if(result <= 0.0f) {
            result = 0.0f;
            pwmDirection = 1.0f;
        }
    }
    return result;
}

void publishChatter()
{
    std::stringstream ss;
    ss<<"[odom]L:"<<left_encoder_counter<<",R:"<<right_encoder_counter;

    const std::string tmp = ss.str();
    const char* cstr = tmp.c_str();
    str_msg.data=cstr;
    chatter.publish( &str_msg );
    nh.spinOnce();
}
int main()
{

    nh.initNode();
    nh.advertise(chatter);

    //int i = 12345;

    setup();
    //button.rise(&adder);
    //encoder_add_int.rise(&encoder);
    //nh.initNode();
    //nh.advertise(chatter);
    while(1) {

        leftPwmOut=getPwm(leftPwmOut);
        rightPwmOut=getPwm(rightPwmOut);

        //right_pwm.write(rightPwmOut);
        //left_pwm.write(leftPwmOut);
        right_pwm.write(0.4f);
        left_pwm.write(0.4f);

        //myled = 1; // LED is ON
        wait(0.01); // 10 ms
        //myled = 0; // LED is OFF
        //wait(0.2); // 1 sec
        publishChatter();
    }

}

$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0