Revision 0:502b364c9f1d, committed 2018-10-21

Comitter:

s0313045

Date:

Sun Oct 21 19:38:09 2018 +0000

Child:

1:58d1caed28d4

Commit message:

main_stable

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ActionEncoder.hpp	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,49 @@
+/*
+ * ActionEncoder.hpp
+ *
+ *  Created on: 7 Mar 2018
+ *      Author: tung
+ */
+
+#ifndef INCLUDE_ACTIONENCODER_HPP_
+#define INCLUDE_ACTIONENCODER_HPP_
+
+#include <stdint.h>
+//#include <Serial.hpp>
+
+class ActionEncoder
+{
+private:
+    uint8_t count,i,done;
+    union{
+        uint8_t data[24];
+        float val[6];
+    }posture;
+    float xangle,yangle,zangle,pos_x,pos_y,angleSpeed,d_angle;
+    float temp_zangle;
+    bool newDataArrived;
+public:
+    ActionEncoder();
+    void readEncoder(char);
+    bool updated();
+    float getXangle();
+    float getYangle();
+    float getZangle();
+    float getXpos();
+    float getYpos();
+    float getAngleSpeed();
+    char * reset();
+    char * calibrate();
+    bool isAlive();
+    bool newDataAvailable();
+    uint32_t LastRead;
+
+};
+
+
+
+
+
+
+
+#endif /* INCLUDE_ACTIONENCODER_HPP_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ActionEncoder.txt	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,265 @@
+#include "mbed.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 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];
+
+
+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;
+
+
+}
+
+void 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;
+                }
+                zangle+=d_angle;
+                temp_zangle=posture.val[0];
+                xangle=posture.val[1];
+                yangle=posture.val[2];
+                pos_x=posture.val[3];
+                pos_y=posture.val[4];
+                angleSpeed=posture.val[5];
+                newDataArrived=true;
+                
+                pc.printf("GET\r\n");
+                sprintf(buffer, "%f", pos_x);
+                pc.printf(buffer);
+                pc.printf("  ");
+                sprintf(buffer, "%f", pos_y);
+                pc.printf(buffer);
+                pc.printf("  ");
+                sprintf(buffer, "%f", zangle);
+                pc.printf(buffer);
+                pc.printf("\r\n");
+            }
+            count=0;
+            done=1;
+            LastRead=millis();
+            break;
+        default:
+            //count=0;
+            break;
+    }
+}
+
+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 action_get()
+{
+
+    /*
+
+    // Loop just in case more than one character is in UART's receive FIFO buffer
+    // Stop if buffer full
+      while ((device.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
+          rx_buffer[rx_in] = device.getc();
+    // Uncomment to Echo to USB serial to watch data flow
+    //        monitor_device.putc(rx_buffer[rx_in]);
+          rx_in = (rx_in + 1) % buffer_size;
+      }
+      */
+
+    readEncoder(Action.getc());
+
+}
+/////////////////////////
+
+
+
+
+
+int main()
+{
+    //Action.attach(&action_get, Serial::RxIrq);
+    Action.baud(115200);
+    Action.format(8,SerialBase::None,1); 
+    
+    
+    ActionEncoder_init();
+
+    while(1) {
+
+        while (Action.readable()==1 ) 
+        {
+            //pc.printf("readable\r\n");
+            char c = Action.getc();
+            
+            readEncoder(c);
+            //pc.putc(c);
+            //pc.printf("\r\n");
+          
+          
+          /*
+                sprintf(buffer, "%f", pos_x);
+                pc.printf(buffer);
+                sprintf(buffer, "%f", pos_y);
+                pc.printf(buffer);
+                sprintf(buffer, "%f", zangle);
+                pc.printf(buffer);
+                pc.printf("\r\n");*/
+            
+
+        }
+
+    }
+
+
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actiondrv.cpp	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "actiondrv.h"
+
+static char msgsetmode[] = {0x2B,0x60,0x60,0x03,0x00,0x00,0x00,0x00};
+static char msgsetacc[] = {0x23,0x83,0x60,0x00,0x80,0x96,0x98,0x00};
+static char msgsetdec[] = {0x23,0x84,0x60,0x00,0x80,0x96,0x98,0x00};
+static char msgsetvel[] = {0x23,0xff,0x60,0x00,0x00,0x00,0x00,0x00};
+//static char heartbeat[] = {0x2B,0x17,0x10,0x00,0x64,0x00,0x00,0x00};
+static char msgenable[] = {0x01,0x00};
+static char enablemotor[] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00};
+DigitalOut led1(LED1);
+CAN can1(PA_11, PA_12, 500000);
+
+int rpm_to_pulse_per_s = 500*4/60;
+
+actionDrv::actionDrv(int _id)
+{
+    id = _id;
+       
+}
+
+void actionDrv::send(char* msg)
+{
+    //if(can1.write(CANMessage(0x600+ id, msg)))
+    //{
+    // led1 = !led1;
+    // wait(0.05);    
+    //}   
+    
+    while(!can1.write(CANMessage(0x600+ id, msg))) {wait(0.05);}
+}
+
+
+void actionDrv::Enable(){
+    //msgenable[1] = ((id >>24)& 0xFF);
+    //can1.write(CANMessage(0x000, msgenable));
+    //wait(0.2);
+    //send(enablemotor);
+    
+    msgenable[1] = ((id >>24)& 0xFF);
+    while(!can1.write(CANMessage(0x000, msgenable)));
+    wait(0.2);
+    send(enablemotor);
+    }
+void actionDrv::SetOperationalMode(){
+    send(msgsetmode);
+    }
+void actionDrv::Configvelocity(int acc, int dec){
+    
+    msgsetacc[4] = ((acc ) & 0xFF);
+    msgsetacc[5] = ((acc >> 8) & 0xFF);
+    msgsetacc[6] = ((acc >> 16) & 0xFF);
+    msgsetacc[7] = ((acc >> 24) & 0xFF);
+    msgsetdec[4] = ((dec ) & 0xFF);
+    msgsetdec[5] = ((dec >> 8) & 0xFF);
+    msgsetdec[6] = ((dec >> 16) & 0xFF);
+    msgsetdec[7] = ((dec >> 24)  & 0xFF);
+    
+    send(msgsetacc);
+    send(msgsetdec);
+    }
+void actionDrv::SetVelocity(int vel){
+    //vel = vel*500*4/60; // rpm to pulse per s
+    vel = vel*rpm_to_pulse_per_s;
+    msgsetvel[4] = ((vel ) &0xFF);
+    msgsetvel[5] = ((vel >> 8) &0xFF);
+    msgsetvel[6] = ((vel >> 16) &0xFF);
+    msgsetvel[7] = ((vel >> 24) & 0xFF);
+    send(msgsetvel);
+    }
+    
+void actionDrv::send_mod(char* msg)
+{
+    //if(can1.write(CANMessage(0x600+ id, msg)))
+    //{
+    // led1 = !led1;    
+    //}   
+    
+    while(!can1.write(CANMessage(0x600+ id, msg)));
+}
+ 
+void actionDrv::SetVelocity_mod(int vel){
+    //vel = vel*500*4/60; // rpm to pulse per s
+    vel = vel*rpm_to_pulse_per_s;
+    msgsetvel[4] = ((vel ) &0xFF);
+    msgsetvel[5] = ((vel >> 8) &0xFF);
+    msgsetvel[6] = ((vel >> 16) &0xFF);
+    msgsetvel[7] = ((vel >> 24) & 0xFF);
+    send_mod(msgsetvel);
+    }
+    
+
+    
+void actionDrv::stop(){
+    SetVelocity(0);
+    
+    }
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actiondrv.h	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+
+class actionDrv
+{
+public:    
+    actionDrv(int _id);
+    void SetOperationalMode();
+    void Enable();
+    void Configvelocity(int acc, int dec);
+    void SetVelocity(int vel);
+    void SetVelocity_mod(int vel);  //added
+    void send(char* msg);
+    void send_mod(char* msg);  //added
+    void stop();
+    int id, SDOid;
+    
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,547 @@
+#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=10;
+
+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 start_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);
+    startup_offset_x_encoder  =  (rotatedPosY/float(1000)+Xshift*sin(radAng)+Yshift*cos(radAng))+offsetX;
+    startup_offset_y_encoder  =  (rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY;
+    //transformed_real_now_w=_A;   //
+    startup_offset_w_encoder=radAng;
+}
+
+
+
+
+
+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                     +    startup_offset_x_encoder;
+       target_y    = points[point_counter].required_y                     +    startup_offset_y_encoder;
+       target_w    = points[point_counter].required_w  /float(180)*pi     +    startup_offset_w_encoder;
+       
+       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;
+                
+                start_calculatePos(  (now_x/1000),(now_y/1000), now_w  );   //global
+                break;
+            
+            }
+            
+        }
+    }    //start first to take offset from encoder... in case already moved
+    
+ 
+   target_x    = points[0].required_x    +  startup_offset_x_encoder;
+   target_y    = points[0].required_y    +  startup_offset_y_encoder;
+   target_w    = points[0].required_w    +  startup_offset_w_encoder;
+    
+    
+    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);
+         
+    }
+    
+*/
+         
+
+    
+
+       
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_stable.txt	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,523 @@
+#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);
+         
+    }
+    
+*/
+         
+
+    
+
+       
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.cpp	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  */
+
+volatile unsigned long  _millis;
+
+void millisStart(void) {
+    SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTick_Handler(void) {
+    _millis++;
+}
+
+unsigned long millis(void) {
+    return _millis;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.h	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,25 @@
+#ifndef MILLIS_H
+#define MILLIS_H
+/*
+ millis.h
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+  */
+
+void           millisStart(void);
+unsigned long  millis(void);
+
+#endif