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