hirromu takaki / Mbed OS kourobo5

kourobo

Dependencies:   omuni_power ikarashiMDC PS3

main.cpp@0:bb6a0c792b11, 2019-03-06 (annotated)

Committer:
THtakahiro702286
Date:
Wed Mar 06 12:03:00 2019 +0000
Revision:
0:bb6a0c792b11
kourobo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:bb6a0c792b11 1 #include "mbed.h"
THtakahiro702286 0:bb6a0c792b11 2 #include "ikarashiMDC.h"
THtakahiro702286 0:bb6a0c792b11 3 #include "PS3.h"
THtakahiro702286 0:bb6a0c792b11 4 #include "wheel.h"
THtakahiro702286 0:bb6a0c792b11 5 Serial serial(PC_6,PC_7);
THtakahiro702286 0:bb6a0c792b11 6 DigitalOut serialcontrol(D2);
THtakahiro702286 0:bb6a0c792b11 7 DigitalOut stop(PB_12,PB_2);
THtakahiro702286 0:bb6a0c792b11 8
THtakahiro702286 0:bb6a0c792b11 9 ikarashiMDC ikarashi[] {
THtakahiro702286 0:bb6a0c792b11 10 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
THtakahiro702286 0:bb6a0c792b11 11 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
THtakahiro702286 0:bb6a0c792b11 12 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
THtakahiro702286 0:bb6a0c792b11 13 ikarashiMDC(&serialcontrol,2,3,SM,&serial)
THtakahiro702286 0:bb6a0c792b11 14 };
THtakahiro702286 0:bb6a0c792b11 15
THtakahiro702286 0:bb6a0c792b11 16 PS3 ps3(PC_10, PC_11);
THtakahiro702286 0:bb6a0c792b11 17 Serial pc(USBTX,USBRX,115200);
THtakahiro702286 0:bb6a0c792b11 18
THtakahiro702286 0:bb6a0c792b11 19 int main()
THtakahiro702286 0:bb6a0c792b11 20 {
THtakahiro702286 0:bb6a0c792b11 21 serial.baud(115200);
THtakahiro702286 0:bb6a0c792b11 22 ikarashi[0].braking = true;
THtakahiro702286 0:bb6a0c792b11 23 int b[12], stick[4], trigger[2],neutral[4],count = 0;
THtakahiro702286 0:bb6a0c792b11 24 double X,Y,acc[5],power;
THtakahiro702286 0:bb6a0c792b11 25 /*ジョイスティック ニュートラル設定*/
THtakahiro702286 0:bb6a0c792b11 26 do{
THtakahiro702286 0:bb6a0c792b11 27 for(int i = 0; i < 4; i++) {
THtakahiro702286 0:bb6a0c792b11 28 neutral[i] = ps3.getStick(i);
THtakahiro702286 0:bb6a0c792b11 29 }
THtakahiro702286 0:bb6a0c792b11 30 count++;
THtakahiro702286 0:bb6a0c792b11 31 } while(count <= 1152000);
THtakahiro702286 0:bb6a0c792b11 32 for(int i = 0; i < 4; i++) {
THtakahiro702286 0:bb6a0c792b11 33 neutral[i] = ps3.getStick(i);
THtakahiro702286 0:bb6a0c792b11 34 }
THtakahiro702286 0:bb6a0c792b11 35 while(1){
THtakahiro702286 0:bb6a0c792b11 36 stop = 1;
THtakahiro702286 0:bb6a0c792b11 37 /*ボタン0=上　1=下 2=左　 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/
THtakahiro702286 0:bb6a0c792b11 38 for(int i = 0; i < 12; i++) {
THtakahiro702286 0:bb6a0c792b11 39 b[i] = ps3.getButton(i);
THtakahiro702286 0:bb6a0c792b11 40 }
THtakahiro702286 0:bb6a0c792b11 41 /*ジョイスティック*/
THtakahiro702286 0:bb6a0c792b11 42 for(int i = 0; i < 4; i++) {
THtakahiro702286 0:bb6a0c792b11 43 stick[i] = ps3.getStick(i);
THtakahiro702286 0:bb6a0c792b11 44 }
THtakahiro702286 0:bb6a0c792b11 45 /*トリガースイッチ*/
THtakahiro702286 0:bb6a0c792b11 46 for(int i = 0; i < 2; i++) {
THtakahiro702286 0:bb6a0c792b11 47 trigger[i] = ps3.getTrigger(i);
THtakahiro702286 0:bb6a0c792b11 48 }
THtakahiro702286 0:bb6a0c792b11 49 X = stick[0] - neutral[0];
THtakahiro702286 0:bb6a0c792b11 50 Y = neutral[1] - stick[1];
THtakahiro702286 0:bb6a0c792b11 51 if(abs(X) <= 20){
THtakahiro702286 0:bb6a0c792b11 52 X = 0;
THtakahiro702286 0:bb6a0c792b11 53 }
THtakahiro702286 0:bb6a0c792b11 54 if(abs(Y) <= 20) {
THtakahiro702286 0:bb6a0c792b11 55 Y = 0;
THtakahiro702286 0:bb6a0c792b11 56 }
THtakahiro702286 0:bb6a0c792b11 57 /*微調整用コマンド*/
THtakahiro702286 0:bb6a0c792b11 58 power = 1;
THtakahiro702286 0:bb6a0c792b11 59 if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
THtakahiro702286 0:bb6a0c792b11 60 X = 0;
THtakahiro702286 0:bb6a0c792b11 61 Y = 128;
THtakahiro702286 0:bb6a0c792b11 62 }
THtakahiro702286 0:bb6a0c792b11 63 if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && X == 0 && Y == 0) {
THtakahiro702286 0:bb6a0c792b11 64 X = 0;
THtakahiro702286 0:bb6a0c792b11 65 Y = -128;
THtakahiro702286 0:bb6a0c792b11 66 }
THtakahiro702286 0:bb6a0c792b11 67 if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && X == 0 && Y == 0) {
THtakahiro702286 0:bb6a0c792b11 68 X = -128;
THtakahiro702286 0:bb6a0c792b11 69 Y = 0;
THtakahiro702286 0:bb6a0c792b11 70 }
THtakahiro702286 0:bb6a0c792b11 71 if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && X == 0 && Y == 0) {
THtakahiro702286 0:bb6a0c792b11 72 X = 128;
THtakahiro702286 0:bb6a0c792b11 73 Y = 0;
THtakahiro702286 0:bb6a0c792b11 74 }
THtakahiro702286 0:bb6a0c792b11 75 /*オムニ制御*/
THtakahiro702286 0:bb6a0c792b11 76 if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || (X == 0 && Y == 0)) {
THtakahiro702286 0:bb6a0c792b11 77 acc[0] = 0;
THtakahiro702286 0:bb6a0c792b11 78 acc[1] = 0;
THtakahiro702286 0:bb6a0c792b11 79 acc[2] = 0;
THtakahiro702286 0:bb6a0c792b11 80 } else {
THtakahiro702286 0:bb6a0c792b11 81 acc[0] = omuni_speed(X,Y,PI * 1 / 6) * power;
THtakahiro702286 0:bb6a0c792b11 82 acc[1] = omuni_speed(X,Y,PI * 9 / 6) * power;
THtakahiro702286 0:bb6a0c792b11 83 acc[2] = omuni_speed(X,Y,PI * 5 / 6) * power;
THtakahiro702286 0:bb6a0c792b11 84 }
THtakahiro702286 0:bb6a0c792b11 85 if((trigger[0] <= 200 && trigger[1] >= 200) && (X == 0 && Y == 0)) {
THtakahiro702286 0:bb6a0c792b11 86 acc[0] = -0.5;
THtakahiro702286 0:bb6a0c792b11 87 acc[1] = -0.5;
THtakahiro702286 0:bb6a0c792b11 88 acc[2] = -0.5;
THtakahiro702286 0:bb6a0c792b11 89 }
THtakahiro702286 0:bb6a0c792b11 90 if((trigger[1] <= 200 && trigger[0] >= 200) && (X == 0 && Y == 0)) {
THtakahiro702286 0:bb6a0c792b11 91 acc[0] = 0.5;
THtakahiro702286 0:bb6a0c792b11 92 acc[1] = 0.5;
THtakahiro702286 0:bb6a0c792b11 93 acc[2] = 0.5;
THtakahiro702286 0:bb6a0c792b11 94 }
THtakahiro702286 0:bb6a0c792b11 95 /*機構制御*/
THtakahiro702286 0:bb6a0c792b11 96 if(b[4] == 1 && b[5] == 0){
THtakahiro702286 0:bb6a0c792b11 97 acc[3] = 0.4;
THtakahiro702286 0:bb6a0c792b11 98 }
THtakahiro702286 0:bb6a0c792b11 99 if(b[4] == 0 && b[5] == 1){
THtakahiro702286 0:bb6a0c792b11 100 acc[3] = -0.4;
THtakahiro702286 0:bb6a0c792b11 101 }
THtakahiro702286 0:bb6a0c792b11 102 if(b[4] == 0 && b[5] == 0){
THtakahiro702286 0:bb6a0c792b11 103 acc[3] = 0;
THtakahiro702286 0:bb6a0c792b11 104 }
THtakahiro702286 0:bb6a0c792b11 105 /*速度制限*/
THtakahiro702286 0:bb6a0c792b11 106 if (b[7] == 1) {
THtakahiro702286 0:bb6a0c792b11 107 for(int i = 0;i < 4;i++) {
THtakahiro702286 0:bb6a0c792b11 108 if(acc[i] >= 0.5){
THtakahiro702286 0:bb6a0c792b11 109 acc[i] = 0.5;
THtakahiro702286 0:bb6a0c792b11 110 }
THtakahiro702286 0:bb6a0c792b11 111 if(acc[i] <= -0.5) {
THtakahiro702286 0:bb6a0c792b11 112 acc[i] = -0.5;
THtakahiro702286 0:bb6a0c792b11 113 }
THtakahiro702286 0:bb6a0c792b11 114 }
THtakahiro702286 0:bb6a0c792b11 115 }
THtakahiro702286 0:bb6a0c792b11 116 /*タイムアウト制御*/
THtakahiro702286 0:bb6a0c792b11 117 if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) {
THtakahiro702286 0:bb6a0c792b11 118 acc[0] = 0;
THtakahiro702286 0:bb6a0c792b11 119 acc[1] = 0;
THtakahiro702286 0:bb6a0c792b11 120 acc[2] = 0;
THtakahiro702286 0:bb6a0c792b11 121 }
THtakahiro702286 0:bb6a0c792b11 122 ikarashi[0].setSpeed(acc[0]);
THtakahiro702286 0:bb6a0c792b11 123 ikarashi[1].setSpeed(acc[1]);
THtakahiro702286 0:bb6a0c792b11 124 ikarashi[2].setSpeed(acc[2]);
THtakahiro702286 0:bb6a0c792b11 125 ikarashi[3].setSpeed(acc[3]);
THtakahiro702286 0:bb6a0c792b11 126 }
THtakahiro702286 0:bb6a0c792b11 127 }