File content as of revision 0:502b364c9f1d:

#include "mbed.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(1);
actionDrv action2(2);
actionDrv action3(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;
////////////////////////////////////
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=0.1;

long odom_last_read= millis();

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

///////////////////////////////////////
int point_counter=0;

struct point_info
{
   float required_x;
   float required_y;
   float required_w;
   float required_tolerance_x;
   float required_tolerance_y;
   float required_tolerance_w;
   float required_speed_max_x;
   float required_speed_max_y;
   float required_speed_max_w;
};

struct point_info points[100];




///////////////////////////
float encoder_2_global_angle = 30;         //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)
////////////////////TUNG////////////////

float    Xshift=  encoder_2_global_x;
float    Yshift=  encoder_2_global_y;
float    offsetX = -Yshift;
float    offsetY = Xshift;

float Ashift  =   -30*pi/float(180);
float offsetA =   -30;

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

void calculatePos(float _X,float _Y,float _A)
{
    float radAng=_A/float(180)*pi;
    /*
    posX=(float(local_posY)/1000 + self.paraX * sin(w_radian) + self.paraY * cos(w_radian) )*(1) +self.offsetX
    posY=(float(local_posX)/1000 + self.paraX * cos(w_radian) - self.paraY * sin(w_radian) )*(-1) +self.offsetY
    */
    float rotatedPosX=_X*cos(Ashift)+_Y*sin(Ashift);
    float rotatedPosY=-_X*sin(Ashift)+_Y*cos(Ashift);
    transformed_real_now_x=(rotatedPosY/float(1000)+Xshift*sin(radAng)+Yshift*cos(radAng))+offsetX;
    transformed_real_now_y=(rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY;
    //transformed_real_now_w=_A;   //
    transformed_real_now_w=radAng;
}





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


float x_PID_P = 0.5;
float y_PID_P = 0.5;
float w_PID_P = 0.1;

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

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

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;
}

bool updated()
{
    if (done) {
        done=false;
        return true;
    } else {
        return false;
    }

}

float getXangle()
{
    return xangle;
}

float getYangle()
{
    return yangle;
}

float getZangle()
{
    return zangle;
}

float getXpos()
{
    return pos_x;
}

float getYpos()
{
    return pos_y;
}

float getAngleSpeed()
{
    return angleSpeed;
}

bool isAlive()
{
    if ((millis()-LastRead)<100) {
        return true;
    } else {
        return false;
    }
}

bool newDataAvailable()
{
    if (newDataArrived) {
        newDataArrived=false;
        return true;
    } else return false;
}

char* reset()
{
    return "ACT0";
}

char* calibrate()
{
    return "ACTR";
}


void inverse(float x_vel, float y_vel, float w_vel)
{
    motor1  =  int(   (    (-1) * sin(pi_div_3) * x_vel   +  cos(pi_div_3) * y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
    motor2  =  int(   (    (-1) *  y_vel + d * w_vel                                                )  * 60 / (wheelR * 2 * pi)  * gear   ); 
    motor3  =  int(   (           sin(pi_div_3) * x_vel   +  cos(pi_div_3) * y_vel +  d * w_vel     )  * 60 / (wheelR * 2 * pi)  * gear   );
    
}



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

void odom_update()
{

   
    calculatePos(now_x,now_y,now_w);
    
    /*
    sprintf(buffer, "%f", transformed_real_now_x);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%f", transformed_real_now_y);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%f", transformed_real_now_w);
    pc.printf(buffer);
    pc.printf("\r\n");*/
    
    
    
    if ((    (fabs(target_x - transformed_real_now_x)) < tolerance_x ) && (   (fabs(target_y - transformed_real_now_y)) < tolerance_y )  && (   (fabs(target_w - transformed_real_now_w)) < tolerance_w )      )
    {
        point_counter+=1;

       tolerance_x = points[point_counter].required_tolerance_x;
       tolerance_y = points[point_counter].required_tolerance_x;
       tolerance_w = points[point_counter].required_tolerance_x;
       
       target_x    = points[point_counter].required_x ;
       target_y    = points[point_counter].required_y;
       target_w    = points[point_counter].required_w  /float(180)*pi;
       
       inverse( 0    ,     0         ,     0   );
       return;
     
    }
    

  
    float local_vel_x = (fabs(target_x - transformed_real_now_x) > tolerance_x ) ?   constrain(  (x_PID_P * (target_x - transformed_real_now_x)    ), -speed_max_x,    speed_max_x)  : 0 ;
    float local_vel_y = (fabs(target_y - transformed_real_now_y) > tolerance_y ) ?   constrain(  (y_PID_P * (target_y - transformed_real_now_y)    ), -speed_max_y,    speed_max_y)  : 0 ;
    float local_vel_w = (fabs(target_w - transformed_real_now_w) > tolerance_w ) ?   constrain(  (w_PID_P * (target_w - transformed_real_now_w)    ), -speed_max_w,    speed_max_w)  : 0 ;
    
    
    
    float global_vel_x = local_vel_x * cos( -transformed_real_now_w  )  -  local_vel_y * sin( -transformed_real_now_w ); 
    float global_vel_y = local_vel_x * sin( -transformed_real_now_w  )  +  local_vel_y * cos( -transformed_real_now_w );  //local to global transformation   (angle only)
    
    /*
    pc.printf("X: ");
    sprintf(buffer, "%f", transformed_real_now_x);
    pc.printf(buffer);
    pc.printf("  Y: ");
    sprintf(buffer, "%f", transformed_real_now_y);
    pc.printf(buffer);
    pc.printf("  W: ");
    sprintf(buffer, "%f", transformed_real_now_w);
    pc.printf(buffer);
    
    pc.printf(" | Global: ");
    sprintf(buffer, "%f", global_vel_x);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%f", global_vel_y);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%f", local_vel_w);
    pc.printf(buffer);*/
    
    
  
    inverse( global_vel_x   ,  global_vel_y       ,     local_vel_w   );
    
    /*
    pc.printf(" | Motor: ");
    sprintf(buffer, "%d", motor1);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%d", motor2);
    pc.printf(buffer);
    pc.printf("  ");
    sprintf(buffer, "%d", motor3);
    pc.printf(buffer);
    pc.printf("\r\n");*/
    
}

int main() {
    //while(1){
////////////////////////////
    points[0] = (point_info){.required_x = 0.2,.required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
    points[1] = (point_info){.required_x = 0,  .required_y = 0, .required_w = 0, .required_tolerance_x = 0.05, .required_tolerance_y = 0.05, .required_tolerance_w = 0.01, .required_speed_max_x = 0.3, .required_speed_max_y = 0.3, .required_speed_max_w = 0.01};
    




////////////////////
        
    millisStart();
    
    
        
    Action.baud(115200);
    Action.format(8,SerialBase::None,1); 
    ActionEncoder_init();
    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;
                break;
            
            }
            
        }
    }    //start first to take offset from encoder... in case already moved
    
 
   target_x    = points[0].required_x;
   target_y    = points[0].required_y;
   target_w    = points[0].required_w;
    
    
    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);
    
        
    while(1) 
    {
        if (Action.readable())
        {
            char c = Action.getc();
            if(readEncoder(c)) odom_update();
        }
        
    }

 
        
/*
        while (Action.readable()==1 ) 
        {
            char c = Action.getc();   
            readEncoder(c);
           
        }
*/
    
    
/*
    while(1)
    {
         
         inverse(0.2,0,0);
         wait(1);
         inverse(-0.2,0,0);
         wait(1);
         
         inverse(0,0,0.25);
         wait(1);
         inverse(0,0,-0.25);
         wait(1);
         
    }
    
*/
         

    

       
}