dmarkova/WoodDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
Dana Markova
2025-07-28 13:21:36 +03:00
commit 0de214c9a1
547 changed files with 287132 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

190
utils/mot.cpp Normal file
View File

@ -0,0 +1,190 @@
#include "mot.h"
#include "pwm.h"
void QUAD_Set(short throt, short x, short y, short z)
{
// 2 0
// 1 3
const unsigned char ESC_UL = 2; // CW // M3
const unsigned char ESC_UR = 0; // CCW // M1
const unsigned char ESC_DL = 1; // CCW // M2
const unsigned char ESC_DR = 3; // CW // M4
const unsigned long count = 4;
short pwm[count] {0,0,0,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+1000;
PWM_SetQuad(pwm, 900, 1500);
}
//------------------------------------------------------------------------------
//tree drone
void QUAD_Set2(short throt, short x, short y, short z)
{
// 1 2
// 3 0
const unsigned char ESC_UL = 1; // CW // M3
const unsigned char ESC_UR = 2; // CCW // M1
const unsigned char ESC_DL = 3; // CCW // M2
const unsigned char ESC_DR = 0; // CW // M4
const unsigned long count = 4;
short pwm[count] {0,0,0,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+1000;
PWM_SetQuad(pwm, 1100, 2000);
}
//------------------------------------------------------------------------------
void QUAD_Set3(short throt, short x, short y, short z)
{
//const unsigned char ESC_UL = 0; // CW // M3
// const unsigned char ESC_UR = 3; // CCW // M1
//const unsigned char ESC_DL = 2; // CCW // M2
// const unsigned char ESC_DR = 1; // CW // M4
// const unsigned char ESC_UL = 3; // CW // M3
//const unsigned char ESC_UR = 1; // CCW // M1
//const unsigned char ESC_DL = 0; // CCW // M2
//const unsigned char ESC_DR = 2; // CW // M4
const unsigned char ESC_UL = 2; // CW // M3
const unsigned char ESC_UR = 0; // CCW // M1
const unsigned char ESC_DL = 1; // CCW // M2
const unsigned char ESC_DR = 3; // CW // M4
const unsigned long count = 4;
short pwm[count] {0,0,0,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+1000;
PWM_SetQuad(pwm, 1100, 2000);
}
//------------------------------------------------------------------------------
void HEXA_Set(short throt, short x, short y, short z)
{
// 2
// 1 4
// 5 0
// 3
const unsigned char ESC_UM = 2; // CW // M1
const unsigned char ESC_UL = 1; // CCW // M5
const unsigned char ESC_UR = 4; // CCW // M4
const unsigned char ESC_DL = 5; // CW // M3
const unsigned char ESC_DR = 0; // CW // M6
const unsigned char ESC_DM = 3; // CCW // M2
const unsigned long count = 6;
static short pwm[count];
for(int a=0; a<count; a++) pwm[a] = 0;
pwm[ESC_UL] += x/2; pwm[ESC_UM] += x; pwm[ESC_UR] += x/2;
pwm[ESC_DL] -= x/2; pwm[ESC_DM] -= x; pwm[ESC_DR] -= x/2;
pwm[ESC_UL] += y; pwm[ESC_UR] -= y;
pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
pwm[ESC_UL] += z; pwm[ESC_UM] -= z; pwm[ESC_UR] += z;
pwm[ESC_DL] -= z; pwm[ESC_DM] += 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+1000;
PWM_SetHexa(pwm, 1100, 1800);
}
//------------------------------------------------------------------------------