49 lines
1.1 KiB
C++
49 lines
1.1 KiB
C++
#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);
|
|
}
|