three point omuni desu
Dependencies: Controller ikarashiMDC
Revision 1:a1ad958c4836, committed 2019-04-26
- Comitter:
- piroro4560
- Date:
- Fri Apr 26 08:27:30 2019 +0000
- Parent:
- 0:8f324e7a7491
- Commit message:
- three omuni
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8f324e7a7491 -r a1ad958c4836 main.cpp --- a/main.cpp Thu Mar 07 07:50:15 2019 +0000 +++ b/main.cpp Fri Apr 26 08:27:30 2019 +0000 @@ -1,7 +1,9 @@ -#include "mbed.h" +#include "mbed.h" //wheel test #include "ikarashiMDC.h" #include "controller.h" -Controller pad(PC_10, PC_11, 208); +#define PI 3.141593 + +Controller pad(PC_12, PD_2, 208); Serial pc(USBTX, USBRX, 115200); Serial serial(PC_6,PC_7); DigitalOut serialcontrol(D2); @@ -10,98 +12,73 @@ ikarashiMDC ikarashi[] { ikarashiMDC(&serialcontrol,2,0,SM,&serial), ikarashiMDC(&serialcontrol,2,1,SM,&serial), - ikarashiMDC(&serialcontrol,2,2,SM,&serial), - ikarashiMDC(&serialcontrol,2,3,SM,&serial) + ikarashiMDC(&serialcontrol,2,2,SM,&serial) }; int main() { serial.baud(115200); ikarashi[0].braking = true; - int button[11]; - float stickRadian[2],stickNorn[2]; - double acc[4]; + int b[11]; + float rad[2],dis[2]; + double value[3]; while(1){ - //コントローラー通信 + +/* + 7 9 + 8 10 + ------------- + 1 | | 3 + | | 2 4 + 0 --| |-- 5 + |12| |11| 6 + + +*/ + + ////stick button//// if(pad.receiveState()){ for(int i = 0; i < 13; i++){ - button[i] = pad.getButton1(i); - pc.printf("%d ", button[i]); + b[i] = pad.getButton1(i); } for(int i = 0; i < 2; i++){ - stickRadian[i] = pad.getRadian(i); - stickNorn[i] = pad.getNorm(i); - pc.printf("%f %f\t", stickRadian[i], stickNorn[i]); - } - pc.printf("\n\r"); - - //十字移動 - if(button[2] == 1 && button[3] == 0 && button[4] == 1 && button[5] == 1){ - stickNorn[0] = 1; - stickRadian[0] = 1.570796; - }else if(button[2] == 0 && button[3] == 1 && button[4] == 1 && button[5] == 1){ - stickNorn[0] = 1; - stickRadian[0] = 0; - }else if(button[2] == 1 && button[3] == 1 && button[4] == 0 && button[5] == 1){ - stickNorn[0] = 1; - stickRadian[0] = 3.141593; - }else if(button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 0){ - stickNorn[0] = 1; - stickRadian[0] = -1.570796; - }else if(stickNorn[0] == 0 && button[7] == 1 && button[9] == 1) { - acc[0] = 0; - acc[1] = 0; - acc[2] = 0; - } - //全方位移動 - if(stickNorn[0] != 0){ - acc[0] = sin(stickRadian[0] - PI * 11 / 6); - acc[1] = sin(stickRadian[0] - PI * 1 / 2); - acc[2] = sin(stickRadian[0] - PI * 7 / 6); + rad[i] = pad.getRadian(i); + dis[i] = pad.getNorm(i); + ////change stick value//// + rad[i] = PI - rad[i]; } - //左右旋回 - if(button[9] == 0 && button[7] == 1 && stickNorn[0] == 0) { - acc[0] = -0.5; - acc[1] = -0.5; - acc[2] = -0.5; - } - if(button[7] == 0 && button[9] == 1 && stickNorn[0] == 0) { - acc[0] = 0.5; - acc[1] = 0.5; - acc[2] = 0.5; - } - //機構制御 - if(button[10] == 0 && button[8] == 1){ - acc[3] = -0.4; - } - if(button[8] == 0 && button[10] == 1){ - acc[3] = 0.4; + + ////set speed//// + + value[0] = sin(rad[1] - PI * 1 / 6) * dis[1]; + value[1] = sin(rad[1] - PI * 5 / 6) * dis[1]; + value[2] = sin(rad[1] - PI * 9 / 6) * dis[1]; + + if(b[8] == 1 && b[10] == 0) { + value[0] -= 0.3; + value[1] -= 0.3; + value[2] -= 0.3; + } else if (b[10] == 1 && b[8] == 0) { + value[0] += 0.3; + value[1] += 0.3; + value[2] += 0.3; } - if(button[8] == button[10]) { - acc[3] = 0; - } - //減速 - if(button[0] == 0) { - for(int i = 0;i < 4;i++) { - if(acc[i] >= 0.5){ - acc[i] = 0.5; - } - if(acc[i] <= -0.5) { - acc[i] = -0.5; - } - } - } - pc.printf("%f %f %f %f",acc[0],acc[1],acc[2],acc[3]); - ikarashi[0].setSpeed(acc[0]); - ikarashi[1].setSpeed(acc[1]); - ikarashi[2].setSpeed(acc[2]); - ikarashi[3].setSpeed(acc[3]); + + pc.printf("%f", value[0]); + pc.printf("%f", value[1]); + pc.printf("%f", value[2]); + + pc.printf("\n\r"); + ikarashi[0].setSpeed(value[0]); + ikarashi[1].setSpeed(value[1]); + ikarashi[2].setSpeed(value[2]); + }else{ pc.printf("error\n\r"); - ikarashi[0].setSpeed(acc[0]); - ikarashi[1].setSpeed(0); - ikarashi[2].setSpeed(0); - ikarashi[3].setSpeed(0); + for (int i = 0; i < 3; i++) + { + ikarashi[i].setSpeed(0); + } } } }