kourobo
Dependencies: omuni_power ikarashiMDC PS3
Revision 0:bb6a0c792b11, committed 2019-03-06
- Comitter:
- THtakahiro702286
- Date:
- Wed Mar 06 12:03:00 2019 +0000
- Commit message:
- kourobo;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.gitignore Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,4 @@ +.build +.mbed +projectfiles +*.py*
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PS3.lib Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/PS3/#78827486d24f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC.lib Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC/#ea34af94e90c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,127 @@ +#include "mbed.h" +#include "ikarashiMDC.h" +#include "PS3.h" +#include "wheel.h" +Serial serial(PC_6,PC_7); +DigitalOut serialcontrol(D2); +DigitalOut stop(PB_12,PB_2); + +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) +}; + +PS3 ps3(PC_10, PC_11); +Serial pc(USBTX,USBRX,115200); + +int main() +{ + serial.baud(115200); + ikarashi[0].braking = true; + int b[12], stick[4], trigger[2],neutral[4],count = 0; + double X,Y,acc[5],power; + /*ジョイスティック ニュートラル設定*/ + do{ + for(int i = 0; i < 4; i++) { + neutral[i] = ps3.getStick(i); + } + count++; + } while(count <= 1152000); + for(int i = 0; i < 4; i++) { + neutral[i] = ps3.getStick(i); + } + while(1){ + stop = 1; + /*ボタン0=上 1=下 2=左 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/ + for(int i = 0; i < 12; i++) { + b[i] = ps3.getButton(i); + } + /*ジョイスティック*/ + for(int i = 0; i < 4; i++) { + stick[i] = ps3.getStick(i); + } + /*トリガースイッチ*/ + for(int i = 0; i < 2; i++) { + trigger[i] = ps3.getTrigger(i); + } + X = stick[0] - neutral[0]; + Y = neutral[1] - stick[1]; + if(abs(X) <= 20){ + X = 0; + } + if(abs(Y) <= 20) { + Y = 0; + } + /*微調整用コマンド*/ + power = 1; + if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) { + X = 0; + Y = 128; + } + if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) { + X = 0; + Y = -128; + } + if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && X == 0 && Y == 0) { + X = -128; + Y = 0; + } + if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && X == 0 && Y == 0) { + X = 128; + Y = 0; + } + /*オムニ制御*/ + if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || (X == 0 && Y == 0)) { + acc[0] = 0; + acc[1] = 0; + acc[2] = 0; + } else { + acc[0] = omuni_speed(X,Y,PI * 1 / 6) * power; + acc[1] = omuni_speed(X,Y,PI * 9 / 6) * power; + acc[2] = omuni_speed(X,Y,PI * 5 / 6) * power; + } + if((trigger[0] <= 200 && trigger[1] >= 200) && (X == 0 && Y == 0)) { + acc[0] = -0.5; + acc[1] = -0.5; + acc[2] = -0.5; + } + if((trigger[1] <= 200 && trigger[0] >= 200) && (X == 0 && Y == 0)) { + acc[0] = 0.5; + acc[1] = 0.5; + acc[2] = 0.5; + } + /*機構制御*/ + if(b[4] == 1 && b[5] == 0){ + acc[3] = 0.4; + } + if(b[4] == 0 && b[5] == 1){ + acc[3] = -0.4; + } + if(b[4] == 0 && b[5] == 0){ + acc[3] = 0; + } + /*速度制限*/ + if (b[7] == 1) { + 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; + } + } + } + /*タイムアウト制御*/ + if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) { + acc[0] = 0; + acc[1] = 0; + acc[2] = 0; + } + ikarashi[0].setSpeed(acc[0]); + ikarashi[1].setSpeed(acc[1]); + ikarashi[2].setSpeed(acc[2]); + ikarashi[3].setSpeed(acc[3]); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#c9e63f14085f5751ff5ead79a7c0382d50a813a2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni_power.lib Wed Mar 06 12:03:00 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/THtakahiro702286/code/omuni_power/#794a087f6216