Diff: main.cpp

Revision:

0:d4deb703d536

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 16 11:17:34 2019 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include "EC.h"
+#include "math.h"
+
+PwmOut motor1F(PC_9);
+PwmOut motor1B(PC_8);
+PwmOut motor2F(PC_7);
+PwmOut motor2B(PC_6);
+Ec ec1(PA_12, PA_11, NC, 500, 0.05);
+Ec ec2(PC_4, PB_14, NC, 500, 0.05);
+Ticker ticker;
+int tag1 = -100;
+int tag2 = 100;
+int gap1;
+int gap2;
+int a=0;
+int i=1;
+
+void calomega() {
+  ec1.CalOmega();
+  ec2.CalOmega();
+  a=a+1;
+  if(a>=30&&i==1){
+      tag1=-400;
+      a=0;
+      i=0;
+      }
+      if(a>=30&&i==0){
+          tag1=-100;
+          a=0;
+          i=1;
+          }}
+
+int main() {
+  ticker.attach(&calomega, 0.05);
+  motor1F.period_us(50);
+  motor1B.period_us(50);
+  motor2F.period_us(50);
+  motor2B.period_us(50);
+  while (1) {
+    printf("count=%d\r\n", ec1.getCount());
+    gap1 = tag1 - ec1.getCount();
+    gap2 = tag2 - ec2.getCount();
+    if (gap1 > 0) {
+      if (gap1 > 50) {
+        motor1F = 0;
+        motor1B = 0.3;
+      }
+      if (gap1 < 50) {
+        motor1F = 0;
+        motor1B = gap1 * 0.005;
+      }
+    }
+    if (gap1 < 0) {
+      if (gap1 < -50) {
+        motor1F = 0.3;
+        motor1B = 0;
+      }
+      if (gap1 > -50) {
+        motor1F = -gap1 * 0.005;
+        motor1B = 0;
+      }
+    }
+  }
+}
+