Diff: mechanam.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mechanam.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,45 @@
+#include <mbed.h>
+#include <mechanam.h>
+#include <math.h>
+
+Mechanam::Mechanam(PinName pwmPin,PinName dirPin):_moter(pwmPin,dirPin){}
+
+void Mechanam::drive(double x,double y,double x_t,double magni,double magni_t,double ex,double ey,double ex_t,char location)
+{
+    double speed=sqrt(x*x+y*y)*magni;
+    double rad=atan2(y,-x);
+    double fx=speed*sqrt(2.0)*((sin(rad)-cos(rad))/4.0);
+    double gx=speed*sqrt(2.0)*((sin(rad)+cos(rad))/4.0);
+    
+    if (-ex_t>x_t or x_t>ex_t) {
+        _moter.drive(x_t*magni_t);
+    }else if (-ex<=x and x<=ex and -ey<=y and y<=ey) {
+        _moter.free();
+    }else {
+        if (location=='f')_moter.drive(fx);
+        else if (location=='g')_moter.drive(gx);
+    }
+}
+
+void Mechanam::free()
+{
+    _moter.free();
+}
+
+
+/*
+fx=(Speed*√2)*((sin(x)-cos(x))/4.0
+gx=(Speed*√2)*((sin(x)-cos(x))/4.0
+
+This is a ASCII art of Mechanam
+     -              -
+    | |←fx         | |←gx
+    | |            | |
+     -              -
+ 
+ 
+     -              -
+    | |←gx         | |←fx
+    | |            | |
+     -              -
+ */
\ No newline at end of file