Sam Shum
Sam
Diff: main.cpp
- Revision:
- 2:611a5eb132a1
- Parent:
- 1:58d1caed28d4
--- 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);
-
- }
-
-*/
-
-
-
-
}