hirromu takaki / Mbed 2 deprecated kourobo

kourobo 2019 tag number 5 program

Dependencies:   mbed omuni_power ikarashiMDC PS3

main.cpp@1:6078ee91e5d7, 2019-03-05 (annotated)

Committer:
THtakahiro702286
Date:
Tue Mar 05 13:00:32 2019 +0000
Revision:
1:6078ee91e5d7
Parent:
0:b2c626a3336d 
kourobo program

Who changed what in which revision?

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