add firmware

utils/mot.cpp

@@ -0,0 +1,48 @@
+#include "mot.h"
+
+#include "pwm.h"
+  const unsigned long count = 4;
+ short pwm[count] {0,0,0,0};
+
+void QUAD_Set(short throt, short x, short y, short z)
+{
+  //  3   2
+  //  4   1
+  const unsigned char ESC_UL = 2;  // CW  // M3
+  const unsigned char ESC_UR = 1;  // CCW // M2
+  const unsigned char ESC_DL = 3;  // CCW // M4
+  const unsigned char ESC_DR = 0;  // CW  // M1
+  
+  
+
+  
+    for(int a=0; a<count; a++) pwm[a] = 0;
+  
+  pwm[ESC_UL] += x; pwm[ESC_UR] += x;
+  pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
+                        
+  pwm[ESC_UL] += y; pwm[ESC_UR] -= y; 
+  pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
+  
+  pwm[ESC_UL] += z; pwm[ESC_UR] -= z;
+  pwm[ESC_DL] -= z; pwm[ESC_DR] += z;
+  
+  short min=500, max=-500;
+  for(int a=0; a<count; a++)
+  {
+    if(pwm[a]>500) pwm[a]=500;
+    else if(pwm[a]<-500) pwm[a]=-500;
+    //---
+    if(min>pwm[a]) min=pwm[a];
+    if(max<pwm[a]) max=pwm[a];
+  }
+  
+  short up=(throt+max)-1000;
+  short down=(throt+min);
+  if(up>0) throt-=up;
+  if(down<0) throt-=down;
+  
+  for(int a=0; a<count; a++) pwm[a] += throt;
+  
+  PWM_SetQuad(pwm, 0, 1000);
+}