File content as of revision 2:611a5eb132a1:

#include "mbed.h"

#include "ros.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>

#include "actiondrv.h"

#include "millis.h"

/*
 * ActionEncoder.cpp
 *
 *  Created on: 7 Mar 2018
 *      Author: tung
 */

#include "ActionEncoder.hpp"
#include "Timer.h"



///////////////////////////
//Serial Action(D8,D2 ); // tx, rx
Serial Action(PB_6,  PB_7 );
//Serial pc(USBTX, USBRX);



union {
    uint8_t data[24];
    float val[6];
} posture;
int count=0;
int i=0;
int done=0;
float xangle=0;
float yangle=0;
float zangle=0;
float d_angle=0;
float pos_x=0;
float pos_y=0;
float angleSpeed=0;
float temp_zangle=0;
int   LastRead=0;
bool newDataArrived=false;

char buffer[8];


//Serial pc(USBTX, USBRX);
char counter = 0;
actionDrv action1(3); //1
actionDrv action2(1); //2
actionDrv action3(2); //3


int motor1 = 0;
int motor2 = 0;
int motor3 = 0;
int motor4 = 0;

float pi = 3.14159265;
double pi_div_3 = 1.04719755;
float d = 0.525;//0.43;
float wheelR = 0.0508; //4 inch wheel
float gear = 10;

Ticker motor_updater;

Ticker odom_updater;

Ticker ros_updater;


////////////////////////////////////
float now_x=0;
float now_y=0;
float now_w=0;

float target_x=0;
float target_y=0;
float target_w=0;

float tolerance_x=0.02;
float tolerance_y=0.02;
float tolerance_w=0.01;

float speed_max_x=1;
float speed_max_y=1;
float speed_max_w=1;

long odom_last_read= millis();

/////////////////////////////////////
const float RATE = 0.18;
//////////////////////////

///////////////////////////
float encoder_2_global_angle = -90;         //encoder coordinate system + 30 degree    =>  global coordinate system
//float encoder_2_global_x     =   0.34;    //encoder to center distance  in x   (tung)
//float encoder_2_global_y     =   -0.235;     //encoder to center distance  in y   (tung)


float encoder_2_global_x     = 0.33;//0.125;    //encoder to center distance  in x   (tung)
float encoder_2_global_y     = 0.24;//0.37;    //encoder to center distance  in y   (tung)
////////////////////TUNG////////////////


float transformed_real_now_x=0;
float transformed_real_now_y=0;
float transformed_real_now_w=0;


float startup_offset_x_encoder =0;
float startup_offset_y_encoder =0;
float startup_offset_w_encoder=0;



float encoder_to_center = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );

//#########################//
float encoder2local_angle = -90 *pi/float(180);
float encoder_position_angle =( ( 360-36.02737)  ) / float(180) * pi ;   //90 +  angle to encoder location
float r = sqrt( ( encoder_2_global_x  * encoder_2_global_x )  + ( encoder_2_global_y  *  encoder_2_global_y ) );   //encoder to center radius


void calculatePos(float _X,float _Y,float _A)
{
    float zangle  =  _A-   360 * int(_A / 360);
    float zrangle =  zangle *pi/float(180);    //degree 2 rad
    
    float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
    float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
    
    float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
    
    float x_bias = s * cos( zrangle / 2 );
    float y_bias = s * sin( zrangle / 2 );
    
    float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
    float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
    
    
   transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder; //-
   transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
    
    //transformed_real_now_x = tx + y_tbias   - startup_offset_x_encoder;  //+
    //transformed_real_now_y = ty - x_tbias   - startup_offset_y_encoder;
    
    
    transformed_real_now_w=   _A *pi/float(180)                                                                        -  startup_offset_w_encoder;
    
    
}





///////////////////////



float x_PID_P = 0.5;
float y_PID_P = 0.5;
float w_PID_P = 1;

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

//////////////////////////////
void start_calculatePos(float _X,float _Y,float _A)
{
    float zangle  =  _A-   360 * int(_A / 360);
    float zrangle =  zangle *pi/float(180);    //degree 2 rad
    
    float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) )  -  (  ( _Y / float(1000) )   *  sin( -encoder2local_angle) );
    float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) )  +  (  ( _Y / float(1000) )   *  cos( -encoder2local_angle) );
    
    float s  = copysign( sqrt(  2*( r*r )  -  2*(r*r)*cos(zrangle)  )    , zrangle );
    
    float x_bias = s * cos( zrangle / 2 );
    float y_bias = s * sin( zrangle / 2 );
    
    float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) )  - ( y_bias ) * ( sin( encoder_position_angle ) )    ;
    float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) )  + ( y_bias ) * ( cos( encoder_position_angle ) )    ;
    
    
    startup_offset_x_encoder = tx + y_tbias ;  //-
    startup_offset_y_encoder = ty - x_tbias ;
    
   //startup_offset_x_encoder = tx + y_tbias ;
   //startup_offset_y_encoder = ty - x_tbias ;  //+
    
    
    startup_offset_w_encoder =  _A *pi/float(180);    //degree 2 rad
    
    
}

void ActionEncoder_init()
{
    count=0;
    i=0;
    done=0;
    xangle=0;
    yangle=0;
    zangle=0;
    d_angle=0;
    pos_x=0;
    pos_y=0;
    angleSpeed=0;
    temp_zangle=0;
    LastRead=0;
    newDataArrived=false;

}

bool readEncoder(char c)
{
    //sprintf(buffer,"%02X",c);
    //sprintf(buffer,"%X",c);
    //pc.printf(buffer);
    //pc.printf("\r\n");
    
    //sprintf(buffer,"%d",count);
    //pc.printf(buffer);
    //pc.printf("\r\n");
    switch(count) {
        case 0:
            if (c==0x0d) count++;
            else count=0;
            break;
        case 1:
            if(c==0x0a) {
                i=0;
                count++;
            } else if(c==0x0d) {}
            else count=0;
            break;
        case 2:
            posture.data[i]=c;
            i++;
            if(i>=24) {
                i=0;
                count++;
            }
            break;
        case 3:
            if(c==0x0a)count++;
            else count=0;
            break;
        case 4:
            if(c==0x0d) {
                d_angle=posture.val[0]-temp_zangle;
                if (d_angle<-180) {
                    d_angle=d_angle+360;
                } else if (d_angle>180) {
                    d_angle=d_angle-360;
                }
                
                now_w+=d_angle;
                temp_zangle=posture.val[0];
                //xangle=posture.val[1];
                //yangle=posture.val[2];
                now_x=posture.val[3];
                now_y=posture.val[4];
                //angleSpeed=posture.val[5];
                newDataArrived=true;
                
            }
            count=0;
            done=1;
            LastRead=millis();
            return true;
            //break;
        default:
            count=0;
            break;
    }
    
    return false;
}




//void inverse(float global_x_vel, float global_y_vel, float w_vel) const std_msgs::UInt16& cmd_msg
void inverse(const geometry_msgs::Twist& cmd_vel)  
{
    float global_vel_x   =  cmd_vel.linear.x;
    float global_vel_y   =  cmd_vel.linear.y;
    float w_vel          =  cmd_vel.angular.z;
    
    float local_x_vel = global_vel_x * cos( -transformed_real_now_w  )  -  global_vel_y * sin( -transformed_real_now_w ); 
    float local_y_vel = global_vel_x * sin( -transformed_real_now_w  )  +  global_vel_y * cos( -transformed_real_now_w );  //local to global transformation   (angle only)
  
    motor1  =  int(   (    (-1) * sin(pi_div_3) * local_x_vel   +  cos(pi_div_3) * local_y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
    motor2  =  int(   (    (-1) *  local_y_vel + d * w_vel                                                      )  * 60 / (wheelR * 2 * pi)  * gear   ); 
    motor3  =  int(   (           sin(pi_div_3) * local_x_vel   +  cos(pi_div_3) * local_y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
    
}

/////////////////////////
ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &inverse );

nav_msgs::Odometry odom;
geometry_msgs::TransformStamped odom_trans;
 
ros::Publisher odom_publisher("odom", &odom);
tf::TransformBroadcaster odom_broadcaster;


/////////////////////////



void motor_update()
{
    action1.SetVelocity_mod(motor1  * -1 );
    action2.SetVelocity_mod(motor2  * -1 );
    action3.SetVelocity_mod(motor3  * -1 );
    wait(0.005);
}

void ros_update()
{

odom_publisher.publish( &odom );
odom_broadcaster.sendTransform(odom_trans);


wait(0.005);
nh.spinOnce();
}


void odom_update()
{
    calculatePos(now_x,now_y,now_w);
    
    //geometry_msgs::Quaternion odom_quat = createQuaternionMsg(transformed_real_now_w );
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(transformed_real_now_w);  // *57.2957795


    //first, we'll publish the transform over tf
    odom_trans.header.stamp = nh.now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = transformed_real_now_x ;
    odom_trans.transform.translation.y = transformed_real_now_y ;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    



    //next, we'll publish the odometry message over ROS
    //nav_msgs::Odometry odom;
    odom.header.stamp = nh.now();
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";

    //set the position
    odom.pose.pose.position.x = transformed_real_now_x;
    odom.pose.pose.position.y = transformed_real_now_y ;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    //set the velocity
    //odom.child_frame_id = "base_link";
    //odom.twist.twist.linear.x = vx;
    //odom.twist.twist.linear.y = vy;
    //odom.twist.twist.angular.z = vth;

    //publish the message    
    //
    
            
}
    


int main() {
    
    millisStart();
    
    
        
    Action.baud(115200);
    Action.format(8,SerialBase::None,1); 
    ActionEncoder_init();
    
    nh.initNode();
    odom_broadcaster.init(nh);
    nh.advertise(odom_publisher);
    nh.subscribe(sub);
    
    while (!nh.connected() )   {nh.spinOnce();}
        odom_trans.header.stamp = nh.now();
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
    
        odom_trans.transform.translation.x = 0.0;
        odom_trans.transform.translation.y = 0.0 ;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionFromYaw(0);
        
        odom.header.stamp = nh.now();
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";
    
        //set the position
        odom.pose.pose.position.x = transformed_real_now_x;
        odom.pose.pose.position.y = transformed_real_now_y ;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionFromYaw(0);;
        
    ros_updater.attach(&ros_update, 0.3);
    
    while(1) 
    {
        if (Action.readable())
        {
            char c = Action.getc();
            if (readEncoder(c))
            {
                //startup_offset_x_encoder = now_x/1000;
                //startup_offset_y_encoder = now_y/1000;
                //startup_offset_w_encoder = now_w/float(180)*pi;
                
                start_calculatePos(  (now_x),(now_y), now_w  );   //global
                break;
            
            }
            
        }
    }    //start first to take offset from encoder... in case already moved
    
    
    
    for( int a = 1; a < 2; a++ ){
      action1.Enable();
      action2.Enable();
      action3.Enable();
      wait(0.1);
      action1.SetOperationalMode();
      action2.SetOperationalMode();
      action3.SetOperationalMode();
      wait(0.1);
      action1.Configvelocity(100000, 100000);
      action2.Configvelocity(100000, 100000);
      action3.Configvelocity(100000, 100000);  
      wait(0.1);
   }
          
    motor_updater.attach(&motor_update, RATE);  
    //odom_updater.attach(&odom_update, RATE);
    ros_updater.attach(&ros_update, 0.5);
    

    
        
    while(1) 
    {
        if (Action.readable())
        {
            //pc.putc('a');
            char c = Action.getc();
            if(readEncoder(c)) odom_update();
        }
        
    }

       
}