Diff: main.cpp

Revision:

0:b63ffda48275

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 21 15:13:43 2020 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+Serial pc(USBTX,USBRX,9600);
+
+AnalogIn y2(A6);
+AnalogIn x2(A5);
+AnalogIn y1(A4);
+AnalogIn x1(A3);
+DigitalIn SW(D0);
+//上下モータ
+PwmOut UDS(A1);
+DigitalOut UDP(A0);
+
+//左前モータ
+PwmOut MS1(D7);
+DigitalOut MP1(D6);
+
+//右前モータ
+PwmOut MS2(D10);
+DigitalOut MP2(D9);
+
+//左後ろモータ
+PwmOut MS3(D12);
+DigitalOut MP3(D11);
+
+//右後ろモータ
+PwmOut MS4(A2);
+DigitalOut MP4(D2);
+
+//ハンド開閉
+PwmOut HAND(D1);
+
+int count=0;//ここ怪しい
+
+int main()
+{
+    int sw,pre_sw;
+    while(1) {
+        double Y2 = y2.read();
+        double X2 = x2.read();
+        double Y1 = y1.read();
+        double X1= x1.read();
+        sw=SW.read();
+
+        if(sw<pre_sw) {
+            count++;
+        }
+
+        pre_sw=sw;
+        //駆動系
+        if(X1>0.49 && X1<0.51 && Y1>=0.6) { //
+            MS1=Y1*2.5-1.5;
+            MP1=1.0;
+            MS2=Y1*2.5-1.5;
+            MP2=0.0;
+            MS3=Y1*2.5-1.5;
+            MP3=0.0;
+            MS4=Y1*2.5-1.5;
+            MP4=0.0;
+        } else if(X1>0.49 && X1<0.51 && Y1<0.4) { //
+            MS1=-2.5*Y1+1;
+            MP1=0.0;
+            MS2=-2.5*Y1+1;
+            MP2=1.0;
+            MS3=-2.5*Y1+1;
+            MP3=1.0;
+            MS4=-2.5*Y1+1;
+            MP4=1.0;
+
+        } else if(X1>0.70 && Y1>0.49 && Y1<0.51 ) {
+            MS1=3.3*X1-2.6;
+            MP1=0.0;
+            MS2=3.3*x1-2.6;
+            MP2=0.0;
+            MS3=3.3*X1-2.6;
+            MP3=1.0;
+            MS4=3.3*X1-2.6;
+            MP4=1.0;
+
+        } else if(X1<0.30 && Y1>0.49 && Y1<0.51) {
+            MS1=-3.3*X1+1;
+            MP1=1.0;
+            MS2=-3.3*X1+1;
+            MP2=1.0;
+            MS3=-3.3*X1+1;
+            MP3=0.0;
+            MS4=-3.3*X1+1;
+            MP4=0.0;
+        } 
+        //////////////////////////////////////
+        else if(X2>0.70 && Y2>0.49 && Y2<0.51 ) {
+            MS1=3.3*X2-2.6;
+            MP1=0.0;
+            MS2=3.3*x2-2.6;
+            MP2=0.0;
+            MS3=3.3*X2-2.6;
+            MP3=0.0;
+            MS4=3.3*X2-2.6;
+            MP4=0.0;
+
+        }
+
+        else if(X2<0.30 && Y2>0.49 && Y2<0.51) {
+            MS1=-3.3*X2+1;
+            MP1=1.0;
+            MS2=-3.3*X2+1;
+            MP2=1.0;
+            MS3=-3.3*X2+1;
+            MP3=1.0;
+            MS4=-3.3*X2+1;
+            MP4=1.0;
+        } 
+        /////////////////////////////////////
+        else {
+            MS1=0.0;
+            MP1=0.0;
+            MS2=0.0;
+            MP2=0.0;
+            MS3=0.0;
+            MP3=0.0;
+            MS4=0.0;
+            MP4=0.0;
+
+        }
+        //pc.printf("%lf\r\n",Y2);
+
+
+//ハンド開閉
+        if(count%2==1) {
+            HAND.pulsewidth(0.00145);//閉じる
+        } else {
+            HAND.pulsewidth(0.002);//開
+        }
+        if(Y2<0.2) {
+            UDS=1.0;
+            UDP=0.0;
+            pc.printf("B");
+        } else if(Y2>0.9) {
+            UDS=1.0;
+            UDP=1.0;
+            pc.printf("A");
+        } else  {
+            UDS=0.0;
+            UDP=0.0;
+
+        }
+        pre_sw=sw;
+
+    }
+}