wheel_test4_ver1
Dependencies: QEI omni_wheel PID R1370 Controller ikarashiMDC
Diff: main.cpp
- Revision:
- 3:4fe8e6e455d0
- Parent:
- 2:8d16db0d55b7
diff -r 8d16db0d55b7 -r 4fe8e6e455d0 main.cpp --- a/main.cpp Tue May 21 08:39:27 2019 +0000 +++ b/main.cpp Tue Jun 11 09:10:36 2019 +0000 @@ -6,7 +6,7 @@ #include "PID.h" #define PI 3.141593 -PID pid1(4.5,100.0,0.00000,0.01); +PID pid1(5.0,0.001,0.0001,0.01); OmniWheel omni(4); Controller pad(PC_10, PC_11, 208); R1370 R1370(PB_6,PA_10); @@ -29,8 +29,13 @@ { // reset = 1; motor[0].braking = true; - int b[11]; - double rad[2], dis[2], value[3], X, Y, set_value, original_angle, now_angle, start_angle; + int b[11], b2[11]; + double rad[2], dis[2], value[3], X, Y, original_angle, now_angle, start_angle, spin_power; + /* + * original_angle : + * now_angle : + * start_angle : + */ pid1.setInputLimits(-180,180); pid1.setOutputLimits(-0.3,0.3); pid1.setBias(0); @@ -43,7 +48,7 @@ while(1){ if(pad.receiveState()){ for(int i = 0; i < 13; i++){ - b[i] = pad.getButton1(i); + b[i] = 1 - pad.getButton1(i); } for(int i = 0; i < 2; i++){ rad[i] = pad.getRadian(i); @@ -53,35 +58,24 @@ X = dis[0] * cos(rad[0]); Y = dis[0] * sin(rad[0]); R1370.update(); - /* - ここまでテンプル - */ + /*ここまでテンプル*/ start_angle = R1370.getAngle(); /* 旋回 */ - if(b[7]==0)set_value++; - if(b[9]==0)set_value--; - set_value %= 4; - switch(((set_value & 4) + 4) & 4){ - case 1: original_angle = 90;break; - case 2: original_angle = 179;break; - case 3: original_angle = -90;break; - default: original_angle = 0;break; /* リセット */ - if(b[6] == 0){ - original_angle=angle; + if(b[6] == 1){ + original_angle=start_angle; } now_angle = start_angle - original_angle; - if(now_angle < -180)now_angle = now_angle + 360; pid1.setProcessValue(now_angle); - double spin_power = pid1.compute(); + spin_power = pid1.compute(); omni.computeXY(X,Y,-spin_power); pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power); for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]); - + for(int i = 0; i < 11; ++i)b2[i] = b[i]; }else{ pc.printf("error\n\r"); for (int i = 0; i < 4; i++)motor[i].setSpeed(0);