Kobayashi Akihiro / Mbed 2 deprecated KATAYANAGI_ROBOCON_2

第1回片柳ロボコンのプログラム

Dependencies:   mbed

main.cpp@0:b63ffda48275, 2020-03-21 (annotated)

Committer:
e5119053f6
Date:
Sat Mar 21 15:13:43 2020 +0000
Revision:
0:b63ffda48275
katayanagi_robocon

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:b63ffda48275 1 #include "mbed.h"
e5119053f6 0:b63ffda48275 2 Serial pc(USBTX,USBRX,9600);
e5119053f6 0:b63ffda48275 3
e5119053f6 0:b63ffda48275 4 AnalogIn y2(A6);
e5119053f6 0:b63ffda48275 5 AnalogIn x2(A5);
e5119053f6 0:b63ffda48275 6 AnalogIn y1(A4);
e5119053f6 0:b63ffda48275 7 AnalogIn x1(A3);
e5119053f6 0:b63ffda48275 8 DigitalIn SW(D0);
e5119053f6 0:b63ffda48275 9 //上下モータ
e5119053f6 0:b63ffda48275 10 PwmOut UDS(A1);
e5119053f6 0:b63ffda48275 11 DigitalOut UDP(A0);
e5119053f6 0:b63ffda48275 12
e5119053f6 0:b63ffda48275 13 //左前モータ
e5119053f6 0:b63ffda48275 14 PwmOut MS1(D7);
e5119053f6 0:b63ffda48275 15 DigitalOut MP1(D6);
e5119053f6 0:b63ffda48275 16
e5119053f6 0:b63ffda48275 17 //右前モータ
e5119053f6 0:b63ffda48275 18 PwmOut MS2(D10);
e5119053f6 0:b63ffda48275 19 DigitalOut MP2(D9);
e5119053f6 0:b63ffda48275 20
e5119053f6 0:b63ffda48275 21 //左後ろモータ
e5119053f6 0:b63ffda48275 22 PwmOut MS3(D12);
e5119053f6 0:b63ffda48275 23 DigitalOut MP3(D11);
e5119053f6 0:b63ffda48275 24
e5119053f6 0:b63ffda48275 25 //右後ろモータ
e5119053f6 0:b63ffda48275 26 PwmOut MS4(A2);
e5119053f6 0:b63ffda48275 27 DigitalOut MP4(D2);
e5119053f6 0:b63ffda48275 28
e5119053f6 0:b63ffda48275 29 //ハンド開閉
e5119053f6 0:b63ffda48275 30 PwmOut HAND(D1);
e5119053f6 0:b63ffda48275 31
e5119053f6 0:b63ffda48275 32 int count=0;//ここ怪しい
e5119053f6 0:b63ffda48275 33
e5119053f6 0:b63ffda48275 34 int main()
e5119053f6 0:b63ffda48275 35 {
e5119053f6 0:b63ffda48275 36 int sw,pre_sw;
e5119053f6 0:b63ffda48275 37 while(1) {
e5119053f6 0:b63ffda48275 38 double Y2 = y2.read();
e5119053f6 0:b63ffda48275 39 double X2 = x2.read();
e5119053f6 0:b63ffda48275 40 double Y1 = y1.read();
e5119053f6 0:b63ffda48275 41 double X1= x1.read();
e5119053f6 0:b63ffda48275 42 sw=SW.read();
e5119053f6 0:b63ffda48275 43
e5119053f6 0:b63ffda48275 44 if(sw<pre_sw) {
e5119053f6 0:b63ffda48275 45 count++;
e5119053f6 0:b63ffda48275 46 }
e5119053f6 0:b63ffda48275 47
e5119053f6 0:b63ffda48275 48 pre_sw=sw;
e5119053f6 0:b63ffda48275 49 //駆動系
e5119053f6 0:b63ffda48275 50 if(X1>0.49 && X1<0.51 && Y1>=0.6) { //
e5119053f6 0:b63ffda48275 51 MS1=Y1*2.5-1.5;
e5119053f6 0:b63ffda48275 52 MP1=1.0;
e5119053f6 0:b63ffda48275 53 MS2=Y1*2.5-1.5;
e5119053f6 0:b63ffda48275 54 MP2=0.0;
e5119053f6 0:b63ffda48275 55 MS3=Y1*2.5-1.5;
e5119053f6 0:b63ffda48275 56 MP3=0.0;
e5119053f6 0:b63ffda48275 57 MS4=Y1*2.5-1.5;
e5119053f6 0:b63ffda48275 58 MP4=0.0;
e5119053f6 0:b63ffda48275 59 } else if(X1>0.49 && X1<0.51 && Y1<0.4) { //
e5119053f6 0:b63ffda48275 60 MS1=-2.5*Y1+1;
e5119053f6 0:b63ffda48275 61 MP1=0.0;
e5119053f6 0:b63ffda48275 62 MS2=-2.5*Y1+1;
e5119053f6 0:b63ffda48275 63 MP2=1.0;
e5119053f6 0:b63ffda48275 64 MS3=-2.5*Y1+1;
e5119053f6 0:b63ffda48275 65 MP3=1.0;
e5119053f6 0:b63ffda48275 66 MS4=-2.5*Y1+1;
e5119053f6 0:b63ffda48275 67 MP4=1.0;
e5119053f6 0:b63ffda48275 68
e5119053f6 0:b63ffda48275 69 } else if(X1>0.70 && Y1>0.49 && Y1<0.51 ) {
e5119053f6 0:b63ffda48275 70 MS1=3.3*X1-2.6;
e5119053f6 0:b63ffda48275 71 MP1=0.0;
e5119053f6 0:b63ffda48275 72 MS2=3.3*x1-2.6;
e5119053f6 0:b63ffda48275 73 MP2=0.0;
e5119053f6 0:b63ffda48275 74 MS3=3.3*X1-2.6;
e5119053f6 0:b63ffda48275 75 MP3=1.0;
e5119053f6 0:b63ffda48275 76 MS4=3.3*X1-2.6;
e5119053f6 0:b63ffda48275 77 MP4=1.0;
e5119053f6 0:b63ffda48275 78
e5119053f6 0:b63ffda48275 79 } else if(X1<0.30 && Y1>0.49 && Y1<0.51) {
e5119053f6 0:b63ffda48275 80 MS1=-3.3*X1+1;
e5119053f6 0:b63ffda48275 81 MP1=1.0;
e5119053f6 0:b63ffda48275 82 MS2=-3.3*X1+1;
e5119053f6 0:b63ffda48275 83 MP2=1.0;
e5119053f6 0:b63ffda48275 84 MS3=-3.3*X1+1;
e5119053f6 0:b63ffda48275 85 MP3=0.0;
e5119053f6 0:b63ffda48275 86 MS4=-3.3*X1+1;
e5119053f6 0:b63ffda48275 87 MP4=0.0;
e5119053f6 0:b63ffda48275 88 }
e5119053f6 0:b63ffda48275 89 //////////////////////////////////////
e5119053f6 0:b63ffda48275 90 else if(X2>0.70 && Y2>0.49 && Y2<0.51 ) {
e5119053f6 0:b63ffda48275 91 MS1=3.3*X2-2.6;
e5119053f6 0:b63ffda48275 92 MP1=0.0;
e5119053f6 0:b63ffda48275 93 MS2=3.3*x2-2.6;
e5119053f6 0:b63ffda48275 94 MP2=0.0;
e5119053f6 0:b63ffda48275 95 MS3=3.3*X2-2.6;
e5119053f6 0:b63ffda48275 96 MP3=0.0;
e5119053f6 0:b63ffda48275 97 MS4=3.3*X2-2.6;
e5119053f6 0:b63ffda48275 98 MP4=0.0;
e5119053f6 0:b63ffda48275 99
e5119053f6 0:b63ffda48275 100 }
e5119053f6 0:b63ffda48275 101
e5119053f6 0:b63ffda48275 102 else if(X2<0.30 && Y2>0.49 && Y2<0.51) {
e5119053f6 0:b63ffda48275 103 MS1=-3.3*X2+1;
e5119053f6 0:b63ffda48275 104 MP1=1.0;
e5119053f6 0:b63ffda48275 105 MS2=-3.3*X2+1;
e5119053f6 0:b63ffda48275 106 MP2=1.0;
e5119053f6 0:b63ffda48275 107 MS3=-3.3*X2+1;
e5119053f6 0:b63ffda48275 108 MP3=1.0;
e5119053f6 0:b63ffda48275 109 MS4=-3.3*X2+1;
e5119053f6 0:b63ffda48275 110 MP4=1.0;
e5119053f6 0:b63ffda48275 111 }
e5119053f6 0:b63ffda48275 112 /////////////////////////////////////
e5119053f6 0:b63ffda48275 113 else {
e5119053f6 0:b63ffda48275 114 MS1=0.0;
e5119053f6 0:b63ffda48275 115 MP1=0.0;
e5119053f6 0:b63ffda48275 116 MS2=0.0;
e5119053f6 0:b63ffda48275 117 MP2=0.0;
e5119053f6 0:b63ffda48275 118 MS3=0.0;
e5119053f6 0:b63ffda48275 119 MP3=0.0;
e5119053f6 0:b63ffda48275 120 MS4=0.0;
e5119053f6 0:b63ffda48275 121 MP4=0.0;
e5119053f6 0:b63ffda48275 122
e5119053f6 0:b63ffda48275 123 }
e5119053f6 0:b63ffda48275 124 //pc.printf("%lf\r\n",Y2);
e5119053f6 0:b63ffda48275 125
e5119053f6 0:b63ffda48275 126
e5119053f6 0:b63ffda48275 127 //ハンド開閉
e5119053f6 0:b63ffda48275 128 if(count%2==1) {
e5119053f6 0:b63ffda48275 129 HAND.pulsewidth(0.00145);//閉じる
e5119053f6 0:b63ffda48275 130 } else {
e5119053f6 0:b63ffda48275 131 HAND.pulsewidth(0.002);//開
e5119053f6 0:b63ffda48275 132 }
e5119053f6 0:b63ffda48275 133 if(Y2<0.2) {
e5119053f6 0:b63ffda48275 134 UDS=1.0;
e5119053f6 0:b63ffda48275 135 UDP=0.0;
e5119053f6 0:b63ffda48275 136 pc.printf("B");
e5119053f6 0:b63ffda48275 137 } else if(Y2>0.9) {
e5119053f6 0:b63ffda48275 138 UDS=1.0;
e5119053f6 0:b63ffda48275 139 UDP=1.0;
e5119053f6 0:b63ffda48275 140 pc.printf("A");
e5119053f6 0:b63ffda48275 141 } else {
e5119053f6 0:b63ffda48275 142 UDS=0.0;
e5119053f6 0:b63ffda48275 143 UDP=0.0;
e5119053f6 0:b63ffda48275 144
e5119053f6 0:b63ffda48275 145 }
e5119053f6 0:b63ffda48275 146 pre_sw=sw;
e5119053f6 0:b63ffda48275 147
e5119053f6 0:b63ffda48275 148 }
e5119053f6 0:b63ffda48275 149 }