hirromu takaki / Mbed 2 deprecated kourobo

kourobo 2019 tag number 5 program

Dependencies:   mbed omuni_power ikarashiMDC PS3

main.cpp@0:b2c626a3336d, 2019-02-28 (annotated)

Committer:
THtakahiro702286
Date:
Thu Feb 28 05:33:24 2019 +0000
Revision:
0:b2c626a3336d
Child:
1:6078ee91e5d7
kourobo 2019 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 0:b2c626a3336d 8 DigitalOut stop(PB_1,PB_15);
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 0:b2c626a3336d 17 PS3 ps3(PC_12, PD_2);
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 pc.printf("%4d",neutral[i]);
THtakahiro702286 0:b2c626a3336d 31 }
THtakahiro702286 0:b2c626a3336d 32 count++;
THtakahiro702286 0:b2c626a3336d 33 } while(count <= 50);
THtakahiro702286 0:b2c626a3336d 34 for(int i = 0; i < 4; i++) {
THtakahiro702286 0:b2c626a3336d 35 neutral[i] = ps3.getStick(i);
THtakahiro702286 0:b2c626a3336d 36 pc.printf("%4d",neutral[i]);
THtakahiro702286 0:b2c626a3336d 37 }
THtakahiro702286 0:b2c626a3336d 38 while(1){
THtakahiro702286 0:b2c626a3336d 39 stop = 1;
THtakahiro702286 0:b2c626a3336d 40 /*ボタン0=上　1=下 2=左　 3=右 4=L1 5=R1 6=△ 7=× 8=□ 9=〇 10=L3 11=R3*/
THtakahiro702286 0:b2c626a3336d 41 for(int i = 0; i < 12; i++) {
THtakahiro702286 0:b2c626a3336d 42 b[i] = ps3.getButton(i);
THtakahiro702286 0:b2c626a3336d 43 pc.printf("%2d",b[i]);
THtakahiro702286 0:b2c626a3336d 44 }
THtakahiro702286 0:b2c626a3336d 45 /*ジョイスティック*/
THtakahiro702286 0:b2c626a3336d 46 for(int i = 0; i < 4; i++) {
THtakahiro702286 0:b2c626a3336d 47 stick[i] = ps3.getStick(i);
THtakahiro702286 0:b2c626a3336d 48 pc.printf("%4d",stick[i] - neutral[i]);
THtakahiro702286 0:b2c626a3336d 49 }
THtakahiro702286 0:b2c626a3336d 50 /*トリガースイッチ*/
THtakahiro702286 0:b2c626a3336d 51 for(int i = 0; i < 2; i++) {
THtakahiro702286 0:b2c626a3336d 52 trigger[i] = ps3.getTrigger(i);
THtakahiro702286 0:b2c626a3336d 53 pc.printf("%4d",trigger[i]);
THtakahiro702286 0:b2c626a3336d 54 }
THtakahiro702286 0:b2c626a3336d 55 X = stick[0] - neutral[0];
THtakahiro702286 0:b2c626a3336d 56 Y = neutral[1] - stick[1];
THtakahiro702286 0:b2c626a3336d 57 /*微調整用コマンド*/
THtakahiro702286 0:b2c626a3336d 58 power = 1;
THtakahiro702286 0:b2c626a3336d 59 if(b[0] == 1 && b[1] == 0 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
THtakahiro702286 0:b2c626a3336d 60 X = 0;
THtakahiro702286 0:b2c626a3336d 61 Y = 1;
THtakahiro702286 0:b2c626a3336d 62 power = 0.2;
THtakahiro702286 0:b2c626a3336d 63 }
THtakahiro702286 0:b2c626a3336d 64 if(b[0] == 0 && b[1] == 1 && b[2] == 0 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
THtakahiro702286 0:b2c626a3336d 65 X = 0;
THtakahiro702286 0:b2c626a3336d 66 Y = -1;
THtakahiro702286 0:b2c626a3336d 67 power = 0.2;
THtakahiro702286 0:b2c626a3336d 68 }
THtakahiro702286 0:b2c626a3336d 69 if(b[0] == 0 && b[1] == 0 && b[2] == 1 && b[3] == 0 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
THtakahiro702286 0:b2c626a3336d 70 X = -1;
THtakahiro702286 0:b2c626a3336d 71 Y = 0;
THtakahiro702286 0:b2c626a3336d 72 power = 0.2;
THtakahiro702286 0:b2c626a3336d 73 }
THtakahiro702286 0:b2c626a3336d 74 if(b[0] == 0 && b[1] == 0 && b[2] == 0 && b[3] == 1 && (stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20)) {
THtakahiro702286 0:b2c626a3336d 75 X = 1;
THtakahiro702286 0:b2c626a3336d 76 Y = 0;
THtakahiro702286 0:b2c626a3336d 77 power = 0.2;
THtakahiro702286 0:b2c626a3336d 78 }
THtakahiro702286 0:b2c626a3336d 79 /*オムニ制御*/
THtakahiro702286 0:b2c626a3336d 80 if((stick[0] == 0 && stick[1] == 0 && stick[2] == 0 && stick [3] == 0) || ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
THtakahiro702286 0:b2c626a3336d 81 acc[0] = 0;
THtakahiro702286 0:b2c626a3336d 82 acc[1] = 0;
THtakahiro702286 0:b2c626a3336d 83 acc[2] = 0;
THtakahiro702286 0:b2c626a3336d 84 } else {
THtakahiro702286 0:b2c626a3336d 85 acc[0] = omuni_speed(X,Y,PI * 1 / 3) * power;
THtakahiro702286 0:b2c626a3336d 86 acc[1] = omuni_speed(X,Y,PI * 3 / 3) * power;
THtakahiro702286 0:b2c626a3336d 87 acc[2] = omuni_speed(X,Y,PI * 5 / 3) * power;
THtakahiro702286 0:b2c626a3336d 88 }
THtakahiro702286 0:b2c626a3336d 89 if((trigger[0] <= 200 && trigger[1] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
THtakahiro702286 0:b2c626a3336d 90 acc[0] = -0.5;
THtakahiro702286 0:b2c626a3336d 91 acc[1] = -0.5;
THtakahiro702286 0:b2c626a3336d 92 acc[2] = -0.5;
THtakahiro702286 0:b2c626a3336d 93 }
THtakahiro702286 0:b2c626a3336d 94 if((trigger[1] <= 200 && trigger[0] >= 200) && ((stick[0] >= neutral[0] - 20 && stick[0] <= neutral[0] + 20) && (stick[1]>= neutral[1] - 20 && stick[1] <= neutral[1] + 20))) {
THtakahiro702286 0:b2c626a3336d 95 acc[0] = 0.5;
THtakahiro702286 0:b2c626a3336d 96 acc[1] = 0.5;
THtakahiro702286 0:b2c626a3336d 97 acc[2] = 0.5;
THtakahiro702286 0:b2c626a3336d 98 }
THtakahiro702286 0:b2c626a3336d 99 /*機構制御*/
THtakahiro702286 0:b2c626a3336d 100 if(b[4] == 1 && b[5] == 0){
THtakahiro702286 0:b2c626a3336d 101 acc[3] = -0.2;
THtakahiro702286 0:b2c626a3336d 102 }
THtakahiro702286 0:b2c626a3336d 103 if(b[4] == 0 && b[5] == 1){
THtakahiro702286 0:b2c626a3336d 104 acc[3] = 0.2;
THtakahiro702286 0:b2c626a3336d 105 }
THtakahiro702286 0:b2c626a3336d 106 if(b[4] == 0 && b[5] == 0){
THtakahiro702286 0:b2c626a3336d 107 acc[3] = 0;
THtakahiro702286 0:b2c626a3336d 108 }
THtakahiro702286 0:b2c626a3336d 109 /*速度制限*/
THtakahiro702286 0:b2c626a3336d 110 for(int i = 0;i < 4;i++) {
THtakahiro702286 0:b2c626a3336d 111 if(acc[i] >= 0.5){
THtakahiro702286 0:b2c626a3336d 112 acc[i] = 0.5;
THtakahiro702286 0:b2c626a3336d 113 }
THtakahiro702286 0:b2c626a3336d 114 if(acc[i] <= -0.5) {
THtakahiro702286 0:b2c626a3336d 115 acc[i] = -0.5;
THtakahiro702286 0:b2c626a3336d 116 }
THtakahiro702286 0:b2c626a3336d 117 }
THtakahiro702286 0:b2c626a3336d 118 /*タイムアウト制御 & 非常停止*/
THtakahiro702286 0:b2c626a3336d 119 if((ps3.status == false) || (b[10] == 1 && b[11] == 1)) {
THtakahiro702286 0:b2c626a3336d 120 stop = 0;
THtakahiro702286 0:b2c626a3336d 121 }
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 for(int i = 0;i < 5;i++) {
THtakahiro702286 0:b2c626a3336d 128 pc.printf(" %f",acc[i]);
THtakahiro702286 0:b2c626a3336d 129 }
THtakahiro702286 0:b2c626a3336d 130 pc.printf("\r\n");
THtakahiro702286 0:b2c626a3336d 131 }
THtakahiro702286 0:b2c626a3336d 132 }