Torque Control Using DC Current: Development Issues with Motor Movement

developement
, ,
1

Hello everyone, I’m working on a torque control system using DC current and an STM32, based on the DC Current Mode | Arduino-FOC for stepper motors. However, the motor isn’t running as expected; it’s only making random steps. I would appreciate any help in improving the code. Below is the code and some output data.

Components utilized include the A1333 magnetic sensor and the A4954 driver.

Code

uint16_t current_a, current_b, current_resultant;
int16_t torque_component, magnitude_current, phase_voltage_q, phase_voltage_d;
int16_t phase_current_q, phase_current_d;
double sin_theta, cos_theta, theta;
char gv_log_buffer32[50];
uint32_t pwm1, pwm2;
extern PIDController gs_pid_foc;
extern motorParams s_motor_params;

int8_t getSign(int16_t value)
{
    if(value>=0) {
        return 1;
    } else {
        return -1;
    }
}

void updateSensor()
{
    theta = (double)200*getRawValueA1333()*((2*PI)/65535) - 3; // -3 is used to set the zero electrical angle
    //sensorUpdate();
    theta = fmod(theta, 2*PI);
    if (theta < 0) {
        theta += 2 * PI;
    }
    sin_theta = sin(theta);
    cos_theta = cos(theta);
}

void currentVectors()
{
    dmaInit();
    current_a = getCurrent(getADC(1)); //CurrentA
    current_b = getCurrent(getADC(2)); //CurrentB
    current_resultant = (int16_t)sqrt((double)(current_a * current_a + current_b * current_b));
}

void parkTransform()
{
    torque_component = cos_theta*current_b-sin_theta*current_a;
    magnitude_current = getSign(torque_component)*current_resultant; // I_dc
}

void phaseCurrents()
{
    phase_current_q = (int16_t)PIDUpdate(&gs_pid_foc, s_motor_params.current_setpoint, magnitude_current);
    phase_current_d = 0;
}

void inverseParkTransform()
{
    phase_voltage_q = (int16_t)(-cos_theta * phase_current_d + sin_theta * phase_current_q);
    phase_voltage_d = (int16_t)(sin_theta * phase_current_d + cos_theta * phase_current_q);
}

void setPWM()
{
    // 1500 mA is the max current, so the max magnitude current is 1060 for each vector q and d
    if(phase_voltage_d<0) {
        pwm1 = (uint32_t)(phase_voltage_d*-1*10);
    } else {
        pwm1 = (uint32_t)(phase_voltage_d*10);
    }
    if(phase_voltage_q<0) {
        pwm2 = (uint32_t)(phase_voltage_q*-1*10);
    } else {
        pwm2 = (uint32_t)(phase_voltage_q*10);
    }
    htim2.Instance->CCR4 = pwm1; //IN1 and IN2
    htim3.Instance->CCR4 = pwm2; //IN3 and IN4
}

void focLoop()
{
    updateSensor();
    currentVectors();
    parkTransform();
    phaseCurrents();
    inverseParkTransform();
    setPWM();
    // Energizing coils
    if(phase_voltage_d>0){
        bridge(1); //IN1
    } else {
        bridge(0); //IN2
    }
    if(phase_voltage_q>0) {
        bridge(2); //IN3
    } else {
        bridge(3); //IN4
    }
    sprintf(gv_log_buffer32, "%d, %d, %ld, %ld, %.2lf, %lu\r\n", phase_voltage_d, phase_voltage_q, pwm1, pwm2, theta, getAngle_A1333());
    HAL_UART_Transmit(&huart4, (uint8_t *)gv_log_buffer32, strlen(gv_log_buffer32), HAL_MAX_DELAY);
    //HAL_Delay(1000);
}

Phase Voltages

The wave I planned to use for the phase voltages was this one:

Phase Voltages
Phase Voltages435×721 59.3 KB

PID return a value between -9 and 9.

Output Values

To get this values I predefined a fixed current a and b and simulated the raw angle to understand the movement.

Phase Voltage D Phase Voltage Q PWM1 PWM2 Theta Simulated Sampled Angle
9072000.0363243
9072000.0763245
91720800.1163247
91720800.1563249
91720800.1963251
927201600.2263253
927201600.2663255
927201600.363257
937202400.3463259
937202400.3863261
947203200.4263263
846403200.4563265
846403200.4963267
856404000.5363269
856404000.5763271
856404000.6163273
765604800.6563275
765604800.6963277
765604800.7263279
765604800.7663281
674805600.863283
674805600.8463285
674805600.8863287
674805600.9263289
584006400.9563291
584006400.9963293
584006401.0363295
483206401.0763297
483206401.1163299
493207201.1563301
392407201.1863303
392407201.2263305
392407201.2663307
291607201.363309
291607201.3463311
19807201.3863313
19807201.4163315
19807201.4563317
0907201.4963319
0907201.5363321
0907201.5763323
0907201.6163325
0907201.6463327
-19807201.6863329
-19807201.7263331
-19807201.7663333
-291607201.863335
-291607201.8463337
-291607201.8763339
-392407201.9163341
-392407201.9563343
-493207201.9963345
-483206402.0363347
-483206402.0763349
-584006402.163351
-584006402.1463353
-584006402.1863355
-674805602.2263357
-674805602.2663359
-674805602.363361
-674805602.3363363
-765604802.3763365
-765604802.4163367
-765604802.4563369
-765604802.4963371
-856404002.5363373
-856404002.5663375
-856404002.663377
-846403202.6463379
-846403202.6863381
-947203202.7263383
-937202402.7663385
-937202402.7963387
-937202402.8363389
-927201602.8763391
-927201602.9163393
-91720802.9563395
-91720802.9963397
-91720803.0263399
-9072003.0663401
-9072003.163403
-9072003.1463405
-9072003.1863407
-9072003.2263409
-9-1720803.2563411
-9-1720803.2963413
-9-1720803.3363415
-9-27201603.3763417
-9-27201603.4163419
-9-27201603.4563421
-9-37202403.4863423
-9-37202403.5263425
-9-47203203.5663427
-8-46403203.663429
-8-46403203.6463431
-8-56404003.6863433
-8-56404003.7163435
-8-56404003.7563437
-7-65604803.7963439
-7-65604803.8363441
-7-65604803.8763443
-7-65604803.9163445
-6-74805603.9463447
-6-74805603.9863449
-6-74805604.0263451
-6-74805604.0663453
-5-84006404.163455
-5-84006404.1463457
-5-84006404.1763459
-4-83206404.2163461
-4-83206404.2563463
-4-93207204.2963465
-3-92407204.3363467
-3-92407204.3763469
-3-92407204.463471
-2-91607204.4463473
-2-91607204.4863475
-1-9807204.5263477
-1-9807204.5663479
-1-9807204.663481
0-907204.6463483
0-907204.6763485
0-907204.7163487
0-907204.7563489
0-907204.7963491
1-9807204.8363493
1-9807204.8763495
1-9807204.963497
2-91607204.9463499
2-91607204.9863501
3-92407205.0263503
3-92407205.0663505
3-92407205.163507
4-93207205.1363509
4-83206405.1763511
4-83206405.2163513
5-84006405.2563515
5-84006405.2963517
5-84006405.3363519
6-74805605.3663521
6-74805605.463523
6-74805605.4463525
6-74805605.4863527
7-65604805.5263529
7-65604805.5663531
7-65604805.5963533
7-65604805.6363535
8-56404005.6763537
8-56404005.7163539
8-56404005.7563541
8-46403205.7963543
8-46403205.8263545
9-47203205.8663547
9-37202405.963549
9-37202405.9463551
9-37202405.9863553
9-27201606.0263555
9-27201606.0563557
9-1720806.0963559
9-1720806.1363561
9-1720806.1763563
9072006.2163565
9072006.2563567