Sam Shum
Sam
Revision 2:611a5eb132a1, committed 2020-09-13
- Comitter:
- s0313045
- Date:
- Sun Sep 13 04:30:54 2020 +0000
- Parent:
- 1:58d1caed28d4
- Commit message:
- by Sam
Changed in this revision
--- a/main.cpp Thu Oct 25 12:14:32 2018 +0000
+++ b/main.cpp Sun Sep 13 04:30:54 2020 +0000
@@ -1,4 +1,13 @@
#include "mbed.h"
+
+#include "ros.h"
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
#include "actiondrv.h"
#include "millis.h"
@@ -18,7 +27,7 @@
///////////////////////////
//Serial Action(D8,D2 ); // tx, rx
Serial Action(PB_6, PB_7 );
-Serial pc(USBTX, USBRX);
+//Serial pc(USBTX, USBRX);
@@ -41,13 +50,13 @@
bool newDataArrived=false;
char buffer[8];
-/////////////////////////
+
//Serial pc(USBTX, USBRX);
char counter = 0;
-actionDrv action1(1);
-actionDrv action2(2);
-actionDrv action3(3);
+actionDrv action1(3); //1
+actionDrv action2(1); //2
+actionDrv action3(2); //3
int motor1 = 0;
@@ -64,6 +73,10 @@
Ticker motor_updater;
Ticker odom_updater;
+
+Ticker ros_updater;
+
+
////////////////////////////////////
float now_x=0;
float now_y=0;
@@ -85,36 +98,16 @@
/////////////////////////////////////
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_angle = -90; //encoder coordinate system + 30 degree => global coordinate system
//float encoder_2_global_x = 0.34; //encoder to center distance in x (tung)
//float encoder_2_global_y = -0.235; //encoder to center distance in y (tung)
-float encoder_2_global_x = 0.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)
+float encoder_2_global_x = 0.33;//0.125; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.24;//0.37; //encoder to center distance in y (tung)
////////////////////TUNG////////////////
@@ -132,8 +125,8 @@
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 encoder2local_angle = -90 *pi/float(180);
+float encoder_position_angle =( ( 360-36.02737) ) / float(180) * pi ; //90 + angle to encoder location
float r = sqrt( ( encoder_2_global_x * encoder_2_global_x ) + ( encoder_2_global_y * encoder_2_global_y ) ); //encoder to center radius
@@ -154,11 +147,11 @@
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_x = tx + y_tbias - startup_offset_x_encoder;
- transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
+ //transformed_real_now_x = tx + y_tbias - startup_offset_x_encoder; //+
+ //transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
transformed_real_now_w= _A *pi/float(180) - startup_offset_w_encoder;
@@ -198,11 +191,11 @@
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_x_encoder = tx + y_tbias ;
- startup_offset_y_encoder = ty - x_tbias ;
+ //startup_offset_x_encoder = tx + y_tbias ;
+ //startup_offset_y_encoder = ty - x_tbias ; //+
startup_offset_w_encoder = _A *pi/float(180); //degree 2 rad
@@ -210,12 +203,6 @@
}
-
-
-
-
-
-
void ActionEncoder_init()
{
count=0;
@@ -300,82 +287,37 @@
return false;
}
-bool updated()
-{
- if (done) {
- done=false;
- return true;
- } else {
- return false;
- }
-}
-float getXangle()
-{
- return xangle;
-}
-float getYangle()
-{
- return yangle;
-}
-
-float getZangle()
+//void inverse(float global_x_vel, float global_y_vel, float w_vel) const std_msgs::UInt16& cmd_msg
+void inverse(const geometry_msgs::Twist& cmd_vel)
{
- return zangle;
-}
-
-float getXpos()
-{
- return pos_x;
-}
-
-float getYpos()
-{
- return pos_y;
+ float global_vel_x = cmd_vel.linear.x;
+ float global_vel_y = cmd_vel.linear.y;
+ float w_vel = cmd_vel.angular.z;
+
+ float local_x_vel = global_vel_x * cos( -transformed_real_now_w ) - global_vel_y * sin( -transformed_real_now_w );
+ float local_y_vel = global_vel_x * sin( -transformed_real_now_w ) + global_vel_y * cos( -transformed_real_now_w ); //local to global transformation (angle only)
+
+ motor1 = int( ( (-1) * sin(pi_div_3) * local_x_vel + cos(pi_div_3) * local_y_vel + d * w_vel ) * 60 / (wheelR * 2 * pi) * gear );
+ motor2 = int( ( (-1) * local_y_vel + d * w_vel ) * 60 / (wheelR * 2 * pi) * gear );
+ motor3 = int( ( sin(pi_div_3) * local_x_vel + cos(pi_div_3) * local_y_vel + d * w_vel ) * 60 / (wheelR * 2 * pi) * gear );
+
}
-float getAngleSpeed()
-{
- return angleSpeed;
-}
-
-bool isAlive()
-{
- if ((millis()-LastRead)<100) {
- return true;
- } else {
- return false;
- }
-}
+/////////////////////////
+ros::NodeHandle nh;
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &inverse );
-bool newDataAvailable()
-{
- if (newDataArrived) {
- newDataArrived=false;
- return true;
- } else return false;
-}
-
-char* reset()
-{
- return "ACT0";
-}
-
-char* calibrate()
-{
- return "ACTR";
-}
+nav_msgs::Odometry odom;
+geometry_msgs::TransformStamped odom_trans;
+
+ros::Publisher odom_publisher("odom", &odom);
+tf::TransformBroadcaster odom_broadcaster;
-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 );
-
-}
+/////////////////////////
@@ -387,111 +329,69 @@
wait(0.005);
}
+void ros_update()
+{
+
+odom_publisher.publish( &odom );
+odom_broadcaster.sendTransform(odom_trans);
+
+
+wait(0.005);
+nh.spinOnce();
+}
+
+
void odom_update()
{
-
-
calculatePos(now_x,now_y,now_w);
-
- /*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;
+ //geometry_msgs::Quaternion odom_quat = createQuaternionMsg(transformed_real_now_w );
+ geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(transformed_real_now_w); // *57.2957795
- 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");*/
-
-}
+ //first, we'll publish the transform over tf
+ odom_trans.header.stamp = nh.now();
+ odom_trans.header.frame_id = "odom";
+ odom_trans.child_frame_id = "base_link";
-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};
+ odom_trans.transform.translation.x = transformed_real_now_x ;
+ odom_trans.transform.translation.y = transformed_real_now_y ;
+ odom_trans.transform.translation.z = 0.0;
+ odom_trans.transform.rotation = odom_quat;
+
+ //send the transform
+ //next, we'll publish the odometry message over ROS
+ //nav_msgs::Odometry odom;
+ odom.header.stamp = nh.now();
+ odom.header.frame_id = "odom";
+ odom.child_frame_id = "base_link";
-////////////////////
-
+ //set the position
+ odom.pose.pose.position.x = transformed_real_now_x;
+ odom.pose.pose.position.y = transformed_real_now_y ;
+ odom.pose.pose.position.z = 0.0;
+ odom.pose.pose.orientation = odom_quat;
+
+ //set the velocity
+ //odom.child_frame_id = "base_link";
+ //odom.twist.twist.linear.x = vx;
+ //odom.twist.twist.linear.y = vy;
+ //odom.twist.twist.angular.z = vth;
+
+ //publish the message
+ //
+
+
+}
+
+
+
+int main() {
+
millisStart();
@@ -499,6 +399,34 @@
Action.baud(115200);
Action.format(8,SerialBase::None,1);
ActionEncoder_init();
+
+ nh.initNode();
+ odom_broadcaster.init(nh);
+ nh.advertise(odom_publisher);
+ nh.subscribe(sub);
+
+ while (!nh.connected() ) {nh.spinOnce();}
+ odom_trans.header.stamp = nh.now();
+ odom_trans.header.frame_id = "odom";
+ odom_trans.child_frame_id = "base_link";
+
+ odom_trans.transform.translation.x = 0.0;
+ odom_trans.transform.translation.y = 0.0 ;
+ odom_trans.transform.translation.z = 0.0;
+ odom_trans.transform.rotation = tf::createQuaternionFromYaw(0);
+
+ odom.header.stamp = nh.now();
+ odom.header.frame_id = "odom";
+ odom.child_frame_id = "base_link";
+
+ //set the position
+ odom.pose.pose.position.x = transformed_real_now_x;
+ odom.pose.pose.position.y = transformed_real_now_y ;
+ odom.pose.pose.position.z = 0.0;
+ odom.pose.pose.orientation = tf::createQuaternionFromYaw(0);;
+
+ ros_updater.attach(&ros_update, 0.3);
+
while(1)
{
if (Action.readable())
@@ -518,10 +446,6 @@
}
} //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++ ){
@@ -541,6 +465,9 @@
motor_updater.attach(&motor_update, RATE);
//odom_updater.attach(&odom_update, RATE);
+ ros_updater.attach(&ros_update, 0.5);
+
+
while(1)
@@ -554,38 +481,5 @@
}
-
-
-/*
- 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_front_is_two_wheel.txt Sun Sep 13 04:30:54 2020 +0000
@@ -0,0 +1,533 @@
+#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(3); //1
+actionDrv action2(1); //2
+actionDrv action3(2); //3
+
+
+int motor1 = 0;
+int motor2 = 0;
+int motor3 = 0;
+int motor4 = 0;
+
+float pi = 3.14159265;
+double pi_div_3 = 1.04719755;
+float d = 0.525;//0.43;
+float wheelR = 0.0508; //4 inch wheel
+float gear = 10;
+
+Ticker motor_updater;
+
+Ticker odom_updater;
+////////////////////////////////////
+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 = -90; //encoder coordinate system + 30 degree => global coordinate system
+//float encoder_2_global_x = 0.34; //encoder to center distance in x (tung)
+//float encoder_2_global_y = -0.235; //encoder to center distance in y (tung)
+
+
+float encoder_2_global_x = 0.33;//0.125; //encoder to center distance in x (tung)
+float encoder_2_global_y = 0.24;//0.37; //encoder to center distance in y (tung)
+////////////////////TUNG////////////////
+
+
+float transformed_real_now_x=0;
+float transformed_real_now_y=0;
+float transformed_real_now_w=0;
+
+
+float startup_offset_x_encoder =0;
+float startup_offset_y_encoder =0;
+float startup_offset_w_encoder=0;
+
+
+
+float encoder_to_center = sqrt( ( encoder_2_global_x * encoder_2_global_x ) + ( encoder_2_global_y * encoder_2_global_y ) );
+
+//#########################//
+float encoder2local_angle = -90 *pi/float(180);
+float encoder_position_angle =( ( 360-36.02737) ) / float(180) * pi ; //90 + angle to encoder location
+float r = sqrt( ( encoder_2_global_x * encoder_2_global_x ) + ( encoder_2_global_y * encoder_2_global_y ) ); //encoder to center radius
+
+
+void calculatePos(float _X,float _Y,float _A)
+{
+ float zangle = _A- 360 * int(_A / 360);
+ float zrangle = zangle *pi/float(180); //degree 2 rad
+
+ float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) ) - ( ( _Y / float(1000) ) * sin( -encoder2local_angle) );
+ float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) ) + ( ( _Y / float(1000) ) * cos( -encoder2local_angle) );
+
+ float s = copysign( sqrt( 2*( r*r ) - 2*(r*r)*cos(zrangle) ) , zrangle );
+
+ float x_bias = s * cos( zrangle / 2 );
+ float y_bias = s * sin( zrangle / 2 );
+
+ float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) ) - ( y_bias ) * ( sin( encoder_position_angle ) ) ;
+ float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) ) + ( y_bias ) * ( cos( encoder_position_angle ) ) ;
+
+
+ transformed_real_now_x = tx + y_tbias - startup_offset_x_encoder; //-
+ transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
+
+ //transformed_real_now_x = tx + y_tbias - startup_offset_x_encoder; //+
+ //transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
+
+
+ transformed_real_now_w= _A *pi/float(180) - startup_offset_w_encoder;
+
+
+}
+
+
+
+
+
+///////////////////////
+
+
+
+float x_PID_P = 0.5;
+float y_PID_P = 0.5;
+float w_PID_P = 1;
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+//////////////////////////////
+void start_calculatePos(float _X,float _Y,float _A)
+{
+ float zangle = _A- 360 * int(_A / 360);
+ float zrangle = zangle *pi/float(180); //degree 2 rad
+
+ float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) ) - ( ( _Y / float(1000) ) * sin( -encoder2local_angle) );
+ float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) ) + ( ( _Y / float(1000) ) * cos( -encoder2local_angle) );
+
+ float s = copysign( sqrt( 2*( r*r ) - 2*(r*r)*cos(zrangle) ) , zrangle );
+
+ float x_bias = s * cos( zrangle / 2 );
+ float y_bias = s * sin( zrangle / 2 );
+
+ float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) ) - ( y_bias ) * ( sin( encoder_position_angle ) ) ;
+ float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) ) + ( y_bias ) * ( cos( encoder_position_angle ) ) ;
+
+
+ startup_offset_x_encoder = tx + y_tbias ; //-
+ startup_offset_y_encoder = ty - x_tbias ;
+
+ //startup_offset_x_encoder = tx + y_tbias ;
+ //startup_offset_y_encoder = ty - x_tbias ; //+
+
+
+ startup_offset_w_encoder = _A *pi/float(180); //degree 2 rad
+
+
+}
+
+
+
+
+
+
+
+void ActionEncoder_init()
+{
+ count=0;
+ i=0;
+ done=0;
+ xangle=0;
+ yangle=0;
+ zangle=0;
+ d_angle=0;
+ pos_x=0;
+ pos_y=0;
+ angleSpeed=0;
+ temp_zangle=0;
+ LastRead=0;
+ newDataArrived=false;
+
+}
+
+bool readEncoder(char c)
+{
+ //sprintf(buffer,"%02X",c);
+ //sprintf(buffer,"%X",c);
+ //pc.printf(buffer);
+ //pc.printf("\r\n");
+
+ //sprintf(buffer,"%d",count);
+ //pc.printf(buffer);
+ //pc.printf("\r\n");
+ switch(count) {
+ case 0:
+ if (c==0x0d) count++;
+ else count=0;
+ break;
+ case 1:
+ if(c==0x0a) {
+ i=0;
+ count++;
+ } else if(c==0x0d) {}
+ else count=0;
+ break;
+ case 2:
+ posture.data[i]=c;
+ i++;
+ if(i>=24) {
+ i=0;
+ count++;
+ }
+ break;
+ case 3:
+ if(c==0x0a)count++;
+ else count=0;
+ break;
+ case 4:
+ if(c==0x0d) {
+ d_angle=posture.val[0]-temp_zangle;
+ if (d_angle<-180) {
+ d_angle=d_angle+360;
+ } else if (d_angle>180) {
+ d_angle=d_angle-360;
+ }
+
+ now_w+=d_angle;
+ temp_zangle=posture.val[0];
+ //xangle=posture.val[1];
+ //yangle=posture.val[2];
+ now_x=posture.val[3];
+ now_y=posture.val[4];
+ //angleSpeed=posture.val[5];
+ newDataArrived=true;
+
+ }
+ count=0;
+ done=1;
+ LastRead=millis();
+ return true;
+ //break;
+ default:
+ count=0;
+ break;
+ }
+
+ return false;
+}
+
+
+
+void inverse(float 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.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[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};
+
+/*
+ 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.5,.required_y = 0.5, .required_w = 45, .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 = 1, .required_y = 0.5, .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[4] = (point_info){.required_x = 0.5,.required_y = 0.5, .required_w = 45, .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.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[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 *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);
+
+ }
+
+*/
+
+
+
+
+
+}
--- a/main_rotate_stable_wtf.txt Thu Oct 25 12:14:32 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,553 +0,0 @@
-#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);
-
- }
-
-*/
-
-
-
-
-
-}
--- a/main_stable.txt Thu Oct 25 12:14:32 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,523 +0,0 @@
-#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);
-
- }
-
-*/
-
-
-
-
-
-}
--- a/main_stable2_still_on9.txt Thu Oct 25 12:14:32 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,583 +0,0 @@
-#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);
-
- }
-
-*/
-
-
-
-
-
-}
--- a/main_stable_ex_1_2018_10_25.txt Thu Oct 25 12:14:32 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,591 +0,0 @@
-#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);
-
- }
-
-*/
-
-
-
-
-
-}