#include "stm32g4xx.h"
#include "motors.h"
//#include "pid.h"
//#include "mixer.h"
#include "dshot2.h"

void Motors_Init(void) {
DSHOT_Init(300'000, true);
    
}

void Set_Motors(int val) {
  unsigned short cmd[4]={val,val,val,val};
    DSHOT_SetCommand(cmd, true);
}

void Set_Motor_Individual(int m1, int m2, int m3, int m4) {
    unsigned short cmd[4]={m1,m2,m3,m4};
    DSHOT_SetCommand(cmd, true);
}

void Motors_StartupDelay(void) {
    unsigned short cmd[4]={0,0,0,0};
    DSHOT_SetCommand(cmd, true);
    for (volatile int i = 0; i < 6000000; i++) __NOP();
}

void Motors_Stop(void) {
    unsigned short cmd[4]={0,0,0,0};
    DSHOT_SetCommand(cmd, true);
}

void Motors_Idle(void) {
    unsigned short cmd[4]={150,150,150,150};
    DSHOT_SetCommand(cmd, true);
}

void Motors_Arm(void) {
    unsigned short cmd[4]={150,150,150,150};
    DSHOT_SetCommand(cmd, true);
}

void Motors_Disarm(void) {
    unsigned short cmd[4]={0,0,0,0};
    DSHOT_SetCommand(cmd, true);
}