AnsweredAssumed Answered

How to change duty cycle using TPM?

Question asked by Nícolas Ludwig on May 14, 2019
Latest reply on May 20, 2019 by Kerry Zhou

Hello everybody.

My board is FRDM-KL27Z and I use Kinetis Design Studio. I'm using the accelerometer (bubble_level_tpm example on KSDK 1.3.0 folder) and the TPM driver example, at the same folder. In short, my project consists to equilibrate a bar using two brushless motors, simulating a drone. This video explains what I'm trying to do: https://www.youtube.com/watch?v=AN3yxIBAxTA

I get the accelerometer data, calculate the PID control, and then the result of PID is used to send PWM signal to the ESCs, which controls the motors. I'm also using LPTMR to make a sampling time of 5ms.

 

#include <stdio.h>
#include <string.h>
// SDK Included Files
#include "board.h"
#include "fsl_lptmr_driver.h"
#include "fsl_tpm_driver.h"
#include "fsl_debug_console.h"
#include "accel.h"
#include "fsl_clock_manager.h"

#define LPTMR_INSTANCE 0U

short int tempoInterrupt = 0;
void lptmr_isr_callback(void)
{
tempoInterrupt = 1;
}

void delay (uint32_t d)
{
uint32_t t;
for (t=0; t < d; t++);
}

int main (void)
{
// ********** VARIÁVEIS PID **********
/* Variáveis de controle do cálculo do PID.*/
float erro = 0; // Variável que recebe o erro.
float erroAnterior = 0; // Variável para a realmientação do erro.
float proporcional = 0;
float integral = 0; // Variável para controle do integrador.
float derivativo = 0; // Variável para controle do derivador.

/* Variáveis de controle para o motor.*/
int PID = 0; // Sentido do Giro do Motor
unsigned int PWM = 0; // Valor do PWM para ser enviado ao motor.

/* Variáveis de controle da posição.*/
int SetPoint = 0; // Valor do SetPoint.
//int posAtual = 0; // Recebera o valor de posicao atual.

/* Valores do controlador e tempo de amostragem.*/
short int tAmost = 5; // Tempo de amostragem que queremos para o nosso sistema em milisegundos.
float KP = 1; // Ganho Porporcional.
float KI = 0; // Ganho Integral.
float KD = 0; // Ganho Derivativo.
float p = 100; // Frequencia de 100 rad/s. Somente passará os 100.


short int ehnegativo = 0;

lptmr_state_t lptmrState;
// Configure LPTMR.
lptmr_user_config_t lptmrUserConfig =
{
.timerMode = kLptmrTimerModeTimeCounter, /*! Use LPTMR in Time Counter mode */
.freeRunningEnable = false, /*! When hit compare value, set counter back to zero */
.prescalerEnable = false, /*! bypass prescaler */
.prescalerClockSource = kClockLptmrSrcLpoClk, /*! use 1kHz Low Power Clock */
.isInterruptEnabled = true
};

// Initialize LPTMR
LPTMR_DRV_Init(LPTMR_INSTANCE, &lptmrState, &lptmrUserConfig);

// Set the timer period for 5 milliseconds
LPTMR_DRV_SetTimerPeriodUs(LPTMR_INSTANCE, 5000);

// Specify the callback function when a LPTMR interrupt occurs
LPTMR_DRV_InstallCallback(LPTMR_INSTANCE, lptmr_isr_callback);


tpm_general_config_t driverInfo;

//*************************************************************
//******************** VARIÁVEIS PARA PWM *********************
tpm_pwm_param_t motor1 = {
.mode = kTpmEdgeAlignedPWM,
.edgeMode = kTpmHighTrue,
.uFrequencyHZ = 50u,
.uDutyCyclePercent = 6.5
};

tpm_pwm_param_t motor2 = {
.mode = kTpmEdgeAlignedPWM,
.edgeMode = kTpmHighTrue,
.uFrequencyHZ = 50u,
.uDutyCyclePercent = 6.5
};

//*************************************************************
//**************** VARIÁVEIS PARA ACELERÔMETRO ****************
accel_dev_t accDev;
accel_dev_interface_t accDevice;
accel_sensor_data_t accelData;
accel_i2c_interface_t i2cInterface;
tpm_pwm_param_t yAxisParams;
tpm_pwm_param_t xAxisParams;
int16_t xData, yData;
int16_t xAngle, yAngle;

// Register callback func for I2C
i2cInterface.i2c_init = I2C_DRV_MasterInit;
i2cInterface.i2c_read = I2C_DRV_MasterReceiveDataBlocking;
i2cInterface.i2c_write = I2C_DRV_MasterSendDataBlocking;

accDev.i2c = &i2cInterface;
accDev.accel = &accDevice;

accDev.slave.baudRate_kbps = BOARD_ACCEL_BAUDRATE;
accDev.slave.address = BOARD_ACCEL_ADDR;
accDev.bus = BOARD_ACCEL_I2C_INSTANCE;


// Enable clock for PORTs, setup board clock source
hardware_init();

//*************************************************************
//************** CONFIGURAÇÕES PARA ACELERÔMETRO **************

// Accel device driver utilizes the OSA, so initialize it.
OSA_Init();

// Initialize the Accel.
accel_init(&accDev);

// Prepare memory for initialization.
memset(&driverInfo, 0, sizeof(driverInfo));

// Init TPM.
TPM_DRV_Init(0, &driverInfo);

// Set clock for TPM.
TPM_DRV_SetClock(0, kTpmClockSourceModuleClk, kTpmDividedBy128);

// Starts PWM.
TPM_DRV_PwmStart(0, &motor1, 0); //pino PTE24
TPM_DRV_PwmStart(0, &motor2, 2); //pino PTA5

// Start counting LPTMR
LPTMR_DRV_Start(LPTMR_INSTANCE);


while(1)
{
if (tempoInterrupt == 1)
{
tempoInterrupt = 0;
// Wait 5 ms in between samples (accelerometer updates at 200Hz).
//OSA_TimeDelay(5);

// Get new accelerometer data.
accDev.accel->accel_read_sensor_data(&accDev, &accelData);

// Get the X and Y data from the sensor data structure.fxos_data
xData = (int16_t) ((accelData.data.accelXMSB << 8) | accelData.data.accelXLSB);
yData = (int16_t) ((accelData.data.accelYMSB << 8) | accelData.data.accelYLSB);

// Convert raw data to angle (normalize to 0-90 degrees). No negative angles.
//xAngle = abs((int16_t)(xData * 0.011));
//yAngle = abs((int16_t)(yData * 0.011));
//xAngle = (int16_t) (xData * 0.011);
//yAngle = (int16_t) (yData * 0.011);

xAngle = (int16_t) (xData * 0.0055);
yAngle = (int16_t) (yData * 0.0055);

//Cálculos PID
erro = yAngle - SetPoint;

if (erro < 0)
ehnegativo = 1;
if ((erro < 0.5) && (erro > -0.5))
erro = 0;

proporcional = KP * erro;
integral = integral + ((KI * (erro + erroAnterior) * (tAmost / 1000)) / 2);
derivativo = (((2 - (p * (tAmost / 1000))) * (derivativo)) + (2 * p * (tAmost / 1000) * KD * erro) - (2 * p * (tAmost / 1000) * KD * erroAnterior) / (2 + (p * (tAmost / 1000))));

PID = proporcional + integral + derivativo;

if (PID < 0)
PID = PID * (-1);

PWM = (5 * PID) / 100;

if (PWM > 10) // 10% = 2ms
PWM = 10;
else if (PWM < 5) // 5% = 1ms
PWM = 5;

if (ehnegativo == 1)
{
motor1.uDutyCyclePercent = 6.5 + PWM;
motor2.uDutyCyclePercent = 6.5 - PWM;
}
else
{
motor1.uDutyCyclePercent = 6.5 - PWM;
motor2.uDutyCyclePercent = 6.5 + PWM; 
}

erroAnterior = erro;
}
}
}

 

I attached the main.c file to help you.

 

My problem is, I can't change the duty cycle of the TPM. I start the PWM signal in "TPM_DRV_PwmStart(0, &motor1, 0)" outside the loop (while) and then, inside the loop, I tried to change de duty cycle with "motor1.uDutyCyclePercent = some value" but not worked. I tried also to stop the PWM and start again inside the loop, but still didn't work.

I tried everything, but the duty cycle of the PWM don't change. I tried to create a simple code, without PID calculations, but happened the same issue. The only way to change it is putting a value outside the loop, at the beginning of the code.

 

How can I change it INSIDE the loop, constantly?

Thanks in advance.

Attachments

Outcomes