Diff: main.cpp

Revision:

0:5b4d5f6c4a68

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Sep 15 06:22:57 2019 +0000
@@ -0,0 +1,71 @@
+#include "mbed.h"
+#include "QEI.h"
+
+
+PwmOut MRA (PB_15);
+PwmOut MRB (PB_14);
+//Right
+PwmOut MLA (PB_13);
+PwmOut MLB (PA_11);
+//Left
+
+PwmOut s1 (PB_8);//包容力
+PwmOut s2 (PB_9);//raise
+
+BusIn Encoder(PA_6,PA_7,PA_8,PA_9);
+
+QEI Encoder_R(PA_6,PA_7,NC,1000,QEI::X2_ENCODING);
+QEI Encoder_L(PA_8,PA_9,NC,1000,QEI::X2_ENCODING);
+
+int main()
+{
+    MRA.period(0.002);
+    MRB.period(0.002);
+    MLA.period(0.002);
+    MLB.period(0.002);
+
+    s1.period_us(20000);
+    s2.period_us(20000);
+    
+    int OldEnc=0;
+
+
+    while(1) {
+
+        Encoder.mode(PullUp);
+        //printf("%d %d\r\n",Encoder_R.getPulses(),-1*Encoder_L.getPulses());
+
+
+        if(Encoder_R.getPulses() < 42024) {
+            MRA = 0.4;
+            MLA = 0.407;
+        } else if (Encoder_R.getPulses() >= 42024) {
+            MRA = 0;
+            MLA = 0;
+
+            wait (1.0);
+
+            s1.pulsewidth_us(1700);
+            wait(1);
+
+            /*MRA=0;
+            MLA=0;*/
+            MRB=0.4;
+            MLB=0.407;
+
+        if(Encoder_R.getPulses() <= 42024-1200) {
+
+            s2.pulsewidth_us(1300);
+            MRB=0;
+            MLB=0;
+            wait(1);
+            MRA=0;
+            MLA=0;
+            break;
+            }
+
+        } else;
+
+
+    }
+}
\ No newline at end of file