kourobo 2019 tag number 5 program
Dependencies: mbed omuni_power ikarashiMDC PS3
main.cpp
- Committer:
- THtakahiro702286
- Date:
- 2019-03-05
- Revision:
- 1:6078ee91e5d7
- Parent:
- 0:b2c626a3336d
File content as of revision 1:6078ee91e5d7:
#include "mbed.h" #include "ikarashiMDC.h" #include "PS3.h" #include "wheel.h" #define PI 3.14159265 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]); } }