190 lines
4.9 KiB
C++
190 lines
4.9 KiB
C++
#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);
|
|
}
|
|
//------------------------------------------------------------------------------
|