Sam Shum
Sam
Revision 1:58d1caed28d4, committed 2018-10-25
- Comitter:
- s0313045
- Date:
- Thu Oct 25 12:14:32 2018 +0000
- Parent:
- 0:502b364c9f1d
- Child:
- 2:611a5eb132a1
- Commit message:
- Stable...; Can start sucessfully with strange orientation..; ; but e-stop reset not ok,,.... maybe driver not close beforehand?
Changed in this revision
--- a/main.cpp Sun Oct 21 19:38:09 2018 +0000
+++ b/main.cpp Thu Oct 25 12:14:32 2018 +0000
@@ -79,7 +79,7 @@
float speed_max_x=1;
float speed_max_y=1;
-float speed_max_w=10;
+float speed_max_w=1;
long odom_last_read= millis();
@@ -109,77 +109,113 @@
///////////////////////////
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)
+//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.125;//0.125;// -0.13 ; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.37; //0.35; //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;
+
+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 = 30 *pi/float(180);
+float encoder_position_angle =( ( 180 + 18.666914) ) / 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 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 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 - x_tbias - startup_offset_x_encoder;
+ // transformed_real_now_y = ty - y_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 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;
+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 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;
+ 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 - x_tbias ;
+ // startup_offset_y_encoder = ty - y_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;
@@ -357,8 +393,8 @@
calculatePos(now_x,now_y,now_w);
- /*
- sprintf(buffer, "%f", transformed_real_now_x);
+
+ /*sprintf(buffer, "%f", transformed_real_now_x);
pc.printf(buffer);
pc.printf(" ");
sprintf(buffer, "%f", transformed_real_now_y);
@@ -366,8 +402,8 @@
pc.printf(" ");
sprintf(buffer, "%f", transformed_real_now_w);
pc.printf(buffer);
- pc.printf("\r\n");*/
-
+ 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 ) )
@@ -378,9 +414,9 @@
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;
+ 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 *pi/float(180); ;//- startup_offset_w_encoder;
inverse( 0 , 0 , 0 );
return;
@@ -440,8 +476,15 @@
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};
+ points[0] = (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};
+ points[1] = (point_info){.required_x = 0.5,.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[2] = (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};
+ points[3] = (point_info){.required_x = 0, .required_y = 0.2, .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[4] = (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};
+ points[5] = (point_info){.required_x = 0, .required_y = 0, .required_w = 90, .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[6] = (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};
+ points[7] = (point_info){.required_x = 0, .required_y = 0, .required_w = -90, .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[8] = (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};
@@ -467,7 +510,7 @@
//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
+ start_calculatePos( (now_x),(now_y), now_w ); //global
break;
}
@@ -476,9 +519,9 @@
} //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;
+ 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 *pi/float(180); // - startup_offset_w_encoder;
for( int a = 1; a < 2; a++ ){
@@ -504,6 +547,7 @@
{
if (Action.readable())
{
+ //pc.putc('a');
char c = Action.getc();
if(readEncoder(c)) odom_update();
}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main_rotate_stable_wtf.txt Thu Oct 25 12:14:32 2018 +0000
@@ -0,0 +1,553 @@
+#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=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)
+
+
+float encoder_2_global_x = 0.125;// -0.13 ; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.37; //0.35; //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 = -encoder_2_global_angle*pi/float(180);
+float offsetA = -encoder_2_global_angle;
+
+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;
+
+void calculatePos(float _X,float _Y,float _A)
+{
+ float radAng=_A/float(180)*pi;
+
+ 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 + startup_offset_x_encoder;
+ transformed_real_now_y=(rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY + startup_offset_y_encoder;
+ //transformed_real_now_w=_A; //
+ transformed_real_now_w=radAng - 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 radAng=_A/float(180)*pi;
+ 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,.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.5,.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[2] = (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};
+ points[3] = (point_info){.required_x = 0, .required_y = 0.2, .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[4] = (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};
+ points[5] = (point_info){.required_x = 0, .required_y = 0, .required_w = 90, .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[6] = (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),(now_y), 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())
+ {
+ //pc.putc('a');
+ 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_stable2_still_on9.txt Thu Oct 25 12:14:32 2018 +0000
@@ -0,0 +1,583 @@
+#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=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)
+
+
+float encoder_2_global_x = 0.125;//0.125;// -0.13 ; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.37; //0.35; //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 = -encoder_2_global_angle*pi/float(180); //degree 2 rad
+float offsetA = -encoder_2_global_angle;
+
+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 ) );
+
+
+void calculatePos(float _X,float _Y,float _A)
+{
+ float radAng=_A*pi/float(180); //degree 2 rad
+ /*
+ posX=(float(local_posY)/1000 + self.paraX * sin(w_radian) + self.paraY * cos(w_radian) )*(1) +self.offsetX
+ posY=(float(local_posX)/1000 + self.pa+raX * 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 - startup_offset_x_encoder;
+ transformed_real_now_y=(rotatedPosX/float(1000)+Xshift*cos(radAng)-Yshift*sin(radAng))*(-1)+offsetY- startup_offset_y_encoder;
+ */
+
+ float rotatedPosX= _X*cos(-Ashift)-_Y*sin(-Ashift);
+ float rotatedPosY= _X*cos(-Ashift)+_Y*sin(-Ashift);
+
+ transformed_real_now_x= rotatedPosY/float(1000)+ encoder_to_center *sin(-radAng -Ashift) + 0*cos(-radAng -Ashift) - startup_offset_x_encoder ;
+ transformed_real_now_y= (rotatedPosX/float(1000)+ encoder_to_center *cos(-radAng -Ashift) - 0*sin(-radAng -Ashift) )*-1 - startup_offset_y_encoder ;
+
+
+ transformed_real_now_w=radAng - 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 radAng=_A*pi/float(180); //degree 2 rad
+
+
+/*
+ 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;
+*/
+
+ float rotatedPosX= _X*cos(-Ashift)-_Y*sin(-Ashift);
+ float rotatedPosY= _X*cos(-Ashift)+_Y*sin(-Ashift);
+
+ startup_offset_x_encoder= ((rotatedPosY/float(1000))+ encoder_to_center *sin(-radAng -Ashift) + 0*cos(-radAng -Ashift)) ;
+ startup_offset_y_encoder = (((rotatedPosX/float(1000))+ encoder_to_center *cos(-radAng -Ashift) - 0*sin(-radAng -Ashift)) )*-1 ;
+
+ 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 *pi/float(180); ;//- 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,.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.5,.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[2] = (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};
+ points[3] = (point_info){.required_x = 0, .required_y = 0.2, .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[4] = (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};
+ points[5] = (point_info){.required_x = 0, .required_y = 0, .required_w = 90, .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[6] = (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};
+ points[7] = (point_info){.required_x = 0, .required_y = 0, .required_w = -90, .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[8] = (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),(now_y), 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 *pi/float(180); // - 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())
+ {
+ //pc.putc('a');
+ 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_ex_1_2018_10_25.txt Thu Oct 25 12:14:32 2018 +0000
@@ -0,0 +1,591 @@
+#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=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)
+
+
+float encoder_2_global_x = 0.125;//0.125;// -0.13 ; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.37; //0.35; //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 = 30 *pi/float(180);
+float encoder_position_angle =( ( 180 + 18.666914) ) / 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 - x_tbias - startup_offset_x_encoder;
+ // transformed_real_now_y = ty - y_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 - x_tbias ;
+ // startup_offset_y_encoder = ty - y_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;
+}
+
+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 *pi/float(180); ;//- 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,.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.5,.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[2] = (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};
+ points[3] = (point_info){.required_x = 0, .required_y = 0.2, .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[4] = (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};
+ points[5] = (point_info){.required_x = 0, .required_y = 0, .required_w = 90, .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[6] = (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};
+ points[7] = (point_info){.required_x = 0, .required_y = 0, .required_w = -90, .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[8] = (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),(now_y), 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 *pi/float(180); // - 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())
+ {
+ //pc.putc('a');
+ 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);
+
+ }
+
+*/
+
+
+
+
+
+}