MOTOR

#include <SPI.h>
#include <SimpleFOC.h>

// Определение пинов для первого SPI (VSPI) M1 (Left)
#define VSPI_MISO 13
#define VSPI_MOSI 11
#define VSPI_SCLK 12
#define VSPI_CS1 10

// Определение пинов для второго SPI (HSPI) M2 (Rigth)
#define HSPI_MISO 17
#define HSPI_MOSI 18
#define HSPI_SCLK 15
#define HSPI_CS2 16

// Объявление объектов SPI
SPIClass* vspi = NULL;
SPIClass* hspi = NULL;

SPIClass SPI_1(SPI);   // Создаем объект для HSPI (SPI_2)
SPIClass SPI_2(HSPI);  // Создаем объект для HSPI (SPI_2)

MagneticSensorSPI sensor1 = MagneticSensorSPI(VSPI_CS1, 14, 0x3FFF);
MagneticSensorSPI sensor2 = MagneticSensorSPI(HSPI_CS2, 14, 0x3FFF);

// Объявление моторов
BLDCMotor motor1 = BLDCMotor(14);
BLDCDriver3PWM driver1(47, 48, 6, 7);

BLDCMotor motor2 = BLDCMotor(14);
BLDCDriver3PWM driver2(4, 5, 1, 2);

Commander command = Commander(Serial);
void doTarget1(char* cmd) { command.scalar(&target1, cmd); }
void doTarget2(char* cmd) { command.scalar(&target2, cmd); }
void doMode(char* cmd)    { command.scalar(&scannerState, cmd); }
void doMotor1(char* cmd)  { command.motor(&motor1, cmd); }
void doMotor2(char* cmd)  { command.motor(&motor2, cmd); }

void initMotors() {
  SimpleFOCDebug::enable(&Serial);

  command.add('T', doTarget1, "target1: ");
  command.add('R', doTarget2, "target2: ");
  command.add('S', doMode, "Mode: ");
  command.add('M', doMotor1, "M1");
  command.add('N', doMotor2, "M2");

  vspi = new SPIClass(SPI);
  hspi = new SPIClass(HSPI);

  vspi->begin(VSPI_SCLK, VSPI_MISO, VSPI_MOSI, VSPI_CS1);
  //vspi->setFrequency(1000000);
  sensor1.init(vspi);

  // Настройка второго SPI
  hspi->begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_CS2);
  //hspi->setFrequency(1000000);
  sensor2.init(hspi);

  // Инициализация первого мотора
  motor1.linkSensor(&sensor1);
  driver1.voltage_power_supply = 25;
  driver1.init();
  motor1.linkDriver(&driver1);

  motor1.voltage_limit = 10; 

  //motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor1.controller = MotionControlType::torque;

  //motor1.useMonitoring(Serial);
  //motor1.monitor_variables = _MON_TARGET | _MON_VEL | _MON_VOLT_D; 

  motor1.init();
  motor1.pole_pairs = 14; //14
  motor1.initFOC();


  // Инициализация второго мотора
  motor2.linkSensor(&sensor2);
  driver2.voltage_power_supply = 25;
  driver2.init();
  motor2.linkDriver(&driver2);
  motor2.voltage_limit = 10;

  motor2.PID_velocity.P = 0.5;
  motor2.PID_velocity.I = 50;
  motor2.PID_velocity.D = 0.000;
  //motor2.PID_velocity.output_ramp = 1000;
  motor2.LPF_velocity.Tf = 0.01;
  
  motor2.P_angle.P = 20;

  //motor2.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor2.controller = MotionControlType::torque;

  motor2.useMonitoring(Serial);
  motor2.monitor_variables = _MON_TARGET | _MON_VEL | _MON_VOLT_D; 

  motor2.init();
  motor2.pole_pairs = 11; //14
  motor2.initFOC();

  /*
  //motor2.useMonitoring(Serial);

  //display variables
  motor2.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; 
  // downsampling
  motor2.monitor_downsample = 100; // default 10
  */
}

void motorLoop() {
  motor1.loopFOC();
  motor2.loopFOC();

  motor1.move(motor1Target); //Slave
  motor2.move(motor2Target); //Master

  //command.run();
}

void callCommand() {
  command.run();
  motor1.monitor();
  //motor2.monitor();
}

/*void motorsQP (int P) {
  motor1.PID_current_q = P;
  motor2.PID_current.P = P;
}*/

void motor1Enable(bool state) {
  if(state) motor1.enable();
  else motor1.disable();

  //Serial.println("Dissable Motors");
}

void motorsDissable() {
  motor1.disable();
  motor2.disable();

  Serial.println("Dissable Motors");
}

void motorsEnable() {
  motor1.enable();
  motor2.enable();

  Serial.println("Enable Motors");
}

/*
void motorRewind(bool state) {
  motor1.enable();
  motor2.enable();

  if (state) {
    motor1.controller = MotionControlType::velocity;
    motor1Target = 6;
    motor2.controller = MotionControlType::torque;
    motor2Target = 3;
    Serial.println("Enable Motors / Rewind");
  }

  else {
    motor1.controller = MotionControlType::torque;
    motor1Target = motor1TargetIsh;
    motor2.controller = MotionControlType::velocity;
    motor2Target = motor2TargetIsh;
    Serial.println("Enable Motors / Scan");
  }
}
*/
void motorsTest() {
  motor1.controller = MotionControlType::velocity;
  motor2.controller = MotionControlType::velocity;

  motor1Target = target1;
  motor2Target = target1;
  
  //Serial.println("Test Motors");
}

void motor2SetVL (int VL) {
  if (VL < 10 && VL > -10) motor2.voltage_limit = VL;
  else Serial.println("M2 VL Wrong!");
}

void motor2ControlTorque (bool torque) {
  if (torque) motor2.controller = MotionControlType::torque;
  else motor2.controller = MotionControlType::velocity;
}

void motor2ControlAngle (bool angle) {
  if (angle) {
    motor2.controller = MotionControlType::angle;
    motor2Target = motor2.shaft_angle;
    //Serial.print(motor2Target);
    //Serial.print(" / ");
    //Serial.println(sensor2.getAngle());
  }
  else motor2.controller = MotionControlType::velocity;
}

float motor2GetAngle () {
  return motor2.shaft_angle;
}

void initialTension(float target_torque, float ramp_time) {
  float start_time = millis();  // Текущее время в секундах
  float current_torque = 0;

  while (current_torque < target_torque) {
    float elapsed_time = millis() - start_time;
    current_torque = (elapsed_time / ramp_time) * target_torque;

    if (current_torque > target_torque) {
      current_torque = target_torque;
    }

    // Применяем одинаковый крутящий момент к обоим моторам
    motor1.move(-current_torque*motorDiff);
    motor2.move(current_torque);  // Обратный знак для второго мотора

    motor1.loopFOC();
    motor2.loopFOC();
  }
}