add firmware

utils/move.cpp

@@ -0,0 +1,102 @@
+#include "move.h"
+#include <math.h>
+
+#include "quat.h"
+
+static const float PI = 3.14159265359f;
+static const float TO_DEG = 180.0f/PI;
+static const float TO_RAD = PI/180.0f;
+
+Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time)
+{
+  //static float move_x=0, move_y=0;
+  
+  //static float move_tof_x=0, move_tof_y=0;
+  //static float move_flo_x=0, move_flo_y=0;
+  
+  /*static long timer=0;
+  timer++;
+  if(timer>1000)
+  {
+    move_flo_x=move_flo_y=move_tof_x=move_tof_y=0;
+    move_x=0; move_y=0;
+    timer=0;
+  }*/
+  
+  //if(!move) return {0,0,0};
+  
+  static float len, high;
+  high = range * vec.z;               // cos(Z)
+  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
+  
+  static float a_x=0, a_y=0;
+  a_x=0, a_y=0;
+  
+  static float v_x, v_y;
+  
+  static float angle_shift=600;
+  
+  if(move)
+  {
+    a_x = (v_x-vec.x)*angle_shift;
+    a_y = (v_y-vec.y)*angle_shift;
+  }
+  
+  v_x=vec.x;
+  v_y=vec.y;
+  
+  static Vec3 v;
+  
+  float x=(dx+a_x), y=(dy-a_y);
+  
+  static float fx=0, fy=0;
+  
+  //fx=(fx*3+x)/4;
+  //fy=(fy*3+y)/4;
+  
+  return {x, y, high};
+  
+  //v = RotateToZ({dx-a_x, dy-a_y, 0});
+  
+  //v = RotateToZ({(dx+a_x)*(range/1000.0f), (dy-a_y)*(range/1000.0f), 0}, true);
+  
+  //v = RotateToZ({1000, 0, 0}, true);
+  //v = RotateToZ({(dx+a_x)*(range/1000.0f), 0, 0}, true);
+  
+  //v={dx+a_x, dy-a_y, 0};
+  
+  //move_x-=v.x;
+  //move_y-=v.y;
+  
+  
+  //move_flo_x+=dx;
+  //move_flo_y+=dy;
+  
+  //move_tof_x+=(float)a_x;
+  //move_tof_y+=(float)a_y;
+  
+  return v;
+  
+}
+
+/*void Motion(Vec3 vec, short dx, short dy, float range, unsigned long time)
+{
+  static long move_x=0, move_y=0;
+  
+  move_x+=dx;
+  move_y+=dy;
+  
+  static float len, high;
+  high = range * vec.z;               // cos(Z)
+  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
+  
+  static float a_x, a_y;
+  
+  a_x = range * vec.x;
+  a_y = -range * vec.y;
+  
+  static float m_x, m_y;
+  
+  m_x=move_x+a_x;
+  m_y=move_y+a_y;
+}*/