hirromu takaki / Mbed OS kourobo_controller_FEP209_208

fep209 208

Dependencies:   Controller ikarashiMDC

main.cpp@0:8f324e7a7491, 2019-03-07 (annotated)

Committer:
THtakahiro702286
Date:
Thu Mar 07 07:50:15 2019 +0000
Revision:
0:8f324e7a7491
fep209 208

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:8f324e7a7491 1 #include "mbed.h"
THtakahiro702286 0:8f324e7a7491 2 #include "ikarashiMDC.h"
THtakahiro702286 0:8f324e7a7491 3 #include "controller.h"
THtakahiro702286 0:8f324e7a7491 4 Controller pad(PC_10, PC_11, 208);
THtakahiro702286 0:8f324e7a7491 5 Serial pc(USBTX, USBRX, 115200);
THtakahiro702286 0:8f324e7a7491 6 Serial serial(PC_6,PC_7);
THtakahiro702286 0:8f324e7a7491 7 DigitalOut serialcontrol(D2);
THtakahiro702286 0:8f324e7a7491 8 DigitalOut stop(PB_12,PB_2);
THtakahiro702286 0:8f324e7a7491 9
THtakahiro702286 0:8f324e7a7491 10 ikarashiMDC ikarashi[] {
THtakahiro702286 0:8f324e7a7491 11 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
THtakahiro702286 0:8f324e7a7491 12 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
THtakahiro702286 0:8f324e7a7491 13 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
THtakahiro702286 0:8f324e7a7491 14 ikarashiMDC(&serialcontrol,2,3,SM,&serial)
THtakahiro702286 0:8f324e7a7491 15 };
THtakahiro702286 0:8f324e7a7491 16
THtakahiro702286 0:8f324e7a7491 17 int main()
THtakahiro702286 0:8f324e7a7491 18 {
THtakahiro702286 0:8f324e7a7491 19 serial.baud(115200);
THtakahiro702286 0:8f324e7a7491 20 ikarashi[0].braking = true;
THtakahiro702286 0:8f324e7a7491 21 int button[11];
THtakahiro702286 0:8f324e7a7491 22 float stickRadian[2],stickNorn[2];
THtakahiro702286 0:8f324e7a7491 23 double acc[4];
THtakahiro702286 0:8f324e7a7491 24 while(1){
THtakahiro702286 0:8f324e7a7491 25 //コントローラー通信
THtakahiro702286 0:8f324e7a7491 26 if(pad.receiveState()){
THtakahiro702286 0:8f324e7a7491 27 for(int i = 0; i < 13; i++){
THtakahiro702286 0:8f324e7a7491 28 button[i] = pad.getButton1(i);
THtakahiro702286 0:8f324e7a7491 29 pc.printf("%d ", button[i]);
THtakahiro702286 0:8f324e7a7491 30 }
THtakahiro702286 0:8f324e7a7491 31 for(int i = 0; i < 2; i++){
THtakahiro702286 0:8f324e7a7491 32 stickRadian[i] = pad.getRadian(i);
THtakahiro702286 0:8f324e7a7491 33 stickNorn[i] = pad.getNorm(i);
THtakahiro702286 0:8f324e7a7491 34 pc.printf("%f %f\t", stickRadian[i], stickNorn[i]);
THtakahiro702286 0:8f324e7a7491 35 }
THtakahiro702286 0:8f324e7a7491 36 pc.printf("\n\r");
THtakahiro702286 0:8f324e7a7491 37
THtakahiro702286 0:8f324e7a7491 38 //十字移動
THtakahiro702286 0:8f324e7a7491 39 if(button[2] == 1 && button[3] == 0 && button[4] == 1 && button[5] == 1){
THtakahiro702286 0:8f324e7a7491 40 stickNorn[0] = 1;
THtakahiro702286 0:8f324e7a7491 41 stickRadian[0] = 1.570796;
THtakahiro702286 0:8f324e7a7491 42 }else if(button[2] == 0 && button[3] == 1 && button[4] == 1 && button[5] == 1){
THtakahiro702286 0:8f324e7a7491 43 stickNorn[0] = 1;
THtakahiro702286 0:8f324e7a7491 44 stickRadian[0] = 0;
THtakahiro702286 0:8f324e7a7491 45 }else if(button[2] == 1 && button[3] == 1 && button[4] == 0 && button[5] == 1){
THtakahiro702286 0:8f324e7a7491 46 stickNorn[0] = 1;
THtakahiro702286 0:8f324e7a7491 47 stickRadian[0] = 3.141593;
THtakahiro702286 0:8f324e7a7491 48 }else if(button[2] == 1 && button[3] == 1 && button[4] == 1 && button[5] == 0){
THtakahiro702286 0:8f324e7a7491 49 stickNorn[0] = 1;
THtakahiro702286 0:8f324e7a7491 50 stickRadian[0] = -1.570796;
THtakahiro702286 0:8f324e7a7491 51 }else if(stickNorn[0] == 0 && button[7] == 1 && button[9] == 1) {
THtakahiro702286 0:8f324e7a7491 52 acc[0] = 0;
THtakahiro702286 0:8f324e7a7491 53 acc[1] = 0;
THtakahiro702286 0:8f324e7a7491 54 acc[2] = 0;
THtakahiro702286 0:8f324e7a7491 55 }
THtakahiro702286 0:8f324e7a7491 56 //全方位移動
THtakahiro702286 0:8f324e7a7491 57 if(stickNorn[0] != 0){
THtakahiro702286 0:8f324e7a7491 58 acc[0] = sin(stickRadian[0] - PI * 11 / 6);
THtakahiro702286 0:8f324e7a7491 59 acc[1] = sin(stickRadian[0] - PI * 1 / 2);
THtakahiro702286 0:8f324e7a7491 60 acc[2] = sin(stickRadian[0] - PI * 7 / 6);
THtakahiro702286 0:8f324e7a7491 61 }
THtakahiro702286 0:8f324e7a7491 62 //左右旋回
THtakahiro702286 0:8f324e7a7491 63 if(button[9] == 0 && button[7] == 1 && stickNorn[0] == 0) {
THtakahiro702286 0:8f324e7a7491 64 acc[0] = -0.5;
THtakahiro702286 0:8f324e7a7491 65 acc[1] = -0.5;
THtakahiro702286 0:8f324e7a7491 66 acc[2] = -0.5;
THtakahiro702286 0:8f324e7a7491 67 }
THtakahiro702286 0:8f324e7a7491 68 if(button[7] == 0 && button[9] == 1 && stickNorn[0] == 0) {
THtakahiro702286 0:8f324e7a7491 69 acc[0] = 0.5;
THtakahiro702286 0:8f324e7a7491 70 acc[1] = 0.5;
THtakahiro702286 0:8f324e7a7491 71 acc[2] = 0.5;
THtakahiro702286 0:8f324e7a7491 72 }
THtakahiro702286 0:8f324e7a7491 73 //機構制御
THtakahiro702286 0:8f324e7a7491 74 if(button[10] == 0 && button[8] == 1){
THtakahiro702286 0:8f324e7a7491 75 acc[3] = -0.4;
THtakahiro702286 0:8f324e7a7491 76 }
THtakahiro702286 0:8f324e7a7491 77 if(button[8] == 0 && button[10] == 1){
THtakahiro702286 0:8f324e7a7491 78 acc[3] = 0.4;
THtakahiro702286 0:8f324e7a7491 79 }
THtakahiro702286 0:8f324e7a7491 80 if(button[8] == button[10]) {
THtakahiro702286 0:8f324e7a7491 81 acc[3] = 0;
THtakahiro702286 0:8f324e7a7491 82 }
THtakahiro702286 0:8f324e7a7491 83 //減速
THtakahiro702286 0:8f324e7a7491 84 if(button[0] == 0) {
THtakahiro702286 0:8f324e7a7491 85 for(int i = 0;i < 4;i++) {
THtakahiro702286 0:8f324e7a7491 86 if(acc[i] >= 0.5){
THtakahiro702286 0:8f324e7a7491 87 acc[i] = 0.5;
THtakahiro702286 0:8f324e7a7491 88 }
THtakahiro702286 0:8f324e7a7491 89 if(acc[i] <= -0.5) {
THtakahiro702286 0:8f324e7a7491 90 acc[i] = -0.5;
THtakahiro702286 0:8f324e7a7491 91 }
THtakahiro702286 0:8f324e7a7491 92 }
THtakahiro702286 0:8f324e7a7491 93 }
THtakahiro702286 0:8f324e7a7491 94 pc.printf("%f %f %f %f",acc[0],acc[1],acc[2],acc[3]);
THtakahiro702286 0:8f324e7a7491 95 ikarashi[0].setSpeed(acc[0]);
THtakahiro702286 0:8f324e7a7491 96 ikarashi[1].setSpeed(acc[1]);
THtakahiro702286 0:8f324e7a7491 97 ikarashi[2].setSpeed(acc[2]);
THtakahiro702286 0:8f324e7a7491 98 ikarashi[3].setSpeed(acc[3]);
THtakahiro702286 0:8f324e7a7491 99 }else{
THtakahiro702286 0:8f324e7a7491 100 pc.printf("error\n\r");
THtakahiro702286 0:8f324e7a7491 101 ikarashi[0].setSpeed(acc[0]);
THtakahiro702286 0:8f324e7a7491 102 ikarashi[1].setSpeed(0);
THtakahiro702286 0:8f324e7a7491 103 ikarashi[2].setSpeed(0);
THtakahiro702286 0:8f324e7a7491 104 ikarashi[3].setSpeed(0);
THtakahiro702286 0:8f324e7a7491 105 }
THtakahiro702286 0:8f324e7a7491 106 }
THtakahiro702286 0:8f324e7a7491 107 }