NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2019_usiro_v6

NHK2019_Team_B_Automatic_machine_usirogawa

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen

main.cpp@1:64871263444f, 2019-10-02 (annotated)

Committer:
skouki
Date:
Wed Oct 02 09:59:32 2019 +0000
Revision:
1:64871263444f
Parent:
0:3ad208cbea5f 
v6;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skouki 0:3ad208cbea5f 1 #include"mbed.h"
skouki 0:3ad208cbea5f 2 #include"measuring_wheel.h"
skouki 0:3ad208cbea5f 3 #include"ikarashiMDC.h"
skouki 0:3ad208cbea5f 4 #include"position_controller.h"
skouki 0:3ad208cbea5f 5 #include"omni_wheel.h"
skouki 0:3ad208cbea5f 6 #include"pin_config.h"
skouki 0:3ad208cbea5f 7 #include"SerialMultiByte.h"
skouki 0:3ad208cbea5f 8 #include"PID.h"
skouki 0:3ad208cbea5f 9 #include"IRsensor.h"
skouki 0:3ad208cbea5f 10
skouki 1:64871263444f 11 #define YPOINT 6400
skouki 1:64871263444f 12 #define GAP 15
skouki 1:64871263444f 13 #define MTOU 300
skouki 0:3ad208cbea5f 14
skouki 0:3ad208cbea5f 15 Serial serial(MDCTX,MDCRX,115200);
skouki 0:3ad208cbea5f 16 ikarashiMDC motor[]={
skouki 0:3ad208cbea5f 17 ikarashiMDC(1,0,SM,&serial),
skouki 0:3ad208cbea5f 18 ikarashiMDC(1,1,SM,&serial),
skouki 0:3ad208cbea5f 19 ikarashiMDC(1,2,SM,&serial),
skouki 0:3ad208cbea5f 20 ikarashiMDC(1,3,SM,&serial)
skouki 0:3ad208cbea5f 21 };
skouki 1:64871263444f 22 PositionController position_control_1(1000,1000,0.3,0.01,0.3);
skouki 0:3ad208cbea5f 23
skouki 0:3ad208cbea5f 24 OmniWheel omni(4);
skouki 0:3ad208cbea5f 25 SerialMultiByte s(SERIALTX,SERIALRX);
skouki 0:3ad208cbea5f 26 MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
skouki 0:3ad208cbea5f 27 PID pid_spin(0,0,0,0.001);
skouki 0:3ad208cbea5f 28 PID pid_x(0,0,0,0.001);
skouki 0:3ad208cbea5f 29 PID pid_y(0,0,0,0.001);
skouki 0:3ad208cbea5f 30 Serial pc(USBTX,USBRX,115200);
skouki 0:3ad208cbea5f 31
skouki 1:64871263444f 32 DigitalIn an(USER_BUTTON);
skouki 0:3ad208cbea5f 33 DigitalOut debug_led_0(LED_0);
skouki 0:3ad208cbea5f 34 DigitalOut debug_led_1(LED_2);
skouki 0:3ad208cbea5f 35 DigitalOut debug_led_2(LED_1);
skouki 0:3ad208cbea5f 36
skouki 0:3ad208cbea5f 37 IRsensor IR0(IR_0);
skouki 1:64871263444f 38 IRsensor IR1(IR_1);
skouki 0:3ad208cbea5f 39
skouki 0:3ad208cbea5f 40 int mode;
skouki 0:3ad208cbea5f 41 int instruction_mode;
skouki 0:3ad208cbea5f 42 double omni_power[4];
skouki 0:3ad208cbea5f 43 double X_power,Y_power;
skouki 0:3ad208cbea5f 44 double spin_power;
skouki 0:3ad208cbea5f 45 float y_point = YPOINT;
skouki 0:3ad208cbea5f 46 int X_,Y_;
skouki 0:3ad208cbea5f 47 double dai_x,dai_low_y;
skouki 0:3ad208cbea5f 48 int gap = GAP;
skouki 1:64871263444f 49 double ir_distance;
skouki 1:64871263444f 50 int data_a;
skouki 1:64871263444f 51 int m_to_u = MTOU;
skouki 0:3ad208cbea5f 52
skouki 0:3ad208cbea5f 53 void set_up()
skouki 0:3ad208cbea5f 54 {
skouki 0:3ad208cbea5f 55 float theta = PIII / 4;
skouki 0:3ad208cbea5f 56 omni.wheel[0].setRadian(PIII - theta);
skouki 0:3ad208cbea5f 57 omni.wheel[1].setRadian(theta);
skouki 0:3ad208cbea5f 58 omni.wheel[2].setRadian(-theta);
skouki 0:3ad208cbea5f 59 omni.wheel[3].setRadian(PIII + theta);
skouki 0:3ad208cbea5f 60
skouki 0:3ad208cbea5f 61 s.setHeaders('A','Z');
skouki 1:64871263444f 62 s.startReceive(6);
skouki 0:3ad208cbea5f 63
skouki 0:3ad208cbea5f 64 IR0.startAveraging(5);
skouki 1:64871263444f 65 IR1.startAveraging(5);
skouki 0:3ad208cbea5f 66
skouki 0:3ad208cbea5f 67 }
skouki 0:3ad208cbea5f 68
skouki 0:3ad208cbea5f 69 void mode1()
skouki 0:3ad208cbea5f 70 {
skouki 0:3ad208cbea5f 71 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 72 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 73
skouki 1:64871263444f 74 position_control_1.compute(1,m.getOutY());
skouki 0:3ad208cbea5f 75 Y_power += position_control_1.getVelocityY();
skouki 0:3ad208cbea5f 76
skouki 1:64871263444f 77 pid_y.setProcessValue(m.getOutY() - Y_ + 200);
skouki 0:3ad208cbea5f 78 Y_power += pid_y.compute();
skouki 0:3ad208cbea5f 79
skouki 1:64871263444f 80 if(Y_power <= 0.0)Y_power = 0.0;
skouki 1:64871263444f 81
skouki 0:3ad208cbea5f 82 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 83 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 84
skouki 0:3ad208cbea5f 85 }
skouki 0:3ad208cbea5f 86
skouki 0:3ad208cbea5f 87 void mode2()
skouki 0:3ad208cbea5f 88 {
skouki 1:64871263444f 89 if(data_a)X_power -= 0.3;
skouki 1:64871263444f 90 else X_power += 0.3;
skouki 0:3ad208cbea5f 91
skouki 0:3ad208cbea5f 92 pid_x.setProcessValue(m.getOutX() - X_);
skouki 0:3ad208cbea5f 93 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 94
skouki 0:3ad208cbea5f 95 pid_y.setProcessValue(m.getOutY());
skouki 0:3ad208cbea5f 96 Y_power += pid_y.compute();
skouki 1:64871263444f 97
skouki 1:64871263444f 98 if(y_point == 0){
skouki 1:64871263444f 99 if(Y_power <= 0.0)Y_power = 0.0;
skouki 1:64871263444f 100 }
skouki 0:3ad208cbea5f 101
skouki 0:3ad208cbea5f 102 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 103 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 104
skouki 0:3ad208cbea5f 105 }
skouki 0:3ad208cbea5f 106
skouki 0:3ad208cbea5f 107 void mode3()
skouki 0:3ad208cbea5f 108 {
skouki 0:3ad208cbea5f 109 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 110 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 111
skouki 1:64871263444f 112 pid_y.setProcessValue(Y_ - m.getOutY());
skouki 1:64871263444f 113 Y_power -= pid_y.compute();
skouki 1:64871263444f 114
skouki 1:64871263444f 115
skouki 1:64871263444f 116 if(Y_power <= 0.0)Y_power = 0.0;
skouki 0:3ad208cbea5f 117
skouki 0:3ad208cbea5f 118 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 119 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 120
skouki 0:3ad208cbea5f 121 }
skouki 0:3ad208cbea5f 122
skouki 0:3ad208cbea5f 123 void mode4()
skouki 0:3ad208cbea5f 124 {
skouki 0:3ad208cbea5f 125 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 126 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 127
skouki 1:64871263444f 128 pid_y.setProcessValue(Y_ - m.getOutY());
skouki 1:64871263444f 129 Y_power -= pid_y.compute();
skouki 1:64871263444f 130
skouki 1:64871263444f 131 if(Y_power <= 0.0)Y_power = 0.0;
skouki 1:64871263444f 132
skouki 0:3ad208cbea5f 133
skouki 0:3ad208cbea5f 134 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 135 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 136
skouki 0:3ad208cbea5f 137 }
skouki 0:3ad208cbea5f 138
skouki 1:64871263444f 139 void mode5()
skouki 1:64871263444f 140 {
skouki 1:64871263444f 141 if(data_a)X_power -= 0.3;
skouki 1:64871263444f 142 else X_power += 0.3;
skouki 1:64871263444f 143
skouki 1:64871263444f 144
skouki 1:64871263444f 145 pid_x.setProcessValue(m.getOutX() - X_);
skouki 1:64871263444f 146 X_power += pid_x.compute();
skouki 1:64871263444f 147
skouki 1:64871263444f 148
skouki 1:64871263444f 149 pid_y.setProcessValue(Y_ - m.getOutY());
skouki 1:64871263444f 150 Y_power -= pid_y.compute();
skouki 1:64871263444f 151
skouki 1:64871263444f 152 pid_spin.setProcessValue(m.getjyroAngle());
skouki 1:64871263444f 153 spin_power = pid_spin.compute();
skouki 1:64871263444f 154
skouki 1:64871263444f 155 }
skouki 1:64871263444f 156
skouki 1:64871263444f 157 void mode6()
skouki 1:64871263444f 158 {
skouki 1:64871263444f 159 if(data_a)X_power -= 0.3;
skouki 1:64871263444f 160 else X_power += 0.3;
skouki 1:64871263444f 161
skouki 1:64871263444f 162
skouki 1:64871263444f 163 pid_x.setProcessValue(m.getOutX() - X_);
skouki 1:64871263444f 164 X_power += pid_x.compute();
skouki 1:64871263444f 165
skouki 1:64871263444f 166
skouki 1:64871263444f 167 pid_y.setProcessValue(Y_ - m.getOutY());
skouki 1:64871263444f 168 Y_power -= pid_y.compute();
skouki 1:64871263444f 169
skouki 1:64871263444f 170 pid_spin.setProcessValue(m.getjyroAngle());
skouki 1:64871263444f 171 spin_power = pid_spin.compute();
skouki 1:64871263444f 172
skouki 1:64871263444f 173 if(m.getjyroAngle() <=-10.0)Y_power += 0.5;
skouki 1:64871263444f 174
skouki 1:64871263444f 175
skouki 1:64871263444f 176 }
skouki 1:64871263444f 177
skouki 0:3ad208cbea5f 178 void to_main()
skouki 0:3ad208cbea5f 179 {
skouki 0:3ad208cbea5f 180 unsigned char data[1];
skouki 1:64871263444f 181 unsigned char getdata[6];
skouki 0:3ad208cbea5f 182 data[0] = mode;
skouki 0:3ad208cbea5f 183 s.sendData(data,1);
skouki 0:3ad208cbea5f 184 s.getData(getdata);
skouki 0:3ad208cbea5f 185 instruction_mode=getdata[0];
skouki 0:3ad208cbea5f 186
skouki 0:3ad208cbea5f 187 if(getdata[2]>=(1<<7)){
skouki 0:3ad208cbea5f 188 getdata[2]-=(1<<7);
skouki 0:3ad208cbea5f 189 X_=-1*(getdata[1]+(getdata[2]<<8));
skouki 0:3ad208cbea5f 190 }
skouki 0:3ad208cbea5f 191 else X_= getdata[1]+(getdata[2]<<8);
skouki 0:3ad208cbea5f 192
skouki 0:3ad208cbea5f 193 if(getdata[4]>=(1<<7)){
skouki 0:3ad208cbea5f 194 getdata[4]-=(1<<7);
skouki 0:3ad208cbea5f 195 Y_=-1*(getdata[3]+(getdata[4]<<8));
skouki 0:3ad208cbea5f 196 }
skouki 0:3ad208cbea5f 197 else Y_= getdata[3]+(getdata[4]<<8);
skouki 1:64871263444f 198 data_a = getdata[5];
skouki 0:3ad208cbea5f 199 if(instruction_mode!=0)debug_led_1 = !debug_led_1;
skouki 0:3ad208cbea5f 200 }
skouki 0:3ad208cbea5f 201 int main()
skouki 0:3ad208cbea5f 202 {
skouki 0:3ad208cbea5f 203 set_up();
skouki 1:64871263444f 204 an.mode(PullUp);
skouki 0:3ad208cbea5f 205 while(true){
skouki 0:3ad208cbea5f 206 debug_led_0 = !debug_led_0;
skouki 1:64871263444f 207 if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){
skouki 1:64871263444f 208 debug_led_2 = 1;
skouki 1:64871263444f 209 }
skouki 1:64871263444f 210 else debug_led_2 = 0;
skouki 0:3ad208cbea5f 211 X_power = 0.0;
skouki 0:3ad208cbea5f 212 Y_power = 0.0;
skouki 0:3ad208cbea5f 213 spin_power = 0.0;
skouki 1:64871263444f 214 if(data_a)ir_distance = IR0.get_Averagingdistance();
skouki 1:64871263444f 215 else ir_distance = IR1.get_Averagingdistance();
skouki 0:3ad208cbea5f 216
skouki 0:3ad208cbea5f 217 to_main();
skouki 0:3ad208cbea5f 218
skouki 0:3ad208cbea5f 219 if(instruction_mode == 1&&mode == 0){
skouki 0:3ad208cbea5f 220
skouki 0:3ad208cbea5f 221 pid_x.reset();
skouki 0:3ad208cbea5f 222 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 223 pid_x.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 224 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 225 pid_x.setBias(0);
skouki 0:3ad208cbea5f 226 pid_x.setMode(1);
skouki 0:3ad208cbea5f 227 pid_x.setSetPoint(0.0);
skouki 0:3ad208cbea5f 228
skouki 0:3ad208cbea5f 229 position_control_1.targetXY(1,int(y_point));
skouki 0:3ad208cbea5f 230
skouki 0:3ad208cbea5f 231 pid_y.reset();
skouki 1:64871263444f 232 pid_y.setTunings(7.0,1.0,0.000001);
skouki 0:3ad208cbea5f 233 pid_y.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 234 pid_y.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 235 pid_y.setBias(0);
skouki 0:3ad208cbea5f 236 pid_y.setMode(1);
skouki 0:3ad208cbea5f 237 pid_y.setSetPoint(0.0);
skouki 0:3ad208cbea5f 238
skouki 0:3ad208cbea5f 239 pid_spin.reset();
skouki 0:3ad208cbea5f 240 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 241 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 242 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 243 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 244 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 245 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 246
skouki 0:3ad208cbea5f 247 mode = 1;
skouki 0:3ad208cbea5f 248 }
skouki 0:3ad208cbea5f 249
skouki 1:64871263444f 250 if((m.getOutY() >= (y_point - 260)) && mode == 1){
skouki 0:3ad208cbea5f 251
skouki 0:3ad208cbea5f 252 pid_x.reset();
skouki 1:64871263444f 253 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 254 pid_x.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 255 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 256 pid_x.setBias(0);
skouki 0:3ad208cbea5f 257 pid_x.setMode(1);
skouki 0:3ad208cbea5f 258 pid_x.setSetPoint(0.0);
skouki 0:3ad208cbea5f 259
skouki 0:3ad208cbea5f 260 pid_y.reset();
skouki 1:64871263444f 261 pid_y.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 262 pid_y.setInputLimits(y_point - 150 - 1000.0,y_point - 150 + 1000.0);
skouki 0:3ad208cbea5f 263 pid_y.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 264 pid_y.setBias(0);
skouki 0:3ad208cbea5f 265 pid_y.setMode(1);
skouki 1:64871263444f 266 pid_y.setSetPoint(y_point - 200);
skouki 0:3ad208cbea5f 267
skouki 0:3ad208cbea5f 268 pid_spin.reset();
skouki 0:3ad208cbea5f 269 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 270 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 271 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 272 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 273 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 274 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 275
skouki 0:3ad208cbea5f 276 mode = 2;
skouki 0:3ad208cbea5f 277 }
skouki 0:3ad208cbea5f 278
skouki 1:64871263444f 279 if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){
skouki 0:3ad208cbea5f 280 dai_x = m.getOutX();
skouki 0:3ad208cbea5f 281 mode = 0xff;
skouki 0:3ad208cbea5f 282 }
skouki 0:3ad208cbea5f 283
skouki 0:3ad208cbea5f 284 if(instruction_mode == 3&&mode == 0xff){
skouki 0:3ad208cbea5f 285 pid_x.reset();
skouki 0:3ad208cbea5f 286 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 287 pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
skouki 0:3ad208cbea5f 288 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 289 pid_x.setBias(0);
skouki 0:3ad208cbea5f 290 pid_x.setMode(1);
skouki 0:3ad208cbea5f 291 pid_x.setSetPoint(dai_x);
skouki 0:3ad208cbea5f 292
skouki 1:64871263444f 293 pid_y.reset();
skouki 1:64871263444f 294 pid_y.setTunings(10.0,1.0,0.000001);
skouki 1:64871263444f 295 pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
skouki 1:64871263444f 296 pid_y.setOutputLimits(-0.3,0.3);
skouki 1:64871263444f 297 pid_y.setBias(0);
skouki 1:64871263444f 298 pid_y.setMode(1);
skouki 1:64871263444f 299 pid_y.setSetPoint(m_to_u);
skouki 1:64871263444f 300
skouki 0:3ad208cbea5f 301 pid_spin.reset();
skouki 0:3ad208cbea5f 302 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 303 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 304 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 305 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 306 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 307 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 308
skouki 0:3ad208cbea5f 309 mode = 3;
skouki 0:3ad208cbea5f 310 }
skouki 0:3ad208cbea5f 311
skouki 1:64871263444f 312 if(ir_distance>=20&&mode == 3){
skouki 0:3ad208cbea5f 313 dai_low_y = m.getOutY();
skouki 0:3ad208cbea5f 314
skouki 0:3ad208cbea5f 315 pid_x.reset();
skouki 0:3ad208cbea5f 316 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 317 pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
skouki 0:3ad208cbea5f 318 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 319 pid_x.setBias(0);
skouki 0:3ad208cbea5f 320 pid_x.setMode(1);
skouki 0:3ad208cbea5f 321 pid_x.setSetPoint(dai_x);
skouki 0:3ad208cbea5f 322
skouki 1:64871263444f 323 // pid_y.reset();
skouki 1:64871263444f 324 // pid_y.setTunings(10.0,1.0,0.000001);
skouki 1:64871263444f 325 // pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
skouki 1:64871263444f 326 // pid_y.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 327 // pid_y.setBias(0);
skouki 1:64871263444f 328 // pid_y.setMode(1);
skouki 1:64871263444f 329 // pid_y.setSetPoint(dai_low_y - gap);
skouki 1:64871263444f 330
skouki 1:64871263444f 331 m_to_u = MTOU + 40 ;
skouki 0:3ad208cbea5f 332 pid_y.reset();
skouki 1:64871263444f 333 pid_y.setTunings(10.0,1.0,0.000001);
skouki 1:64871263444f 334 pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
skouki 1:64871263444f 335 pid_y.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 336 pid_y.setBias(0);
skouki 0:3ad208cbea5f 337 pid_y.setMode(1);
skouki 1:64871263444f 338 pid_y.setSetPoint(m_to_u);
skouki 1:64871263444f 339
skouki 0:3ad208cbea5f 340 pid_spin.reset();
skouki 0:3ad208cbea5f 341 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 342 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 343 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 344 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 345 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 346 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 347
skouki 0:3ad208cbea5f 348 mode = 4;
skouki 0:3ad208cbea5f 349 }
skouki 0:3ad208cbea5f 350
skouki 1:64871263444f 351 if(instruction_mode == 5&&mode == 4){
skouki 1:64871263444f 352 //gap = GAP - 30;
skouki 1:64871263444f 353
skouki 1:64871263444f 354 pid_x.reset();
skouki 1:64871263444f 355 pid_x.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 356 pid_x.setInputLimits(-1000.0,1000.0);
skouki 1:64871263444f 357 pid_x.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 358 pid_x.setBias(0);
skouki 1:64871263444f 359 pid_x.setMode(1);
skouki 1:64871263444f 360 pid_x.setSetPoint(0.0);
skouki 1:64871263444f 361
skouki 1:64871263444f 362 // pid_y.reset();
skouki 1:64871263444f 363 // pid_y.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 364 // pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
skouki 1:64871263444f 365 // pid_y.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 366 // pid_y.setBias(0);
skouki 1:64871263444f 367 // pid_y.setMode(1);
skouki 1:64871263444f 368 // pid_y.setSetPoint(dai_low_y - gap);
skouki 1:64871263444f 369
skouki 1:64871263444f 370 pid_y.reset();
skouki 1:64871263444f 371 pid_y.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 372 pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
skouki 1:64871263444f 373 pid_y.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 374 pid_y.setBias(0);
skouki 1:64871263444f 375 pid_y.setMode(1);
skouki 1:64871263444f 376 pid_y.setSetPoint(m_to_u);
skouki 1:64871263444f 377
skouki 1:64871263444f 378 pid_spin.reset();
skouki 1:64871263444f 379 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 380 pid_spin.setInputLimits(-180.0,180.0);
skouki 1:64871263444f 381 pid_spin.setOutputLimits(-0.5,0.5);
skouki 1:64871263444f 382 pid_spin.setBias(0);
skouki 1:64871263444f 383 pid_spin.setMode(1);
skouki 1:64871263444f 384 pid_spin.setSetPoint(0.0);
skouki 1:64871263444f 385
skouki 1:64871263444f 386 mode = 5;
skouki 0:3ad208cbea5f 387 }
skouki 1:64871263444f 388 /*
skouki 1:64871263444f 389 if(m.getOutX() <= -1600&&mode == 5){
skouki 1:64871263444f 390
skouki 1:64871263444f 391 //gap = GAP - 50;
skouki 1:64871263444f 392
skouki 1:64871263444f 393 pid_x.reset();
skouki 1:64871263444f 394 pid_x.setTunings(1.0,1.0,0.000001);
skouki 1:64871263444f 395 pid_x.setInputLimits(-1000.0,1000.0);
skouki 1:64871263444f 396 pid_x.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 397 pid_x.setBias(0);
skouki 1:64871263444f 398 pid_x.setMode(1);
skouki 1:64871263444f 399 pid_x.setSetPoint(0.0);
skouki 1:64871263444f 400
skouki 1:64871263444f 401 pid_y.reset();
skouki 1:64871263444f 402 pid_y.setTunings(8.0,1.0,0.000001);
skouki 1:64871263444f 403 pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
skouki 1:64871263444f 404 pid_y.setOutputLimits(-1.0,1.0);
skouki 1:64871263444f 405 pid_y.setBias(0);
skouki 1:64871263444f 406 pid_y.setMode(1);
skouki 1:64871263444f 407 pid_y.setSetPoint(dai_low_y - gap);
skouki 1:64871263444f 408
skouki 1:64871263444f 409 pid_spin.reset();
skouki 1:64871263444f 410 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 1:64871263444f 411 pid_spin.setInputLimits(-180.0,180.0);
skouki 1:64871263444f 412 pid_spin.setOutputLimits(-0.5,0.5);
skouki 1:64871263444f 413 pid_spin.setBias(0);
skouki 1:64871263444f 414 pid_spin.setMode(1);
skouki 1:64871263444f 415 pid_spin.setSetPoint(0.0);
skouki 1:64871263444f 416
skouki 1:64871263444f 417 mode = 6;
skouki 1:64871263444f 418 }
skouki 1:64871263444f 419 */
skouki 1:64871263444f 420
skouki 1:64871263444f 421 if(an.read()==0)y_point = 0;
skouki 0:3ad208cbea5f 422
skouki 0:3ad208cbea5f 423 if(mode == 1)mode1();
skouki 0:3ad208cbea5f 424 if(mode == 2)mode2();
skouki 0:3ad208cbea5f 425 if(mode == 3)mode3();
skouki 0:3ad208cbea5f 426 if(mode == 4)mode4();
skouki 1:64871263444f 427 if(mode == 5)mode5();
skouki 1:64871263444f 428 if(mode == 6)mode6();
skouki 1:64871263444f 429 if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;}
skouki 0:3ad208cbea5f 430
skouki 0:3ad208cbea5f 431 omni.computeXY(Y_power,-X_power,-spin_power);
skouki 0:3ad208cbea5f 432
skouki 0:3ad208cbea5f 433 for(int i = 0; i < 4; i++){
skouki 0:3ad208cbea5f 434 omni_power[i] = 0.0;
skouki 0:3ad208cbea5f 435 omni_power[i] = omni.wheel[i];
skouki 0:3ad208cbea5f 436 motor[i].setSpeed(omni_power[i]);
skouki 0:3ad208cbea5f 437 }
skouki 1:64871263444f 438 //pc.printf("%.2f,%.2f,%d,%d,%.2f,%.2f\n\r",m.getOutX(),m.getOutY(),X_,Y_,m.getjyroAngle(),ir_distance);
skouki 1:64871263444f 439
skouki 0:3ad208cbea5f 440
skouki 0:3ad208cbea5f 441 }
skouki 0:3ad208cbea5f 442 }