Diff: main.cpp

Revision:

0:5feeeb404a6a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Aug 09 13:37:22 2021 +0000
@@ -0,0 +1,112 @@
+#include "mbed.h"
+#include "PS_PAD.h"
+
+
+//入力pin設定
+PS_PAD vsc3(PA_7,PA_6,PA_5,PB_6);
+
+
+//出力pin設定
+//RIGHT
+PwmOut MRA (PA_11);
+PwmOut MRB (PB_13);
+//LEFT
+PwmOut MLA (PB_14);
+PwmOut MLB (PB_15);
+
+
+//SERVO
+PwmOut s1 (PC_9);//raise
+PwmOut s2 (PB_8);//grab
+PwmOut s3 (PB_9);//table
+
+
+//joystick
+int RY;
+int LY;
+
+double du;
+
+int main()
+{
+
+    vsc3.init();
+
+    MRA.period(0.002);
+    MRB.period(0.002);
+    MLA.period(0.002);
+    MLB.period(0.002);
+
+    s1.period_us(20000);
+    s2.period_us(20000);
+    s3.period_us(20000);
+
+
+    while(1) {
+
+        vsc3.poll();
+
+        RY = vsc3.read(PS_PAD::ANALOG_RY);
+        LY = vsc3.read(PS_PAD::ANALOG_LY);
+
+
+        //サーボ操作
+        if(vsc3.read(PS_PAD::PAD_TOP)) {
+            s1.pulsewidth_us(400);
+        } else if(vsc3.read(PS_PAD::PAD_BOTTOM)) {
+            s1.pulsewidth_us(1550);
+        } else if(vsc3.read(PS_PAD::PAD_RIGHT)) {
+            s2.pulsewidth_us(1700);
+        } else if(vsc3.read(PS_PAD::PAD_LEFT)) {
+            s2.pulsewidth_us(1350);
+        } else if(vsc3.read(PS_PAD::PAD_TRIANGLE)) {
+            s3.pulsewidth_us(1550);
+        } else if(vsc3.read(PS_PAD::PAD_X)) {
+            s3.pulsewidth_us(1900);
+        } else;
+
+        //速度設定
+        if(vsc3.read(PS_PAD::PAD_L1)) {
+            du = 0.2;
+        } else if(vsc3.read(PS_PAD::PAD_R1)) {
+            du = 0.99;
+        } else {
+            du = 0.4;
+        }
+        
+        
+        wait(0.05);
+
+
+        //右車輪
+        if (-25 < RY < 25) {
+            MRA = 0;
+            MRB = 0;
+        }
+        if(RY < -25) {
+            MRA = du;
+            MRB = 0;
+        }
+        if(25 < RY) {
+            MRA = 0;
+            MRB = du;
+        }
+
+
+        //左車輪
+        if (-25 < LY < 25) {
+            MLA = 0;
+            MLB = 0;
+        }
+        if(LY < -25) {
+            MLA = du;
+            MLB = 0;
+        }
+        if(25 < LY) {
+            MLA = 0;
+            MLB = du;
+        }
+
+    }
+
+}
\ No newline at end of file