NagaokaRoboticsClub_mbedTeam / Mbed OS 2019NHK_A_manual_blue

Program used by A team in the blue coat of qualifying at NHK in 2019.

Dependencies:   SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3

main.cpp@1:9d1ef7d7f874, 2019-10-03 (annotated)

Committer:
ec30109b
Date:
Thu Oct 03 08:42:26 2019 +0000
Revision:
1:9d1ef7d7f874
Parent:
0:d3db2b0afc76 
tukauyo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ec30109b 1:9d1ef7d7f874 1 #include "mbed.h" //Blue court
ec30109b 0:d3db2b0afc76 2 #include "ikarashiMDC.h"
ec30109b 0:d3db2b0afc76 3 #include "omni_wheel.h"
ec30109b 0:d3db2b0afc76 4 #include "proto01.h"
ec30109b 0:d3db2b0afc76 5 #include "OmniPosition.h"
ec30109b 0:d3db2b0afc76 6 #include "SerialMultiByte.h"
ec30109b 0:d3db2b0afc76 7 #include "PID.h"
ec30109b 0:d3db2b0afc76 8 #include "PS3.h"
ec30109b 0:d3db2b0afc76 9 #include "air.h"
ec30109b 0:d3db2b0afc76 10 #include "towelest.h"
ec30109b 0:d3db2b0afc76 11 #include "chatteringremoval.h"
ec30109b 0:d3db2b0afc76 12 #include "pinconfig_main.h"
ec30109b 0:d3db2b0afc76 13
ec30109b 1:9d1ef7d7f874 14 Timer time_;
ec30109b 1:9d1ef7d7f874 15 PID angle(20.0,5.0,0.0000001,0.01);
ec30109b 0:d3db2b0afc76 16 OmniWheel omni(4);
ec30109b 0:d3db2b0afc76 17 OmniPosition posi(main_0TX,main_0RX);
ec30109b 0:d3db2b0afc76 18 Serial serial(mdTX, mdRX, 115200);
ec30109b 0:d3db2b0afc76 19 RawSerial pc(USBTX, USBRX, 115200);
ec30109b 0:d3db2b0afc76 20 SerialMultiByte sensor(main_1TX,main_1RX);
ec30109b 1:9d1ef7d7f874 21 towelest towel(&serial,3,schmitt_trigger_0);
ec30109b 0:d3db2b0afc76 22 air air;
ec30109b 0:d3db2b0afc76 23 PS3 ps3(FEPTX,FEPRX);
ec30109b 1:9d1ef7d7f874 24 chatteringremoval button0(0.3);
ec30109b 1:9d1ef7d7f874 25 chatteringremoval button1(0.3);
ec30109b 1:9d1ef7d7f874 26 chatteringremoval button3(0.3);
ec30109b 1:9d1ef7d7f874 27 chatteringremoval button4(0.3);
ec30109b 0:d3db2b0afc76 28 DigitalOut sw1(hijoteisi);
ec30109b 0:d3db2b0afc76 29 DigitalOut LED(LED1);
ec30109b 0:d3db2b0afc76 30 Timer timer;
ec30109b 0:d3db2b0afc76 31
ec30109b 0:d3db2b0afc76 32 int num = 0,X,Y,b[12],stick[4],trigger[2],loli[3];
ec30109b 1:9d1ef7d7f874 33 int X1,Y1,bb[12],mecha_num = 0,towel_num = 0;
ec30109b 1:9d1ef7d7f874 34 double X0,Y0,valueX, valueY, value[4],mecha_power[3],spin_power=0,power,theta,IRdistance[2],TFdistance[2],direction = 0.0;
ec30109b 0:d3db2b0afc76 35
ec30109b 0:d3db2b0afc76 36 union UnionBytes{
ec30109b 0:d3db2b0afc76 37 uint8_t bytes[28];
ec30109b 0:d3db2b0afc76 38 float IRdist[7]; //0~1
ec30109b 0:d3db2b0afc76 39 int32_t TFdist[7]; //2~3
ec30109b 0:d3db2b0afc76 40 int32_t pulses[7]; //4~6
ec30109b 0:d3db2b0afc76 41 };
ec30109b 1:9d1ef7d7f874 42
ec30109b 0:d3db2b0afc76 43 ikarashiMDC motor[]=
ec30109b 0:d3db2b0afc76 44 {
ec30109b 0:d3db2b0afc76 45 ikarashiMDC(0,0,SM,&serial),
ec30109b 0:d3db2b0afc76 46 ikarashiMDC(0,1,SM,&serial),
ec30109b 0:d3db2b0afc76 47 ikarashiMDC(0,2,SM,&serial),
ec30109b 0:d3db2b0afc76 48 ikarashiMDC(0,3,SM,&serial),
ec30109b 0:d3db2b0afc76 49 ikarashiMDC(1,0,SM,&serial),
ec30109b 0:d3db2b0afc76 50 ikarashiMDC(1,1,SM,&serial),
ec30109b 1:9d1ef7d7f874 51 ikarashiMDC(1,2,SM,&serial)
ec30109b 0:d3db2b0afc76 52 };
ec30109b 1:9d1ef7d7f874 53
ec30109b 1:9d1ef7d7f874 54 Proto1 proto(500.0, 1000.0, 0.8, 0.14);
ec30109b 0:d3db2b0afc76 55
ec30109b 0:d3db2b0afc76 56 void getSensorData() //Receive sensor value
ec30109b 0:d3db2b0afc76 57 {
ec30109b 0:d3db2b0afc76 58 UnionBytes bytedata;
ec30109b 0:d3db2b0afc76 59 sensor.getData(bytedata.bytes);
ec30109b 0:d3db2b0afc76 60 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 61 IRdistance[i] = bytedata.IRdist[i];
ec30109b 0:d3db2b0afc76 62 }
ec30109b 0:d3db2b0afc76 63 for(int i=0; i<2; i++){
ec30109b 0:d3db2b0afc76 64 TFdistance[i] = bytedata.TFdist[i+2];
ec30109b 0:d3db2b0afc76 65 }
ec30109b 0:d3db2b0afc76 66 for(int i=0; i<3; i++){
ec30109b 0:d3db2b0afc76 67 loli[i] = bytedata.pulses[i+4];
ec30109b 0:d3db2b0afc76 68 }
ec30109b 0:d3db2b0afc76 69 }
ec30109b 0:d3db2b0afc76 70
ec30109b 0:d3db2b0afc76 71 void Recovery() //Recovery mechanism
ec30109b 0:d3db2b0afc76 72 {
ec30109b 0:d3db2b0afc76 73 /* 洗濯物を挟む機構 */
ec30109b 1:9d1ef7d7f874 74 if(b[5] == 1 && trigger[1] < 50){
ec30109b 1:9d1ef7d7f874 75 mecha_power[1] -= 0.03;
ec30109b 1:9d1ef7d7f874 76 if(mecha_power[1] < -1.0){
ec30109b 1:9d1ef7d7f874 77 mecha_power[1] = -1.0;
ec30109b 1:9d1ef7d7f874 78 }
ec30109b 1:9d1ef7d7f874 79 }else if(b[5] == 0 && trigger[1] > 50){
ec30109b 1:9d1ef7d7f874 80 mecha_power[1] += 0.03;
ec30109b 1:9d1ef7d7f874 81 if(mecha_power[1] > 1.0){
ec30109b 1:9d1ef7d7f874 82 mecha_power[1] = 1.0;
ec30109b 1:9d1ef7d7f874 83 }
ec30109b 0:d3db2b0afc76 84 }else{
ec30109b 0:d3db2b0afc76 85 mecha_power[1] = 0;
ec30109b 0:d3db2b0afc76 86 }
ec30109b 1:9d1ef7d7f874 87 if(b[4] == 1 && trigger[0] < 50){
ec30109b 1:9d1ef7d7f874 88 mecha_power[2] += 0.03;
ec30109b 1:9d1ef7d7f874 89 if(mecha_power[2] > 1.0){
ec30109b 1:9d1ef7d7f874 90 mecha_power[2] = 1.0;
ec30109b 1:9d1ef7d7f874 91 }
ec30109b 1:9d1ef7d7f874 92 }else if(b[4] == 0 && trigger[0] > 50){
ec30109b 1:9d1ef7d7f874 93 mecha_power[2] -= 0.03;
ec30109b 1:9d1ef7d7f874 94 if(mecha_power[2] < -1.0){
ec30109b 1:9d1ef7d7f874 95 mecha_power[2] = -1.0;
ec30109b 1:9d1ef7d7f874 96 }
ec30109b 0:d3db2b0afc76 97 }else{
ec30109b 0:d3db2b0afc76 98 mecha_power[2] = 0;
ec30109b 0:d3db2b0afc76 99 }
ec30109b 0:d3db2b0afc76 100 /* 洗濯物を挟む機構の首 */
ec30109b 1:9d1ef7d7f874 101 if (b[2] == 1) {
ec30109b 1:9d1ef7d7f874 102 mecha_power[0] += 0.04;
ec30109b 1:9d1ef7d7f874 103 if(mecha_power[0] > 0.8){
ec30109b 1:9d1ef7d7f874 104 mecha_power[0] = 0.8;
ec30109b 1:9d1ef7d7f874 105 }
ec30109b 1:9d1ef7d7f874 106 }else{
ec30109b 1:9d1ef7d7f874 107 mecha_power[0] = 0;
ec30109b 0:d3db2b0afc76 108 }
ec30109b 0:d3db2b0afc76 109
ec30109b 0:d3db2b0afc76 110 motor[4].setSpeed(mecha_power[0]);
ec30109b 1:9d1ef7d7f874 111 motor[5].setSpeed(mecha_power[2]);
ec30109b 1:9d1ef7d7f874 112 motor[6].setSpeed(mecha_power[1]);
ec30109b 0:d3db2b0afc76 113 }
ec30109b 0:d3db2b0afc76 114
ec30109b 0:d3db2b0afc76 115 void stop()
ec30109b 0:d3db2b0afc76 116 {
ec30109b 1:9d1ef7d7f874 117 for(int i=4;i<7;i++) motor[i].setSpeed(0);
ec30109b 0:d3db2b0afc76 118 }
ec30109b 0:d3db2b0afc76 119
ec30109b 1:9d1ef7d7f874 120 void mechamove()
ec30109b 0:d3db2b0afc76 121 {
ec30109b 1:9d1ef7d7f874 122 if(towel_num == 1){
ec30109b 1:9d1ef7d7f874 123 towel.setPoint(2000);
ec30109b 0:d3db2b0afc76 124 }else{
ec30109b 1:9d1ef7d7f874 125 towel.setPoint(3650);
ec30109b 0:d3db2b0afc76 126 }
ec30109b 0:d3db2b0afc76 127 }
ec30109b 1:9d1ef7d7f874 128
ec30109b 0:d3db2b0afc76 129 int main()
ec30109b 0:d3db2b0afc76 130 {
ec30109b 1:9d1ef7d7f874 131 sw1 = true;
ec30109b 1:9d1ef7d7f874 132 double nowX,nowY,t,LPF[4],lastLPF[4],start_angle,now_angle,original_angle = -3.14;
ec30109b 1:9d1ef7d7f874 133 int b2[12],b3[12],estop_num=0;
ec30109b 0:d3db2b0afc76 134
ec30109b 0:d3db2b0afc76 135 for(int i=0;i<4;i++)motor[i].braking = false;
ec30109b 0:d3db2b0afc76 136 omni.wheel[0].setRadian(PI * 1 / 4);
ec30109b 0:d3db2b0afc76 137 omni.wheel[1].setRadian(PI * 3 / 4);
ec30109b 0:d3db2b0afc76 138 omni.wheel[2].setRadian(PI * 5 / 4);
ec30109b 0:d3db2b0afc76 139 omni.wheel[3].setRadian(PI * 7 / 4);
ec30109b 0:d3db2b0afc76 140 angle.setInputLimits(-6.28,6.28);
ec30109b 1:9d1ef7d7f874 141 angle.setOutputLimits(-0.5,0.5);
ec30109b 0:d3db2b0afc76 142 angle.setBias(0);
ec30109b 0:d3db2b0afc76 143 angle.setMode(1);
ec30109b 0:d3db2b0afc76 144 angle.setSetPoint(0);
ec30109b 0:d3db2b0afc76 145
ec30109b 1:9d1ef7d7f874 146 button0.countreset();
ec30109b 0:d3db2b0afc76 147 button1.countreset();
ec30109b 1:9d1ef7d7f874 148 button3.countreset();
ec30109b 0:d3db2b0afc76 149
ec30109b 0:d3db2b0afc76 150 sensor.startReceive(28);
ec30109b 1:9d1ef7d7f874 151 // time_.reset();
ec30109b 1:9d1ef7d7f874 152 // time_.start();
ec30109b 0:d3db2b0afc76 153 while(1){
ec30109b 1:9d1ef7d7f874 154 // pc.printf("%d\n\r",time_.read_ms());
ec30109b 1:9d1ef7d7f874 155 // time_.reset();
ec30109b 1:9d1ef7d7f874 156 // time_.start();
ec30109b 1:9d1ef7d7f874 157 LED = !LED;
ec30109b 0:d3db2b0afc76 158
ec30109b 0:d3db2b0afc76 159 towel.setPulse(loli[2]);
ec30109b 0:d3db2b0afc76 160
ec30109b 1:9d1ef7d7f874 161 // PS3 value
ec30109b 0:d3db2b0afc76 162 for(int i = 0; i < 12; i++) {
ec30109b 0:d3db2b0afc76 163 b[i] = ps3.getButton(i);
ec30109b 1:9d1ef7d7f874 164 b3[i] = b2[i] - b[i];
ec30109b 1:9d1ef7d7f874 165 b2[i] = b[i];
ec30109b 1:9d1ef7d7f874 166 // pc.printf("%2d", b[i] );
ec30109b 0:d3db2b0afc76 167 }
ec30109b 0:d3db2b0afc76 168 for(int i = 0; i < 4; i++) {
ec30109b 0:d3db2b0afc76 169 stick[i] = ps3.getStick(i);
ec30109b 1:9d1ef7d7f874 170 // pc.printf("%4d", stick[i] );
ec30109b 0:d3db2b0afc76 171 }
ec30109b 1:9d1ef7d7f874 172 X0 = (stick[0] - 127.0) / 127.0;
ec30109b 1:9d1ef7d7f874 173 Y0 = ((stick[1] * -1) + 127.0) / 127.0;
ec30109b 0:d3db2b0afc76 174 X1 = stick[2];
ec30109b 0:d3db2b0afc76 175 Y1 = stick[3];
ec30109b 0:d3db2b0afc76 176
ec30109b 0:d3db2b0afc76 177 for(int i = 0; i < 2; i++) {
ec30109b 0:d3db2b0afc76 178 trigger[i] = ps3.getTrigger(i);
ec30109b 0:d3db2b0afc76 179 // pc.printf("%4d",trigger[i]);
ec30109b 0:d3db2b0afc76 180 }
ec30109b 1:9d1ef7d7f874 181
ec30109b 0:d3db2b0afc76 182 //Sensor value
ec30109b 0:d3db2b0afc76 183 getSensorData();
ec30109b 1:9d1ef7d7f874 184
ec30109b 0:d3db2b0afc76 185 //Count
ec30109b 1:9d1ef7d7f874 186 button0.assignvalue(b[6]);
ec30109b 1:9d1ef7d7f874 187 button1.assignvalue(b[11]);
ec30109b 1:9d1ef7d7f874 188 button3.assignvalue(b[1]);
ec30109b 1:9d1ef7d7f874 189 button4.assignvalue(b[0]);
ec30109b 1:9d1ef7d7f874 190 num = button0.getCount();
ec30109b 1:9d1ef7d7f874 191 mecha_num = button3.getCount();
ec30109b 1:9d1ef7d7f874 192 towel_num = button4.getCount() % 2;
ec30109b 1:9d1ef7d7f874 193 estop_num = button1.getCount() % 2;
ec30109b 1:9d1ef7d7f874 194
ec30109b 1:9d1ef7d7f874 195 //estop
ec30109b 1:9d1ef7d7f874 196 if(estop_num == 1){
ec30109b 1:9d1ef7d7f874 197 sw1 = false;
ec30109b 1:9d1ef7d7f874 198 }else{
ec30109b 1:9d1ef7d7f874 199 sw1 = true;
ec30109b 1:9d1ef7d7f874 200 }
ec30109b 0:d3db2b0afc76 201
ec30109b 0:d3db2b0afc76 202 /*【Action】*/
ec30109b 1:9d1ef7d7f874 203 switch(num){
ec30109b 1:9d1ef7d7f874 204 case 0: towel.allstop();
ec30109b 1:9d1ef7d7f874 205 stop();
ec30109b 1:9d1ef7d7f874 206 break;
ec30109b 1:9d1ef7d7f874 207 case 1: Recovery();
ec30109b 1:9d1ef7d7f874 208 mechamove();
ec30109b 0:d3db2b0afc76 209 towel.open();
ec30109b 0:d3db2b0afc76 210 air.solenoid1_open();
ec30109b 0:d3db2b0afc76 211 nowX = posi.getX();
ec30109b 0:d3db2b0afc76 212 nowY = posi.getY();
ec30109b 0:d3db2b0afc76 213 break;
ec30109b 1:9d1ef7d7f874 214 //case 2: proto.targetXY(2000,3000, nowX, nowY);
ec30109b 1:9d1ef7d7f874 215 // towel.allstop();
ec30109b 1:9d1ef7d7f874 216 // stop();
ec30109b 1:9d1ef7d7f874 217 // break;
ec30109b 1:9d1ef7d7f874 218 // case 3: mechamove();
ec30109b 1:9d1ef7d7f874 219 // stop();
ec30109b 0:d3db2b0afc76 220 // nowX = posi.getX();
ec30109b 0:d3db2b0afc76 221 // nowY = posi.getY();
ec30109b 0:d3db2b0afc76 222 // break;
ec30109b 1:9d1ef7d7f874 223 // case 4: proto.targetXY(0, 0, nowX, nowY);
ec30109b 1:9d1ef7d7f874 224 // towel.allstop();
ec30109b 1:9d1ef7d7f874 225 // stop();
ec30109b 0:d3db2b0afc76 226 // break;
ec30109b 0:d3db2b0afc76 227 default: motor[0].setSpeed(0);
ec30109b 0:d3db2b0afc76 228 motor[1].setSpeed(0);
ec30109b 0:d3db2b0afc76 229 motor[2].setSpeed(0);
ec30109b 0:d3db2b0afc76 230 motor[3].setSpeed(0);
ec30109b 0:d3db2b0afc76 231
ec30109b 1:9d1ef7d7f874 232 towel.allstop();
ec30109b 1:9d1ef7d7f874 233
ec30109b 0:d3db2b0afc76 234 air.solenoid1_close();
ec30109b 0:d3db2b0afc76 235 air.solenoid2_close();
ec30109b 0:d3db2b0afc76 236
ec30109b 0:d3db2b0afc76 237 }
ec30109b 0:d3db2b0afc76 238
ec30109b 1:9d1ef7d7f874 239 towel.pid_compute();
ec30109b 1:9d1ef7d7f874 240
ec30109b 1:9d1ef7d7f874 241 if(num == 0){
ec30109b 1:9d1ef7d7f874 242 motor[0].setSpeed(0);
ec30109b 1:9d1ef7d7f874 243 motor[1].setSpeed(0);
ec30109b 1:9d1ef7d7f874 244 motor[2].setSpeed(0);
ec30109b 1:9d1ef7d7f874 245 motor[3].setSpeed(0);
ec30109b 1:9d1ef7d7f874 246 air.solenoid1_close();
ec30109b 1:9d1ef7d7f874 247 air.solenoid2_close();
ec30109b 1:9d1ef7d7f874 248
ec30109b 0:d3db2b0afc76 249 }else{
ec30109b 1:9d1ef7d7f874 250 power = hypot(X0,Y0);
ec30109b 1:9d1ef7d7f874 251 theta = atan2(Y0,X0) + posi.getTheta();
ec30109b 1:9d1ef7d7f874 252 /**
ec30109b 1:9d1ef7d7f874 253 * ここまでテンプル
ec30109b 1:9d1ef7d7f874 254 */
ec30109b 1:9d1ef7d7f874 255 start_angle = posi.getTheta();
ec30109b 1:9d1ef7d7f874 256
ec30109b 1:9d1ef7d7f874 257 if ( b3[8] == 1) original_angle -= 1.57;
ec30109b 1:9d1ef7d7f874 258 if ( b3[9] == 1) original_angle += 1.57;
ec30109b 1:9d1ef7d7f874 259
ec30109b 1:9d1ef7d7f874 260 if ( b[3] ) original_angle = start_angle;
ec30109b 1:9d1ef7d7f874 261
ec30109b 1:9d1ef7d7f874 262 now_angle = start_angle - original_angle;
ec30109b 1:9d1ef7d7f874 263
ec30109b 1:9d1ef7d7f874 264 if ( now_angle > 3.14 ) {
ec30109b 1:9d1ef7d7f874 265 now_angle -= 6.28;
ec30109b 1:9d1ef7d7f874 266 }
ec30109b 1:9d1ef7d7f874 267 if ( now_angle < -3.14 ) {
ec30109b 1:9d1ef7d7f874 268 now_angle += 6.28;
ec30109b 1:9d1ef7d7f874 269 }
ec30109b 1:9d1ef7d7f874 270
ec30109b 1:9d1ef7d7f874 271 angle.setProcessValue(now_angle);
ec30109b 1:9d1ef7d7f874 272 spin_power = -angle.compute();
ec30109b 1:9d1ef7d7f874 273 if(b[7] == 1){
ec30109b 1:9d1ef7d7f874 274 power *= 1.0;
ec30109b 1:9d1ef7d7f874 275 }else{
ec30109b 1:9d1ef7d7f874 276 power *= 0.3;
ec30109b 1:9d1ef7d7f874 277 }
ec30109b 1:9d1ef7d7f874 278 omni.computeCircular(power,theta,spin_power);
ec30109b 1:9d1ef7d7f874 279
ec30109b 1:9d1ef7d7f874 280 for(int i = 0; i < 4; i++){
ec30109b 1:9d1ef7d7f874 281 value[i] = omni.wheel[i];
ec30109b 1:9d1ef7d7f874 282 LPF[i] = (1 - 0.8) * lastLPF[i] + 0.8 * value[i];
ec30109b 1:9d1ef7d7f874 283 lastLPF[i] = LPF[i];
ec30109b 1:9d1ef7d7f874 284 motor[i].setSpeed(LPF[i]);
ec30109b 1:9d1ef7d7f874 285 // pc.printf("%f ", LPF[i]);
ec30109b 1:9d1ef7d7f874 286 }
ec30109b 1:9d1ef7d7f874 287
ec30109b 1:9d1ef7d7f874 288 //}else{
ec30109b 1:9d1ef7d7f874 289 // angle.setProcessValue(posi.getTheta());
ec30109b 1:9d1ef7d7f874 290 // spin_power = -angle.compute();
ec30109b 1:9d1ef7d7f874 291 // proto.Input_nowXY( posi.getX() , posi.getY() );
ec30109b 1:9d1ef7d7f874 292 // proto.calculate();
ec30109b 1:9d1ef7d7f874 293 // omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
ec30109b 1:9d1ef7d7f874 294 //
ec30109b 1:9d1ef7d7f874 295 // for(int i = 0; i < 4; i++){
ec30109b 1:9d1ef7d7f874 296 // value[i] = omni.wheel[i];
ec30109b 1:9d1ef7d7f874 297 // motor[i].setSpeed(value[i]);
ec30109b 1:9d1ef7d7f874 298 //// pc.printf("%f ", value[i]);
ec30109b 1:9d1ef7d7f874 299 // }
ec30109b 1:9d1ef7d7f874 300 //
ec30109b 0:d3db2b0afc76 301 }
ec30109b 0:d3db2b0afc76 302
ec30109b 0:d3db2b0afc76 303
ec30109b 0:d3db2b0afc76 304 // wait(0.001);
ec30109b 1:9d1ef7d7f874 305 // pc.printf(" loli:%d ",loli[2]);
ec30109b 1:9d1ef7d7f874 306 // pc.printf(" %d %d case:%d ",mecha_num,towel_num,num);
ec30109b 1:9d1ef7d7f874 307 // for(int i=0;i<12;i++) pc.printf(" %d ",b3[i]);
ec30109b 0:d3db2b0afc76 308 //Wheel value
ec30109b 1:9d1ef7d7f874 309 // pc.printf(" stickX:%f stickY:%f power:%f θ:%f",X0,Y0,power,theta);
ec30109b 0:d3db2b0afc76 310 // pc.printf(" vector:%f pid:%.2f",proto.vector,spin_power);
ec30109b 0:d3db2b0afc76 311 //Odmetry
ec30109b 1:9d1ef7d7f874 312 // pc.printf("x:%d y:%d θ:%f ",posi.getX(),posi.getY(),posi.getTheta());
ec30109b 0:d3db2b0afc76 313
ec30109b 1:9d1ef7d7f874 314 // pc.printf("\r\n");
ec30109b 0:d3db2b0afc76 315 }
ec30109b 0:d3db2b0afc76 316 }
ec30109b 1:9d1ef7d7f874 317