Sam Shum / ros_mbed_base_controller

Sam

main_front_is_two_wheel.txt@2:611a5eb132a1, 2020-09-13 (annotated)

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

Who changed what in which revision?

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