abe takumi / Mbed OS 2022NHK_A_main

NHK2022Aチームの足回りと機構のセット メインプログラム

Dependencies:   FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel

main.cpp@11:10d3dc96c469, 19 months ago (annotated)

Committer:
me33004m
Date:
Tue Oct 11 08:29:14 2022 +0000
Revision:
11:10d3dc96c469
Parent:
10:886268a42090 
teraterm change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
me33004m 0:5d4705b2893c 1 #include "mbed.h"
me33004m 0:5d4705b2893c 2 #include "ikarashiMDC.h"
me33004m 0:5d4705b2893c 3 #include "pinconfig.h"
me33004m 0:5d4705b2893c 4 #include "omni_wheel.h"
me33004m 7:778eaeae8128 5 #include "Servo.h"
me33004m 0:5d4705b2893c 6 #include "math.h"
me33004m 7:778eaeae8128 7 #include "SerialMultiByte.h"
me33004m 7:778eaeae8128 8 #include "PID.h"
me33004m 7:778eaeae8128 9 #include "R1370.h"
me33004m 0:5d4705b2893c 10 #include "OmniPosition.h"
me33004m 7:778eaeae8128 11 #include "FEP_RX22.h"
me33004m 7:778eaeae8128 12 #include "cmath"
me33004m 1:5132f966db08 13
me33004m 0:5d4705b2893c 14 #define PI 3.141592653589
me33004m 3:4c0c8046c3a7 15 #define maxSpeed 0.4
me33004m 1:5132f966db08 16
me33004m 7:778eaeae8128 17 DigitalIn userButton(USER_BUTTON);
me33004m 7:778eaeae8128 18 AnalogIn VOLUME(A0);
me33004m 7:778eaeae8128 19
me33004m 7:778eaeae8128 20
me33004m 2:d84346eaa720 21 FEP_RX22 mycon(fepTX, fepRX, fepad);
me33004m 0:5d4705b2893c 22 Serial pc(pcTX, pcRX, 115200);
me33004m 7:778eaeae8128 23 Serial RS485(mdcTX,mdcRX,115200);
me33004m 1:5132f966db08 24
me33004m 1:5132f966db08 25
me33004m 7:778eaeae8128 26 DigitalOut stop(em_stop);
me33004m 7:778eaeae8128 27
me33004m 7:778eaeae8128 28 SerialMultiByte rcv(sub2TX,sub2RX);
me33004m 7:778eaeae8128 29
me33004m 7:778eaeae8128 30 ikarashiMDC motor[] = {
me33004m 7:778eaeae8128 31 ikarashiMDC(0,0,SM,&RS485), //asi
me33004m 7:778eaeae8128 32 ikarashiMDC(0,1,SM,&RS485), //asi
me33004m 7:778eaeae8128 33 ikarashiMDC(0,2,SM,&RS485), //asi
me33004m 7:778eaeae8128 34 ikarashiMDC(0,3,SM,&RS485), //asi
me33004m 7:778eaeae8128 35 ikarashiMDC(1,0,SM,&RS485), //全体昇降1
me33004m 7:778eaeae8128 36 ikarashiMDC(1,1,SM,&RS485), //全体昇降2
me33004m 7:778eaeae8128 37 ikarashiMDC(1,2,SM,&RS485), //フジモン機構
me33004m 7:778eaeae8128 38 ikarashiMDC(1,3,SM,&RS485), //フジモン機構
me33004m 7:778eaeae8128 39 ikarashiMDC(2,0,SM,&RS485), //井上左昇降
me33004m 7:778eaeae8128 40 ikarashiMDC(2,1,SM,&RS485), //井上右昇降
me33004m 7:778eaeae8128 41 ikarashiMDC(2,2,SM,&RS485), //井上左前後
me33004m 7:778eaeae8128 42 ikarashiMDC(2,3,SM,&RS485), //井上右前後
me33004m 7:778eaeae8128 43 };
me33004m 7:778eaeae8128 44
me33004m 7:778eaeae8128 45 Servo pwm_imput1(BLDC1);//ブラシレス宣言
me33004m 7:778eaeae8128 46 Servo pwm_imput2(BLDC2);
me33004m 7:778eaeae8128 47 Servo pwm_imput3(BLDC3);
me33004m 8:f40325f2d182 48 // angとanglとangleの変数とクラス名がある。気をつけよう*********************重要重要***********************
me33004m 3:4c0c8046c3a7 49 uint8_t b[16];
me33004m 0:5d4705b2893c 50 int16_t stick[4];
me33004m 2:d84346eaa720 51 int16_t trigger[4];
me33004m 6:d4b82ba4836a 52 int16_t volume[3];
me33004m 6:d4b82ba4836a 53 uint8_t toggle[4];
me33004m 6:d4b82ba4836a 54 uint8_t timeout;
me33004m 6:d4b82ba4836a 55 uint8_t data[128];
me33004m 6:d4b82ba4836a 56 int pw;
me33004m 7:778eaeae8128 57
me33004m 7:778eaeae8128 58 double speed[12];
me33004m 0:5d4705b2893c 59 double engine[4];
me33004m 0:5d4705b2893c 60 double spin_power;
me33004m 0:5d4705b2893c 61 double posiX , posiY , posiTheta;
me33004m 7:778eaeae8128 62
me33004m 6:d4b82ba4836a 63 int TargetAngle = 0;
me33004m 7:778eaeae8128 64
me33004m 10:886268a42090 65 int sum_1 = 0;
me33004m 10:886268a42090 66 int sum_2 = 0;
me33004m 10:886268a42090 67
me33004m 7:778eaeae8128 68 double bldcspeed = 0.8;
me33004m 7:778eaeae8128 69
me33004m 7:778eaeae8128 70 int16_t angle[4] = {0};//エンコーダ受信格納
me33004m 7:778eaeae8128 71 uint8_t pulse[8] = {0};
me33004m 1:5132f966db08 72
me33004m 0:5d4705b2893c 73 OmniWheel omni(4);
me33004m 7:778eaeae8128 74 OmniPosition posi(sub1TX, sub1RX);
me33004m 1:5132f966db08 75
me33004m 7:778eaeae8128 76 PID angl(10.0, 5.0, 0.0000005, 0.01);
me33004m 1:5132f966db08 77
me33004m 7:778eaeae8128 78 //プロトタイプ宣言
me33004m 10:886268a42090 79 void chassis(); //機体自体の制御
me33004m 3:4c0c8046c3a7 80 void spin(double ang); //PID
me33004m 3:4c0c8046c3a7 81 int pm(double num); //正負判定
me33004m 1:5132f966db08 82
me33004m 7:778eaeae8128 83 Timer t;
me33004m 7:778eaeae8128 84
me33004m 0:5d4705b2893c 85 int main(void){
me33004m 7:778eaeae8128 86 t.start();
me33004m 7:778eaeae8128 87
me33004m 7:778eaeae8128 88 rcv.setHeaders(0xff,0xff);
me33004m 7:778eaeae8128 89 rcv.startReceive(4); //SerialMultiByte受信
me33004m 7:778eaeae8128 90
me33004m 7:778eaeae8128 91 mycon.StartReceive(); //コントローラー受信・宣言
me33004m 1:5132f966db08 92
me33004m 0:5d4705b2893c 93 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
me33004m 0:5d4705b2893c 94 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
me33004m 0:5d4705b2893c 95 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
me33004m 0:5d4705b2893c 96 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
me33004m 0:5d4705b2893c 97
me33004m 7:778eaeae8128 98 angl.setInputLimits(-180,180);
me33004m 7:778eaeae8128 99 angl.setOutputLimits(-0.4,0.4);
me33004m 7:778eaeae8128 100 angl.setBias(0);
me33004m 7:778eaeae8128 101 angl.setMode(1);
me33004m 7:778eaeae8128 102 angl.setSetPoint(0);
me33004m 0:5d4705b2893c 103 while(1){
me33004m 6:d4b82ba4836a 104 stop = toggle[0];
me33004m 0:5d4705b2893c 105 chassis();
me33004m 0:5d4705b2893c 106 }
me33004m 0:5d4705b2893c 107
me33004m 0:5d4705b2893c 108 }
me33004m 1:5132f966db08 109
me33004m 0:5d4705b2893c 110 void chassis(){
me33004m 3:4c0c8046c3a7 111
me33004m 7:778eaeae8128 112 #if ControllerMode
me33004m 7:778eaeae8128 113 for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
me33004m 7:778eaeae8128 114 for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
me33004m 7:778eaeae8128 115 for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
me33004m 7:778eaeae8128 116
me33004m 7:778eaeae8128 117 // for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
me33004m 7:778eaeae8128 118 // pc.printf(" | ");
me33004m 7:778eaeae8128 119 // for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
me33004m 7:778eaeae8128 120 // pc.printf(" | ");
me33004m 7:778eaeae8128 121 // for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
me33004m 7:778eaeae8128 122 // pc.printf(" | ");
me33004m 7:778eaeae8128 123 #else
me33004m 7:778eaeae8128 124
me33004m 0:5d4705b2893c 125 /*ジャイロ読み取り*/
me33004m 8:f40325f2d182 126 /*足回りのXY座標は多分使わないので無くてもよし。*/
me33004m 0:5d4705b2893c 127 posiX = posi.getX();
me33004m 0:5d4705b2893c 128 posiY = posi.getY();
me33004m 0:5d4705b2893c 129 posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
me33004m 0:5d4705b2893c 130 pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
me33004m 7:778eaeae8128 131
me33004m 6:d4b82ba4836a 132 mycon.getData(data);
me33004m 10:886268a42090 133 /* for (int i=0, tmp=1; i<8; i++) {
me33004m 6:d4b82ba4836a 134 pw = pow((float)2,i);
me33004m 6:d4b82ba4836a 135 b[i] = (int)((data[0] & tmp)/pw);
me33004m 6:d4b82ba4836a 136 pc.printf("%d ", b[i]);
me33004m 6:d4b82ba4836a 137 tmp *= 2;
me33004m 6:d4b82ba4836a 138 }
me33004m 6:d4b82ba4836a 139 for (int i=8, tmp=1, j=0; i<16; i++, j++) {
me33004m 6:d4b82ba4836a 140 pw = pow((float)2,j);
me33004m 6:d4b82ba4836a 141 b[i] = (int)((data[1] & tmp)/pw);
me33004m 6:d4b82ba4836a 142 pc.printf("%d ", b[i]);
me33004m 6:d4b82ba4836a 143 tmp *= 2;
me33004m 6:d4b82ba4836a 144 }
me33004m 10:886268a42090 145 */
me33004m 10:886268a42090 146 for (int i=0, tmp=1; i<8; i++) {
me33004m 10:886268a42090 147 pw = pow((float)2,i);
me33004m 10:886268a42090 148 b[i] = (int)((data[0] & tmp)/pw);
me33004m 10:886268a42090 149 // pc.printf("%d ", b[i]); 上のポインタの式が分からんので無理やり10進数に変える
me33004m 10:886268a42090 150 sum_1 += b[i]*pow((float)2,i+1);
me33004m 10:886268a42090 151 tmp *= 2;
me33004m 10:886268a42090 152 }
me33004m 10:886268a42090 153 pc.printf("%3d ",sum_1);
me33004m 10:886268a42090 154 /*初期化*/
me33004m 10:886268a42090 155 sum_1 = 0;
me33004m 10:886268a42090 156
me33004m 10:886268a42090 157 for (int i=8, tmp=1, j=0; i<16; i++, j++) {
me33004m 10:886268a42090 158 pw = pow((float)2,j);
me33004m 10:886268a42090 159 b[i] = (int)((data[1] & tmp)/pw);
me33004m 10:886268a42090 160 // pc.printf("%d ", b[i]);
me33004m 10:886268a42090 161 sum_2 += b[i]*pow((float)2,i-7);
me33004m 10:886268a42090 162 tmp *= 2;
me33004m 10:886268a42090 163 }
me33004m 10:886268a42090 164 pc.printf("%3d ",sum_2);
me33004m 10:886268a42090 165 /*初期化*/
me33004m 10:886268a42090 166 sum_2 = 0;
me33004m 6:d4b82ba4836a 167 pc.printf(" | ");
me33004m 6:d4b82ba4836a 168
me33004m 6:d4b82ba4836a 169 for (int i=0; i<4; i++) {
me33004m 7:778eaeae8128 170 stick[i] = (data[i+2] - 128)*(-1);
me33004m 6:d4b82ba4836a 171 pc.printf("%3d ", stick[i]);
me33004m 2:d84346eaa720 172 }
me33004m 6:d4b82ba4836a 173 pc.printf(" | ");
me33004m 6:d4b82ba4836a 174
me33004m 6:d4b82ba4836a 175 for (int i=0; i<2; i++) {
me33004m 6:d4b82ba4836a 176 trigger[i] = data[i+6];
me33004m 6:d4b82ba4836a 177 pc.printf("%3d ", trigger[i]);
me33004m 6:d4b82ba4836a 178 }
me33004m 0:5d4705b2893c 179 pc.printf(" | ");
me33004m 6:d4b82ba4836a 180
me33004m 6:d4b82ba4836a 181 for (int i=0; i<3; i++) {
me33004m 6:d4b82ba4836a 182 volume[i] = data[i+9];
me33004m 6:d4b82ba4836a 183 pc.printf("%3d ", volume[i]);
me33004m 6:d4b82ba4836a 184 }
me33004m 0:5d4705b2893c 185 pc.printf(" | ");
me33004m 6:d4b82ba4836a 186
me33004m 6:d4b82ba4836a 187 for (int i=0; i<4; i++) {
me33004m 6:d4b82ba4836a 188 toggle[i] = data[i+12];
me33004m 7:778eaeae8128 189 pc.printf("%3d ", toggle[i]);
me33004m 6:d4b82ba4836a 190 }
me33004m 6:d4b82ba4836a 191 pc.printf(" | ");
me33004m 6:d4b82ba4836a 192
me33004m 6:d4b82ba4836a 193 timeout = data[8];
me33004m 6:d4b82ba4836a 194 pc.printf("%3d ", timeout);
me33004m 6:d4b82ba4836a 195 pc.printf(" | ");
me33004m 6:d4b82ba4836a 196
me33004m 7:778eaeae8128 197
me33004m 7:778eaeae8128 198 #endif
me33004m 7:778eaeae8128 199 if (mycon.getStatus()) pc.printf("receive");
me33004m 7:778eaeae8128 200 else pc.printf("anything error...");
me33004m 7:778eaeae8128 201
me33004m 7:778eaeae8128 202 rcv.getData(pulse); //エンコーダ受信
me33004m 7:778eaeae8128 203 for(int i=0,j=0;i<4;i++,j+=2){
me33004m 7:778eaeae8128 204 angle[i] = pulse[j] << 8 | pulse[j+1];
me33004m 7:778eaeae8128 205 }
me33004m 10:886268a42090 206 pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
me33004m 10:886268a42090 207 if(abs(stick[2])<10){
me33004m 10:886268a42090 208 pc.printf("TA:%.2f pid:%.2f T-p:%.2f",TargetAngle,-angl.compute(),TargetAngle-posiTheta);
me33004m 10:886268a42090 209 }
me33004m 7:778eaeae8128 210 pc.printf("\r\n");
me33004m 7:778eaeae8128 211
me33004m 7:778eaeae8128 212 //ブラシレスモーター
me33004m 7:778eaeae8128 213 pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
me33004m 7:778eaeae8128 214
me33004m 7:778eaeae8128 215 pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
me33004m 7:778eaeae8128 216
me33004m 7:778eaeae8128 217 pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
me33004m 7:778eaeae8128 218 // pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
me33004m 7:778eaeae8128 219 // ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
me33004m 7:778eaeae8128 220
me33004m 7:778eaeae8128 221 /*井上機構ON,OFF*/
me33004m 7:778eaeae8128 222 if(toggle[1]){
me33004m 7:778eaeae8128 223 speed[10] = -0.9;
me33004m 7:778eaeae8128 224 }else{
me33004m 7:778eaeae8128 225 speed[10] = 0;
me33004m 7:778eaeae8128 226 }
me33004m 7:778eaeae8128 227 if(toggle[1] && b[15]){
me33004m 7:778eaeae8128 228 speed[10] = 0.4;
me33004m 7:778eaeae8128 229 }
me33004m 7:778eaeae8128 230
me33004m 7:778eaeae8128 231 if(toggle[3]){
me33004m 7:778eaeae8128 232 speed[11] = -0.9;
me33004m 7:778eaeae8128 233 }else{
me33004m 7:778eaeae8128 234 speed[11] = 0;
me33004m 7:778eaeae8128 235 }
me33004m 7:778eaeae8128 236 if(toggle[3] && b[15]){
me33004m 7:778eaeae8128 237 speed[11] = 0.4;
me33004m 0:5d4705b2893c 238 }
me33004m 7:778eaeae8128 239
me33004m 7:778eaeae8128 240 /*フジモン機構*/
me33004m 7:778eaeae8128 241 if(toggle[2]){
me33004m 7:778eaeae8128 242 speed[6] = 0.6;
me33004m 7:778eaeae8128 243 speed[7] = 0.6;
me33004m 7:778eaeae8128 244 }else{
me33004m 7:778eaeae8128 245 speed[6] = 0;
me33004m 7:778eaeae8128 246 speed[7] = 0;
me33004m 7:778eaeae8128 247 }
me33004m 7:778eaeae8128 248
me33004m 7:778eaeae8128 249 /*展開昇降*/
me33004m 7:778eaeae8128 250 if(b[5] != 0){
me33004m 7:778eaeae8128 251 speed[4] = 0.5;
me33004m 7:778eaeae8128 252 speed[5] = 0.5;
me33004m 7:778eaeae8128 253 }else if(b[4] != 0){
me33004m 7:778eaeae8128 254 speed[4] = -0.35;
me33004m 7:778eaeae8128 255 speed[5] = -0.35;
me33004m 7:778eaeae8128 256 }else{
me33004m 7:778eaeae8128 257 speed[4] = 0;
me33004m 7:778eaeae8128 258 speed[5] = 0;
me33004m 0:5d4705b2893c 259 }
me33004m 7:778eaeae8128 260 //機構昇降
me33004m 7:778eaeae8128 261 if(b[9]){
me33004m 7:778eaeae8128 262 speed[8] = -0.35;
me33004m 7:778eaeae8128 263 }else
me33004m 7:778eaeae8128 264 if(b[13]){
me33004m 7:778eaeae8128 265 speed[8] = 0.4;
me33004m 7:778eaeae8128 266 }
me33004m 7:778eaeae8128 267 if(b[11]){
me33004m 7:778eaeae8128 268 speed[9] = -0.35;
me33004m 7:778eaeae8128 269 }else
me33004m 7:778eaeae8128 270 if(b[14]){
me33004m 7:778eaeae8128 271 speed[9] = 0.4;
me33004m 7:778eaeae8128 272 }
me33004m 7:778eaeae8128 273 if((b[9]!=1) && (b[13]!=1)){
me33004m 7:778eaeae8128 274 speed[8]=0;
me33004m 7:778eaeae8128 275 }
me33004m 7:778eaeae8128 276 if((b[11]!=1) && (b[14]!=1)){
me33004m 7:778eaeae8128 277 speed[9]=0;
me33004m 7:778eaeae8128 278 }
me33004m 7:778eaeae8128 279
me33004m 0:5d4705b2893c 280 /*PID*/
me33004m 7:778eaeae8128 281 if(abs(stick[2]) < 10){
me33004m 2:d84346eaa720 282 if(fabs(TargetAngle-posiTheta)>8){
me33004m 2:d84346eaa720 283 TargetAngle += 15*pm(posiTheta-TargetAngle);
me33004m 5:885bffdceaa2 284 if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
me33004m 2:d84346eaa720 285 TargetAngle += -360*pm(TargetAngle);
me33004m 2:d84346eaa720 286 }
me33004m 2:d84346eaa720 287 }
me33004m 6:d4b82ba4836a 288 spin(TargetAngle);
me33004m 0:5d4705b2893c 289 }
me33004m 6:d4b82ba4836a 290 /*全方位*/
me33004m 6:d4b82ba4836a 291 for(int i=0; i<4; i++){
me33004m 6:d4b82ba4836a 292 if(abs(stick[i]) > 10){
me33004m 7:778eaeae8128 293 engine[i] = maxSpeed*(stick[i]/128.0);
me33004m 7:778eaeae8128 294 }else if(b[0]){
me33004m 7:778eaeae8128 295 engine[1] = maxSpeed*(trigger[1]/225.0);
me33004m 7:778eaeae8128 296 engine[0] = 0;
me33004m 7:778eaeae8128 297 }else if(b[1]){
me33004m 7:778eaeae8128 298 engine[0] = -maxSpeed*(trigger[1]/225.0);
me33004m 6:d4b82ba4836a 299 engine[1] = 0;
me33004m 7:778eaeae8128 300 }else if(b[2]){
me33004m 7:778eaeae8128 301 engine[1] = -maxSpeed*(trigger[1]/225.0);
me33004m 6:d4b82ba4836a 302 engine[0] = 0;
me33004m 7:778eaeae8128 303 }else if(b[3]){
me33004m 7:778eaeae8128 304 engine[0] = maxSpeed*(trigger[1]/255.0);
me33004m 7:778eaeae8128 305 engine[1] = 0;
me33004m 6:d4b82ba4836a 306 }else{
me33004m 6:d4b82ba4836a 307 engine[i] = 0;
me33004m 7:778eaeae8128 308 }
me33004m 6:d4b82ba4836a 309 }
me33004m 1:5132f966db08 310 /*旋回*/
me33004m 1:5132f966db08 311 if(abs(stick[2]) > 10){
me33004m 1:5132f966db08 312 spin_power = engine[2];
me33004m 1:5132f966db08 313 }else{
me33004m 7:778eaeae8128 314 spin_power = angl.compute();
me33004m 1:5132f966db08 315 }
me33004m 1:5132f966db08 316
me33004m 1:5132f966db08 317 omni.computeXY(engine[0],engine[1],-spin_power);
me33004m 1:5132f966db08 318 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
me33004m 1:5132f966db08 319
me33004m 1:5132f966db08 320
me33004m 7:778eaeae8128 321 for(int i=0; i<12; i++) motor[i].setSpeed(speed[i]);
me33004m 1:5132f966db08 322
me33004m 0:5d4705b2893c 323 }
me33004m 0:5d4705b2893c 324 void spin(double ang)
me33004m 0:5d4705b2893c 325 {
me33004m 7:778eaeae8128 326 angl.setSetPoint(ang);
me33004m 2:d84346eaa720 327 posiTheta = posi.getTheta()*(180.0/M_PI);
me33004m 7:778eaeae8128 328 angl.setProcessValue(posiTheta);
me33004m 9:8dd49bc48d59 329 //for(int i=4; i<12; i++) motor[i].setSpeed(0);旋回時以外はPID判定なのでsetSpeed(0)だと機構が動かなくなるので多分このコードいらないです。
me33004m 10:886268a42090 330 //pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta);
me33004m 7:778eaeae8128 331
me33004m 2:d84346eaa720 332 }
me33004m 2:d84346eaa720 333 int pm(double num){
me33004m 2:d84346eaa720 334 return((num>0)-(num<0));
me33004m 7:778eaeae8128 335 }