3
unknown
c_cpp
a month ago
6.5 kB
2
Indexable
#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(¤t_sense1); //current_sense1.driverAlign(mAlignV); current_sense1.skip_align = true; current_sense2.init(); motor2.linkCurrentSense(¤t_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"); } } */
Editor is loading...
Leave a Comment