Sam Shum / ros_mbed_base_controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@2:611a5eb132a1, 2020-09-13 (annotated)

Committer:
s0313045
Date:
Sun Sep 13 04:30:54 2020 +0000
Revision:
2:611a5eb132a1
Parent:
1:58d1caed28d4 
by Sam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s0313045 0:502b364c9f1d 1 #include "mbed.h"
s0313045 2:611a5eb132a1 2
s0313045 2:611a5eb132a1 3 #include "ros.h"
s0313045 2:611a5eb132a1 4 #include <nav_msgs/Odometry.h>
s0313045 2:611a5eb132a1 5 #include <geometry_msgs/Twist.h>
s0313045 2:611a5eb132a1 6 #include <geometry_msgs/Quaternion.h>
s0313045 2:611a5eb132a1 7 #include <geometry_msgs/TransformStamped.h>
s0313045 2:611a5eb132a1 8 #include <tf/tf.h>
s0313045 2:611a5eb132a1 9 #include <tf/transform_broadcaster.h>
s0313045 2:611a5eb132a1 10
s0313045 0:502b364c9f1d 11 #include "actiondrv.h"
s0313045 0:502b364c9f1d 12
s0313045 0:502b364c9f1d 13 #include "millis.h"
s0313045 0:502b364c9f1d 14
s0313045 0:502b364c9f1d 15 /*
s0313045 0:502b364c9f1d 16 * ActionEncoder.cpp
s0313045 0:502b364c9f1d 17 *
s0313045 0:502b364c9f1d 18 * Created on: 7 Mar 2018
s0313045 0:502b364c9f1d 19 * Author: tung
s0313045 0:502b364c9f1d 20 */
s0313045 0:502b364c9f1d 21
s0313045 0:502b364c9f1d 22 #include "ActionEncoder.hpp"
s0313045 0:502b364c9f1d 23 #include "Timer.h"
s0313045 0:502b364c9f1d 24
s0313045 0:502b364c9f1d 25
s0313045 0:502b364c9f1d 26
s0313045 0:502b364c9f1d 27 ///////////////////////////
s0313045 0:502b364c9f1d 28 //Serial Action(D8,D2 ); // tx, rx
s0313045 0:502b364c9f1d 29 Serial Action(PB_6, PB_7 );
s0313045 2:611a5eb132a1 30 //Serial pc(USBTX, USBRX);
s0313045 0:502b364c9f1d 31
s0313045 0:502b364c9f1d 32
s0313045 0:502b364c9f1d 33
s0313045 0:502b364c9f1d 34 union {
s0313045 0:502b364c9f1d 35 uint8_t data[24];
s0313045 0:502b364c9f1d 36 float val[6];
s0313045 0:502b364c9f1d 37 } posture;
s0313045 0:502b364c9f1d 38 int count=0;
s0313045 0:502b364c9f1d 39 int i=0;
s0313045 0:502b364c9f1d 40 int done=0;
s0313045 0:502b364c9f1d 41 float xangle=0;
s0313045 0:502b364c9f1d 42 float yangle=0;
s0313045 0:502b364c9f1d 43 float zangle=0;
s0313045 0:502b364c9f1d 44 float d_angle=0;
s0313045 0:502b364c9f1d 45 float pos_x=0;
s0313045 0:502b364c9f1d 46 float pos_y=0;
s0313045 0:502b364c9f1d 47 float angleSpeed=0;
s0313045 0:502b364c9f1d 48 float temp_zangle=0;
s0313045 0:502b364c9f1d 49 int LastRead=0;
s0313045 0:502b364c9f1d 50 bool newDataArrived=false;
s0313045 0:502b364c9f1d 51
s0313045 0:502b364c9f1d 52 char buffer[8];
s0313045 2:611a5eb132a1 53
s0313045 0:502b364c9f1d 54
s0313045 0:502b364c9f1d 55 //Serial pc(USBTX, USBRX);
s0313045 0:502b364c9f1d 56 char counter = 0;
s0313045 2:611a5eb132a1 57 actionDrv action1(3); //1
s0313045 2:611a5eb132a1 58 actionDrv action2(1); //2
s0313045 2:611a5eb132a1 59 actionDrv action3(2); //3
s0313045 0:502b364c9f1d 60
s0313045 0:502b364c9f1d 61
s0313045 0:502b364c9f1d 62 int motor1 = 0;
s0313045 0:502b364c9f1d 63 int motor2 = 0;
s0313045 0:502b364c9f1d 64 int motor3 = 0;
s0313045 0:502b364c9f1d 65 int motor4 = 0;
s0313045 0:502b364c9f1d 66
s0313045 0:502b364c9f1d 67 float pi = 3.14159265;
s0313045 0:502b364c9f1d 68 double pi_div_3 = 1.04719755;
s0313045 0:502b364c9f1d 69 float d = 0.525;//0.43;
s0313045 0:502b364c9f1d 70 float wheelR = 0.0508; //4 inch wheel
s0313045 0:502b364c9f1d 71 float gear = 10;
s0313045 0:502b364c9f1d 72
s0313045 0:502b364c9f1d 73 Ticker motor_updater;
s0313045 0:502b364c9f1d 74
s0313045 0:502b364c9f1d 75 Ticker odom_updater;
s0313045 2:611a5eb132a1 76
s0313045 2:611a5eb132a1 77 Ticker ros_updater;
s0313045 2:611a5eb132a1 78
s0313045 2:611a5eb132a1 79
s0313045 0:502b364c9f1d 80 ////////////////////////////////////
s0313045 0:502b364c9f1d 81 float now_x=0;
s0313045 0:502b364c9f1d 82 float now_y=0;
s0313045 0:502b364c9f1d 83 float now_w=0;
s0313045 0:502b364c9f1d 84
s0313045 0:502b364c9f1d 85 float target_x=0;
s0313045 0:502b364c9f1d 86 float target_y=0;
s0313045 0:502b364c9f1d 87 float target_w=0;
s0313045 0:502b364c9f1d 88
s0313045 0:502b364c9f1d 89 float tolerance_x=0.02;
s0313045 0:502b364c9f1d 90 float tolerance_y=0.02;
s0313045 0:502b364c9f1d 91 float tolerance_w=0.01;
s0313045 0:502b364c9f1d 92
s0313045 0:502b364c9f1d 93 float speed_max_x=1;
s0313045 0:502b364c9f1d 94 float speed_max_y=1;
s0313045 1:58d1caed28d4 95 float speed_max_w=1;
s0313045 0:502b364c9f1d 96
s0313045 0:502b364c9f1d 97 long odom_last_read= millis();
s0313045 0:502b364c9f1d 98
s0313045 0:502b364c9f1d 99 /////////////////////////////////////
s0313045 0:502b364c9f1d 100 const float RATE = 0.18;
s0313045 2:611a5eb132a1 101 //////////////////////////
s0313045 0:502b364c9f1d 102
s0313045 0:502b364c9f1d 103 ///////////////////////////
s0313045 2:611a5eb132a1 104 float encoder_2_global_angle = -90; //encoder coordinate system + 30 degree => global coordinate system
s0313045 1:58d1caed28d4 105 //float encoder_2_global_x = 0.34; //encoder to center distance in x (tung)
s0313045 1:58d1caed28d4 106 //float encoder_2_global_y = -0.235; //encoder to center distance in y (tung)
s0313045 1:58d1caed28d4 107
s0313045 1:58d1caed28d4 108
s0313045 2:611a5eb132a1 109 float encoder_2_global_x = 0.33;//0.125; //encoder to center distance in x (tung)
s0313045 2:611a5eb132a1 110 float encoder_2_global_y = 0.24;//0.37; //encoder to center distance in y (tung)
s0313045 0:502b364c9f1d 111 ////////////////////TUNG////////////////
s0313045 0:502b364c9f1d 112
s0313045 0:502b364c9f1d 113
s0313045 0:502b364c9f1d 114 float transformed_real_now_x=0;
s0313045 0:502b364c9f1d 115 float transformed_real_now_y=0;
s0313045 0:502b364c9f1d 116 float transformed_real_now_w=0;
s0313045 0:502b364c9f1d 117
s0313045 1:58d1caed28d4 118
s0313045 1:58d1caed28d4 119 float startup_offset_x_encoder =0;
s0313045 1:58d1caed28d4 120 float startup_offset_y_encoder =0;
s0313045 1:58d1caed28d4 121 float startup_offset_w_encoder=0;
s0313045 1:58d1caed28d4 122
s0313045 1:58d1caed28d4 123
s0313045 1:58d1caed28d4 124
s0313045 1:58d1caed28d4 125 float encoder_to_center = sqrt( ( encoder_2_global_x * encoder_2_global_x ) + ( encoder_2_global_y * encoder_2_global_y ) );
s0313045 1:58d1caed28d4 126
s0313045 1:58d1caed28d4 127 //#########################//
s0313045 2:611a5eb132a1 128 float encoder2local_angle = -90 *pi/float(180);
s0313045 2:611a5eb132a1 129 float encoder_position_angle =( ( 360-36.02737) ) / float(180) * pi ; //90 + angle to encoder location
s0313045 1:58d1caed28d4 130 float r = sqrt( ( encoder_2_global_x * encoder_2_global_x ) + ( encoder_2_global_y * encoder_2_global_y ) ); //encoder to center radius
s0313045 1:58d1caed28d4 131
s0313045 1:58d1caed28d4 132
s0313045 0:502b364c9f1d 133 void calculatePos(float _X,float _Y,float _A)
s0313045 0:502b364c9f1d 134 {
s0313045 1:58d1caed28d4 135 float zangle = _A- 360 * int(_A / 360);
s0313045 1:58d1caed28d4 136 float zrangle = zangle *pi/float(180); //degree 2 rad
s0313045 1:58d1caed28d4 137
s0313045 1:58d1caed28d4 138 float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) ) - ( ( _Y / float(1000) ) * sin( -encoder2local_angle) );
s0313045 1:58d1caed28d4 139 float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) ) + ( ( _Y / float(1000) ) * cos( -encoder2local_angle) );
s0313045 1:58d1caed28d4 140
s0313045 1:58d1caed28d4 141 float s = copysign( sqrt( 2*( r*r ) - 2*(r*r)*cos(zrangle) ) , zrangle );
s0313045 1:58d1caed28d4 142
s0313045 1:58d1caed28d4 143 float x_bias = s * cos( zrangle / 2 );
s0313045 1:58d1caed28d4 144 float y_bias = s * sin( zrangle / 2 );
s0313045 1:58d1caed28d4 145
s0313045 1:58d1caed28d4 146 float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) ) - ( y_bias ) * ( sin( encoder_position_angle ) ) ;
s0313045 1:58d1caed28d4 147 float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) ) + ( y_bias ) * ( cos( encoder_position_angle ) ) ;
s0313045 1:58d1caed28d4 148
s0313045 1:58d1caed28d4 149
s0313045 2:611a5eb132a1 150 transformed_real_now_x = tx + y_tbias - startup_offset_x_encoder; //-
s0313045 2:611a5eb132a1 151 transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
s0313045 1:58d1caed28d4 152
s0313045 2:611a5eb132a1 153 //transformed_real_now_x = tx + y_tbias - startup_offset_x_encoder; //+
s0313045 2:611a5eb132a1 154 //transformed_real_now_y = ty - x_tbias - startup_offset_y_encoder;
s0313045 1:58d1caed28d4 155
s0313045 1:58d1caed28d4 156
s0313045 1:58d1caed28d4 157 transformed_real_now_w= _A *pi/float(180) - startup_offset_w_encoder;
s0313045 1:58d1caed28d4 158
s0313045 1:58d1caed28d4 159
s0313045 0:502b364c9f1d 160 }
s0313045 0:502b364c9f1d 161
s0313045 0:502b364c9f1d 162
s0313045 0:502b364c9f1d 163
s0313045 0:502b364c9f1d 164
s0313045 0:502b364c9f1d 165
s0313045 1:58d1caed28d4 166 ///////////////////////
s0313045 0:502b364c9f1d 167
s0313045 0:502b364c9f1d 168
s0313045 0:502b364c9f1d 169
s0313045 0:502b364c9f1d 170 float x_PID_P = 0.5;
s0313045 0:502b364c9f1d 171 float y_PID_P = 0.5;
s0313045 1:58d1caed28d4 172 float w_PID_P = 1;
s0313045 0:502b364c9f1d 173
s0313045 0:502b364c9f1d 174 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
s0313045 0:502b364c9f1d 175
s0313045 0:502b364c9f1d 176 //////////////////////////////
s0313045 0:502b364c9f1d 177 void start_calculatePos(float _X,float _Y,float _A)
s0313045 0:502b364c9f1d 178 {
s0313045 1:58d1caed28d4 179 float zangle = _A- 360 * int(_A / 360);
s0313045 1:58d1caed28d4 180 float zrangle = zangle *pi/float(180); //degree 2 rad
s0313045 1:58d1caed28d4 181
s0313045 1:58d1caed28d4 182 float tx = ( ( _X / float(1000) ) * cos( -encoder2local_angle) ) - ( ( _Y / float(1000) ) * sin( -encoder2local_angle) );
s0313045 1:58d1caed28d4 183 float ty = ( ( _X / float(1000) ) * sin( -encoder2local_angle) ) + ( ( _Y / float(1000) ) * cos( -encoder2local_angle) );
s0313045 1:58d1caed28d4 184
s0313045 1:58d1caed28d4 185 float s = copysign( sqrt( 2*( r*r ) - 2*(r*r)*cos(zrangle) ) , zrangle );
s0313045 1:58d1caed28d4 186
s0313045 1:58d1caed28d4 187 float x_bias = s * cos( zrangle / 2 );
s0313045 1:58d1caed28d4 188 float y_bias = s * sin( zrangle / 2 );
s0313045 1:58d1caed28d4 189
s0313045 1:58d1caed28d4 190 float x_tbias = ( x_bias ) * ( cos( encoder_position_angle) ) - ( y_bias ) * ( sin( encoder_position_angle ) ) ;
s0313045 1:58d1caed28d4 191 float y_tbias = ( x_bias ) * ( sin( encoder_position_angle) ) + ( y_bias ) * ( cos( encoder_position_angle ) ) ;
s0313045 1:58d1caed28d4 192
s0313045 1:58d1caed28d4 193
s0313045 2:611a5eb132a1 194 startup_offset_x_encoder = tx + y_tbias ; //-
s0313045 2:611a5eb132a1 195 startup_offset_y_encoder = ty - x_tbias ;
s0313045 1:58d1caed28d4 196
s0313045 2:611a5eb132a1 197 //startup_offset_x_encoder = tx + y_tbias ;
s0313045 2:611a5eb132a1 198 //startup_offset_y_encoder = ty - x_tbias ; //+
s0313045 1:58d1caed28d4 199
s0313045 1:58d1caed28d4 200
s0313045 1:58d1caed28d4 201 startup_offset_w_encoder = _A *pi/float(180); //degree 2 rad
s0313045 1:58d1caed28d4 202
s0313045 1:58d1caed28d4 203
s0313045 0:502b364c9f1d 204 }
s0313045 0:502b364c9f1d 205
s0313045 0:502b364c9f1d 206 void ActionEncoder_init()
s0313045 0:502b364c9f1d 207 {
s0313045 0:502b364c9f1d 208 count=0;
s0313045 0:502b364c9f1d 209 i=0;
s0313045 0:502b364c9f1d 210 done=0;
s0313045 0:502b364c9f1d 211 xangle=0;
s0313045 0:502b364c9f1d 212 yangle=0;
s0313045 0:502b364c9f1d 213 zangle=0;
s0313045 0:502b364c9f1d 214 d_angle=0;
s0313045 0:502b364c9f1d 215 pos_x=0;
s0313045 0:502b364c9f1d 216 pos_y=0;
s0313045 0:502b364c9f1d 217 angleSpeed=0;
s0313045 0:502b364c9f1d 218 temp_zangle=0;
s0313045 0:502b364c9f1d 219 LastRead=0;
s0313045 0:502b364c9f1d 220 newDataArrived=false;
s0313045 0:502b364c9f1d 221
s0313045 0:502b364c9f1d 222 }
s0313045 0:502b364c9f1d 223
s0313045 0:502b364c9f1d 224 bool readEncoder(char c)
s0313045 0:502b364c9f1d 225 {
s0313045 0:502b364c9f1d 226 //sprintf(buffer,"%02X",c);
s0313045 0:502b364c9f1d 227 //sprintf(buffer,"%X",c);
s0313045 0:502b364c9f1d 228 //pc.printf(buffer);
s0313045 0:502b364c9f1d 229 //pc.printf("\r\n");
s0313045 0:502b364c9f1d 230
s0313045 0:502b364c9f1d 231 //sprintf(buffer,"%d",count);
s0313045 0:502b364c9f1d 232 //pc.printf(buffer);
s0313045 0:502b364c9f1d 233 //pc.printf("\r\n");
s0313045 0:502b364c9f1d 234 switch(count) {
s0313045 0:502b364c9f1d 235 case 0:
s0313045 0:502b364c9f1d 236 if (c==0x0d) count++;
s0313045 0:502b364c9f1d 237 else count=0;
s0313045 0:502b364c9f1d 238 break;
s0313045 0:502b364c9f1d 239 case 1:
s0313045 0:502b364c9f1d 240 if(c==0x0a) {
s0313045 0:502b364c9f1d 241 i=0;
s0313045 0:502b364c9f1d 242 count++;
s0313045 0:502b364c9f1d 243 } else if(c==0x0d) {}
s0313045 0:502b364c9f1d 244 else count=0;
s0313045 0:502b364c9f1d 245 break;
s0313045 0:502b364c9f1d 246 case 2:
s0313045 0:502b364c9f1d 247 posture.data[i]=c;
s0313045 0:502b364c9f1d 248 i++;
s0313045 0:502b364c9f1d 249 if(i>=24) {
s0313045 0:502b364c9f1d 250 i=0;
s0313045 0:502b364c9f1d 251 count++;
s0313045 0:502b364c9f1d 252 }
s0313045 0:502b364c9f1d 253 break;
s0313045 0:502b364c9f1d 254 case 3:
s0313045 0:502b364c9f1d 255 if(c==0x0a)count++;
s0313045 0:502b364c9f1d 256 else count=0;
s0313045 0:502b364c9f1d 257 break;
s0313045 0:502b364c9f1d 258 case 4:
s0313045 0:502b364c9f1d 259 if(c==0x0d) {
s0313045 0:502b364c9f1d 260 d_angle=posture.val[0]-temp_zangle;
s0313045 0:502b364c9f1d 261 if (d_angle<-180) {
s0313045 0:502b364c9f1d 262 d_angle=d_angle+360;
s0313045 0:502b364c9f1d 263 } else if (d_angle>180) {
s0313045 0:502b364c9f1d 264 d_angle=d_angle-360;
s0313045 0:502b364c9f1d 265 }
s0313045 0:502b364c9f1d 266
s0313045 0:502b364c9f1d 267 now_w+=d_angle;
s0313045 0:502b364c9f1d 268 temp_zangle=posture.val[0];
s0313045 0:502b364c9f1d 269 //xangle=posture.val[1];
s0313045 0:502b364c9f1d 270 //yangle=posture.val[2];
s0313045 0:502b364c9f1d 271 now_x=posture.val[3];
s0313045 0:502b364c9f1d 272 now_y=posture.val[4];
s0313045 0:502b364c9f1d 273 //angleSpeed=posture.val[5];
s0313045 0:502b364c9f1d 274 newDataArrived=true;
s0313045 0:502b364c9f1d 275
s0313045 0:502b364c9f1d 276 }
s0313045 0:502b364c9f1d 277 count=0;
s0313045 0:502b364c9f1d 278 done=1;
s0313045 0:502b364c9f1d 279 LastRead=millis();
s0313045 0:502b364c9f1d 280 return true;
s0313045 0:502b364c9f1d 281 //break;
s0313045 0:502b364c9f1d 282 default:
s0313045 0:502b364c9f1d 283 count=0;
s0313045 0:502b364c9f1d 284 break;
s0313045 0:502b364c9f1d 285 }
s0313045 0:502b364c9f1d 286
s0313045 0:502b364c9f1d 287 return false;
s0313045 0:502b364c9f1d 288 }
s0313045 0:502b364c9f1d 289
s0313045 0:502b364c9f1d 290
s0313045 0:502b364c9f1d 291
s0313045 0:502b364c9f1d 292
s0313045 2:611a5eb132a1 293 //void inverse(float global_x_vel, float global_y_vel, float w_vel) const std_msgs::UInt16& cmd_msg
s0313045 2:611a5eb132a1 294 void inverse(const geometry_msgs::Twist& cmd_vel)
s0313045 0:502b364c9f1d 295 {
s0313045 2:611a5eb132a1 296 float global_vel_x = cmd_vel.linear.x;
s0313045 2:611a5eb132a1 297 float global_vel_y = cmd_vel.linear.y;
s0313045 2:611a5eb132a1 298 float w_vel = cmd_vel.angular.z;
s0313045 2:611a5eb132a1 299
s0313045 2:611a5eb132a1 300 float local_x_vel = global_vel_x * cos( -transformed_real_now_w ) - global_vel_y * sin( -transformed_real_now_w );
s0313045 2:611a5eb132a1 301 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)
s0313045 2:611a5eb132a1 302
s0313045 2:611a5eb132a1 303 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 );
s0313045 2:611a5eb132a1 304 motor2 = int( ( (-1) * local_y_vel + d * w_vel ) * 60 / (wheelR * 2 * pi) * gear );
s0313045 2:611a5eb132a1 305 motor3 = int( ( sin(pi_div_3) * local_x_vel + cos(pi_div_3) * local_y_vel + d * w_vel ) * 60 / (wheelR * 2 * pi) * gear );
s0313045 2:611a5eb132a1 306
s0313045 0:502b364c9f1d 307 }
s0313045 0:502b364c9f1d 308
s0313045 2:611a5eb132a1 309 /////////////////////////
s0313045 2:611a5eb132a1 310 ros::NodeHandle nh;
s0313045 2:611a5eb132a1 311 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &inverse );
s0313045 0:502b364c9f1d 312
s0313045 2:611a5eb132a1 313 nav_msgs::Odometry odom;
s0313045 2:611a5eb132a1 314 geometry_msgs::TransformStamped odom_trans;
s0313045 2:611a5eb132a1 315
s0313045 2:611a5eb132a1 316 ros::Publisher odom_publisher("odom", &odom);
s0313045 2:611a5eb132a1 317 tf::TransformBroadcaster odom_broadcaster;
s0313045 0:502b364c9f1d 318
s0313045 0:502b364c9f1d 319
s0313045 2:611a5eb132a1 320 /////////////////////////
s0313045 0:502b364c9f1d 321
s0313045 0:502b364c9f1d 322
s0313045 0:502b364c9f1d 323
s0313045 0:502b364c9f1d 324 void motor_update()
s0313045 0:502b364c9f1d 325 {
s0313045 0:502b364c9f1d 326 action1.SetVelocity_mod(motor1 * -1 );
s0313045 0:502b364c9f1d 327 action2.SetVelocity_mod(motor2 * -1 );
s0313045 0:502b364c9f1d 328 action3.SetVelocity_mod(motor3 * -1 );
s0313045 0:502b364c9f1d 329 wait(0.005);
s0313045 0:502b364c9f1d 330 }
s0313045 0:502b364c9f1d 331
s0313045 2:611a5eb132a1 332 void ros_update()
s0313045 2:611a5eb132a1 333 {
s0313045 2:611a5eb132a1 334
s0313045 2:611a5eb132a1 335 odom_publisher.publish( &odom );
s0313045 2:611a5eb132a1 336 odom_broadcaster.sendTransform(odom_trans);
s0313045 2:611a5eb132a1 337
s0313045 2:611a5eb132a1 338
s0313045 2:611a5eb132a1 339 wait(0.005);
s0313045 2:611a5eb132a1 340 nh.spinOnce();
s0313045 2:611a5eb132a1 341 }
s0313045 2:611a5eb132a1 342
s0313045 2:611a5eb132a1 343
s0313045 0:502b364c9f1d 344 void odom_update()
s0313045 0:502b364c9f1d 345 {
s0313045 0:502b364c9f1d 346 calculatePos(now_x,now_y,now_w);
s0313045 0:502b364c9f1d 347
s0313045 2:611a5eb132a1 348 //geometry_msgs::Quaternion odom_quat = createQuaternionMsg(transformed_real_now_w );
s0313045 2:611a5eb132a1 349 geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(transformed_real_now_w); // *57.2957795
s0313045 0:502b364c9f1d 350
s0313045 0:502b364c9f1d 351
s0313045 2:611a5eb132a1 352 //first, we'll publish the transform over tf
s0313045 2:611a5eb132a1 353 odom_trans.header.stamp = nh.now();
s0313045 2:611a5eb132a1 354 odom_trans.header.frame_id = "odom";
s0313045 2:611a5eb132a1 355 odom_trans.child_frame_id = "base_link";
s0313045 0:502b364c9f1d 356
s0313045 2:611a5eb132a1 357 odom_trans.transform.translation.x = transformed_real_now_x ;
s0313045 2:611a5eb132a1 358 odom_trans.transform.translation.y = transformed_real_now_y ;
s0313045 2:611a5eb132a1 359 odom_trans.transform.translation.z = 0.0;
s0313045 2:611a5eb132a1 360 odom_trans.transform.rotation = odom_quat;
s0313045 2:611a5eb132a1 361
s0313045 2:611a5eb132a1 362 //send the transform
s0313045 0:502b364c9f1d 363
s0313045 0:502b364c9f1d 364
s0313045 0:502b364c9f1d 365
s0313045 0:502b364c9f1d 366
s0313045 2:611a5eb132a1 367 //next, we'll publish the odometry message over ROS
s0313045 2:611a5eb132a1 368 //nav_msgs::Odometry odom;
s0313045 2:611a5eb132a1 369 odom.header.stamp = nh.now();
s0313045 2:611a5eb132a1 370 odom.header.frame_id = "odom";
s0313045 2:611a5eb132a1 371 odom.child_frame_id = "base_link";
s0313045 0:502b364c9f1d 372
s0313045 2:611a5eb132a1 373 //set the position
s0313045 2:611a5eb132a1 374 odom.pose.pose.position.x = transformed_real_now_x;
s0313045 2:611a5eb132a1 375 odom.pose.pose.position.y = transformed_real_now_y ;
s0313045 2:611a5eb132a1 376 odom.pose.pose.position.z = 0.0;
s0313045 2:611a5eb132a1 377 odom.pose.pose.orientation = odom_quat;
s0313045 2:611a5eb132a1 378
s0313045 2:611a5eb132a1 379 //set the velocity
s0313045 2:611a5eb132a1 380 //odom.child_frame_id = "base_link";
s0313045 2:611a5eb132a1 381 //odom.twist.twist.linear.x = vx;
s0313045 2:611a5eb132a1 382 //odom.twist.twist.linear.y = vy;
s0313045 2:611a5eb132a1 383 //odom.twist.twist.angular.z = vth;
s0313045 2:611a5eb132a1 384
s0313045 2:611a5eb132a1 385 //publish the message
s0313045 2:611a5eb132a1 386 //
s0313045 2:611a5eb132a1 387
s0313045 2:611a5eb132a1 388
s0313045 2:611a5eb132a1 389 }
s0313045 2:611a5eb132a1 390
s0313045 2:611a5eb132a1 391
s0313045 2:611a5eb132a1 392
s0313045 2:611a5eb132a1 393 int main() {
s0313045 2:611a5eb132a1 394
s0313045 0:502b364c9f1d 395 millisStart();
s0313045 0:502b364c9f1d 396
s0313045 0:502b364c9f1d 397
s0313045 0:502b364c9f1d 398
s0313045 0:502b364c9f1d 399 Action.baud(115200);
s0313045 0:502b364c9f1d 400 Action.format(8,SerialBase::None,1);
s0313045 0:502b364c9f1d 401 ActionEncoder_init();
s0313045 2:611a5eb132a1 402
s0313045 2:611a5eb132a1 403 nh.initNode();
s0313045 2:611a5eb132a1 404 odom_broadcaster.init(nh);
s0313045 2:611a5eb132a1 405 nh.advertise(odom_publisher);
s0313045 2:611a5eb132a1 406 nh.subscribe(sub);
s0313045 2:611a5eb132a1 407
s0313045 2:611a5eb132a1 408 while (!nh.connected() ) {nh.spinOnce();}
s0313045 2:611a5eb132a1 409 odom_trans.header.stamp = nh.now();
s0313045 2:611a5eb132a1 410 odom_trans.header.frame_id = "odom";
s0313045 2:611a5eb132a1 411 odom_trans.child_frame_id = "base_link";
s0313045 2:611a5eb132a1 412
s0313045 2:611a5eb132a1 413 odom_trans.transform.translation.x = 0.0;
s0313045 2:611a5eb132a1 414 odom_trans.transform.translation.y = 0.0 ;
s0313045 2:611a5eb132a1 415 odom_trans.transform.translation.z = 0.0;
s0313045 2:611a5eb132a1 416 odom_trans.transform.rotation = tf::createQuaternionFromYaw(0);
s0313045 2:611a5eb132a1 417
s0313045 2:611a5eb132a1 418 odom.header.stamp = nh.now();
s0313045 2:611a5eb132a1 419 odom.header.frame_id = "odom";
s0313045 2:611a5eb132a1 420 odom.child_frame_id = "base_link";
s0313045 2:611a5eb132a1 421
s0313045 2:611a5eb132a1 422 //set the position
s0313045 2:611a5eb132a1 423 odom.pose.pose.position.x = transformed_real_now_x;
s0313045 2:611a5eb132a1 424 odom.pose.pose.position.y = transformed_real_now_y ;
s0313045 2:611a5eb132a1 425 odom.pose.pose.position.z = 0.0;
s0313045 2:611a5eb132a1 426 odom.pose.pose.orientation = tf::createQuaternionFromYaw(0);;
s0313045 2:611a5eb132a1 427
s0313045 2:611a5eb132a1 428 ros_updater.attach(&ros_update, 0.3);
s0313045 2:611a5eb132a1 429
s0313045 0:502b364c9f1d 430 while(1)
s0313045 0:502b364c9f1d 431 {
s0313045 0:502b364c9f1d 432 if (Action.readable())
s0313045 0:502b364c9f1d 433 {
s0313045 0:502b364c9f1d 434 char c = Action.getc();
s0313045 0:502b364c9f1d 435 if (readEncoder(c))
s0313045 0:502b364c9f1d 436 {
s0313045 0:502b364c9f1d 437 //startup_offset_x_encoder = now_x/1000;
s0313045 0:502b364c9f1d 438 //startup_offset_y_encoder = now_y/1000;
s0313045 0:502b364c9f1d 439 //startup_offset_w_encoder = now_w/float(180)*pi;
s0313045 0:502b364c9f1d 440
s0313045 1:58d1caed28d4 441 start_calculatePos( (now_x),(now_y), now_w ); //global
s0313045 0:502b364c9f1d 442 break;
s0313045 0:502b364c9f1d 443
s0313045 0:502b364c9f1d 444 }
s0313045 0:502b364c9f1d 445
s0313045 0:502b364c9f1d 446 }
s0313045 0:502b364c9f1d 447 } //start first to take offset from encoder... in case already moved
s0313045 0:502b364c9f1d 448
s0313045 0:502b364c9f1d 449
s0313045 0:502b364c9f1d 450
s0313045 0:502b364c9f1d 451 for( int a = 1; a < 2; a++ ){
s0313045 0:502b364c9f1d 452 action1.Enable();
s0313045 0:502b364c9f1d 453 action2.Enable();
s0313045 0:502b364c9f1d 454 action3.Enable();
s0313045 0:502b364c9f1d 455 wait(0.1);
s0313045 0:502b364c9f1d 456 action1.SetOperationalMode();
s0313045 0:502b364c9f1d 457 action2.SetOperationalMode();
s0313045 0:502b364c9f1d 458 action3.SetOperationalMode();
s0313045 0:502b364c9f1d 459 wait(0.1);
s0313045 0:502b364c9f1d 460 action1.Configvelocity(100000, 100000);
s0313045 0:502b364c9f1d 461 action2.Configvelocity(100000, 100000);
s0313045 0:502b364c9f1d 462 action3.Configvelocity(100000, 100000);
s0313045 0:502b364c9f1d 463 wait(0.1);
s0313045 0:502b364c9f1d 464 }
s0313045 0:502b364c9f1d 465
s0313045 0:502b364c9f1d 466 motor_updater.attach(&motor_update, RATE);
s0313045 0:502b364c9f1d 467 //odom_updater.attach(&odom_update, RATE);
s0313045 2:611a5eb132a1 468 ros_updater.attach(&ros_update, 0.5);
s0313045 2:611a5eb132a1 469
s0313045 2:611a5eb132a1 470
s0313045 0:502b364c9f1d 471
s0313045 0:502b364c9f1d 472
s0313045 0:502b364c9f1d 473 while(1)
s0313045 0:502b364c9f1d 474 {
s0313045 0:502b364c9f1d 475 if (Action.readable())
s0313045 0:502b364c9f1d 476 {
s0313045 1:58d1caed28d4 477 //pc.putc('a');
s0313045 0:502b364c9f1d 478 char c = Action.getc();
s0313045 0:502b364c9f1d 479 if(readEncoder(c)) odom_update();
s0313045 0:502b364c9f1d 480 }
s0313045 0:502b364c9f1d 481
s0313045 0:502b364c9f1d 482 }
s0313045 0:502b364c9f1d 483
s0313045 0:502b364c9f1d 484
s0313045 0:502b364c9f1d 485 }