d

Dependencies:   mbed

Revision:
0:ee4e3ab4db45
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sshape.cpp	Wed Nov 16 08:37:38 2022 +0000
@@ -0,0 +1,51 @@
+#include "sshape.h"
+
+shape::shape(double time){
+    time_c = time;
+}
+
+int shape::pm(double num){
+    return (num>0)-(num<0);
+}
+double shape::start(){
+    start_m = clock();
+    return start_m;
+}
+double shape::shapeTime(double starttime){
+    return ((double)(clock() - starttime) / 100);
+}
+    
+double shape::compute(double stickpoints,double starttime,double add){
+    shapeS_m = kasokudo(time_c,starttime,stickpoints,add);
+    if(pm(stickpoints - add_m)*shapeS_m <= pm(stickpoints - add)*stickpoints){
+        shapeS_m = kasokudo(time_c,starttime,stickpoints,add);
+    }else{
+        add_m = stickpoints;
+        return add_m;
+    }
+    return shapeS_m;
+}
+
+double shape::kasokudo(double time_s,double start_s,double points_s,double add_s){
+
+    if(points_s != 0){
+        if(points_s>add_s){
+            tmp =  ((double)(clock() - start_s) / 100)/time_s;
+            shapeSpeed = tmp*tmp*pm(points_s)*fabs(points_s-add_s)*pm(points_s)+add_s;
+        }else{
+            tmp =  ((double)(clock() - start_s) / 100)/time_s;
+            shapeSpeed = tmp*tmp*pm(points_s)*-fabs(points_s-add_s)*pm(points_s)+add_s;
+        }
+    }else{
+        if(points_s>add_s){
+            tmp =  ((double)(clock() - start_s) / 100)/time_s;
+            shapeSpeed = tmp*tmp*-add_s + add_s;
+        }else{
+            tmp =  ((double)(clock() - start_s) / 100)/time_s;
+            shapeSpeed = -tmp*tmp*add_s + add_s;
+        }
+    }
+
+    return shapeSpeed;
+    
+}
\ No newline at end of file