3

#include <SPI.h>

bool motorsDebugState = true;
bool skipInitFOC = true;
int mDebugDownSample = 500;

float m1VolLim = 12;
float m2VolLim = 12;
//float mAlignV = 8;

MagneticSensorSPI sensor1 = MagneticSensorSPI(10, 14, 0x3FFF);
MagneticSensorSPI sensor2 = MagneticSensorSPI(38, 14, 0x3FFF);

InlineCurrentSense current_sense1 = InlineCurrentSense(185.0, 18, 17, _NC); //ACS712 5A
InlineCurrentSense current_sense2 = InlineCurrentSense(185.0, 16, 15, _NC);

BLDCDriver3PWM driver1(47, 48, 45, 1);
BLDCDriver3PWM driver2(4, 5, 6, 7);

Commander command = Commander(Serial);
void doTarget1(char* cmd) { command.scalar(&target1, cmd); }
void doTarget2(char* cmd) { command.scalar(&target2, cmd); }
void doTarget3(char* cmd) { command.scalar(&init_Spool1D, cmd); }
void doTarget4(char* cmd) { command.scalar(&init_Spool2D, 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('Z', doTarget3, "M1D");
  command.add('X', doTarget4, "M2D");
  command.add('M', doMotor1, "M1");
  command.add('N', doMotor2, "M2");

  //sensor1.clock_speed = 100000; // spi clock frequency - OPTIONAL
  sensor1.init();
  sensor2.init();

  motor1.linkSensor(&sensor1);
  motor2.linkSensor(&sensor2);

  if(skipInitFOC) {
    motor1.sensor_direction = Direction::CCW; // or Direction::CCW
    motor1.zero_electric_angle = 6.2199;     // use the real value!
    motor2.sensor_direction = Direction::CCW; // or Direction::CCW
    motor2.zero_electric_angle = 2.3746;     // use the real value
  }

  driver1.voltage_power_supply = 25;
  driver2.voltage_power_supply = 25;
  driver1.init();
  driver2.init();
  motor1.linkDriver(&driver1);
  motor2.linkDriver(&driver2);

  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  
  motor1.voltage_limit = m1VolLim;
  motor2.voltage_limit = m2VolLim;

  initPID();

  motor1.useMonitoring(Serial);
  motor2.useMonitoring(Serial);

  current_sense1.linkDriver(&driver1);
  current_sense2.linkDriver(&driver2);

  motor1.init();
  motor2.init();
  
  //КРАЙНЕ ВАЖНО, ЧТОБ МОТОР БЫЛ МАКИСМАЛЬНО СТАБИЛЕН ПРИ КАЛИБРОВКЕ ПО ТОКУ
  current_sense1.init(); 
  motor1.linkCurrentSense(&current_sense1);
  //current_sense1.driverAlign(mAlignV);
  current_sense1.skip_align = true;
  
  current_sense2.init(); 
  motor2.linkCurrentSense(&current_sense2);
  current_sense2.skip_align = true;
  
  if(!skipInitFOC) {
    //motor1.voltage_sensor_align = mAlignV;
    //motor2.voltage_sensor_align = mAlignV;
    motor1.initFOC(); 
    motor2.initFOC();
  }

  if(motorsDebugState) motorInitDebug();
}

//MON_TARGET 0b1000000  // monitor target value
//_MON_VOLT_Q 0b0100000  // monitor voltage q value
//_MON_VOLT_D 0b0010000  // monitor voltage d value
// _MON_CURR_Q 0b0001000  // monitor current q value - if measured
//_MON_CURR_D 0b0000100  // monitor current d value - if measured
//_MON_VEL    0b0000010  // monitor velocity value
//_MON_ANGLE  0b0000001  // monitor angle value

void motorInitDebug() {
  motor1.useMonitoring(Serial);
  motor1.monitor_variables = _MON_TARGET | _MON_CURR_Q;
  motor2.monitor_downsample = mDebugDownSample; // default 10

  motor2.useMonitoring(Serial);
  motor2.monitor_variables = _MON_TARGET | _MON_CURR_Q | _MON_ANGLE;
  motor2.monitor_downsample = mDebugDownSample; // default 10
}

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

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

void callCommand() {
  command.run();

  if(motorsDebugState) {
    //Serial.println(motor2.shaft_angle);  
    //motor1.monitor(); 
    //motor2.monitor();   
  }
}


void initialTension(float m1_Target, float m2_Target, float ramp_time) {
  float start_time = millis();  // Текущее время в секундах
  float m1_current_torque = 0;
  float m2_current_torque = 0;
  
  Serial.println("Setting initial Tension / M1: " + String(m1_Target) + " / M2: " + String(m2_Target));

  while (m1_current_torque < m1_Target) {
    float elapsed_time = millis() - start_time;
    m1_current_torque = (elapsed_time / ramp_time) * m1_Target;
    m2_current_torque = (elapsed_time / ramp_time) * m2_Target;

    if (m1_current_torque > m1_Target) {
      m1_current_torque = m1_Target;
      m2_current_torque = m1_Target;
    }

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

    motor1.move(-m1_current_torque); // Обратный знак для m1
    motor2.move(m2_current_torque);  
  }

  Serial.println("Desired Tension Equared");
}

/*
void motorEnable(int motor, bool state) {
  if(state && motor == 1) { motor1.enable(); Serial.println("Enable M1");}
  if(state && motor == 2) { motor2.enable(); Serial.println("Enable M2");}
  if(!state && motor == 1) { motor1.disable(); Serial.println("Dissable M1");}
  if(!state && motor == 2) { motor2.disable(); Serial.println("Dissable M2");}
  else Serial.println("MEnable wrong input param");
}

void motorsEnable(bool state) {
  if(state) { motor1.enable(); motor2.enable(); Serial.println("Enable Motors");}
  else { motor1.disable(); motor2.disable(); Serial.println("Dissable Motors");}
}

float motorGetShaftAngle (int motor) {
  if(motor == 1) return motor1.shaft_angle;
  else if(motor == 2) return motor2.shaft_angle;
  else Serial.println("Wrong motor number!");
}
*/

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

/*
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");
  }
}
*/