For the last step in this journey, the software-based controller is implemented. The most used control is the PID controller defined as,
Discretizing the continuous model, we can obtain the following recursive action,
Where the Q constants are dependent of sampling time, proportional, differential, and integral constants. This allows the implementation of the new PID_t data type and function data handlers
#ifndef __PID_H__
#define __PID_H__
typedef struct{
float SetPoint;
float Q[3];
float E[3];
float Response;
}PID_t;
void PID_Init(PID_t *controller, float Kp, float Ki, float Kd, float Ts, float Set);
void PID_SetReference(PID_t *controller, float SetPoint);
float PID_CalcResponse(PID_t *controller, float feedback);
float PID_GetResponse(PID_t *controller);
#endif
The QEI interrupt handler has two tasks, update the control action according to the sensor response and data transmission. For this, the QEI peripheral is configured to manage interrupt times of 10 ms, allowing a precise and periodic sampling time required by the actual model of the PID. The processor only requires three peripheral memory accesses for the control operation: one for the sensor position register, one for the PWM action control update, and the rotation direction. The QEI decodes the signals and does not require the processor to calculate the position and velocity. This means that the processor is free while the next 10 ms interrupt occurs. We can transmit information to computer by the serial port. The printf function could consume more processor time, but the endianness feature in the processor is a great advantage to minimize the transmission time. The MPLAB Data Visualizer has a simple protocol based on byte framing addressing. The requirement in the GUI is establish the protocol shape, how many bytes will be received and what kind of variables will be decoded. All of this allow the follow interrupt handler,
void QEI_Handler(void){
XIntc_Acknowledge(&intc, XPAR_INTC_0_QEI_0_VEC_ID);
XIntc_Disable(&intc, XPAR_INTC_0_QEI_0_VEC_ID);
static uint32_t pPWM;
response = PID_CalcResponse(&MotorCtrl, (float)sMt -> Position);
if(response >= 0)
OUTS -> DATA2 |= 0x02;
else
OUTS -> DATA2 &= 0x0D;
pPWM = (uint32_t)(fabs(response));
*Duty = pPWM - 2;
dvSendCode(0x03, DV_START);
dvSendVar(&sMt -> Velocity, sizeof(sMt -> Velocity));
dvSendVar(&sMt -> Position, sizeof(sMt -> Position));
dvSendVar(&response, sizeof(response));
dvSendVar(&(MotorCtrl.SetPoint), sizeof(MotorCtrl.SetPoint));
dvSendCode(0x03, DV_END);
XIntc_Enable(&intc, XPAR_INTC_0_QEI_0_VEC_ID);
}
It's observable that, according AXI Timer documentation, a subtraction by two of the conditioned action response is required.
An experimental tuning allow the next behavior of the system,
The use of Q constants on the controller definition allows the reduction of calculus each interrupt, reducing the processor time on runtime, while in the initialization process all these calculi are required once according to the following,
We only require a precise time processing. To do it, we need a peripheral like Fixed time peripheral, Timer peripheral. For the QEI, a Fixed time peripheral behavior was embedded to trigger the interrupt and catch the values for velocity calculation.
The Dual TB9051FTG Motor Driver Shield for Arduino is used for power signal conditioning. In addition, it is possible to sense the current through the motor using a single-ended input ADC. Despite the current signal was not used, it is important to mention that this shield has a current response between the safe voltage range. With 500mV/A resolution, at the peak current allowed by the circuitry, 5A, we obtain a signal of 2.5V peak. The signals for the encoder and motor encoders were remapped in the architecture to avoid signal inconsistency and shield manipulation. The newest port definition is shown below,
set_property PACKAGE_PIN H17 [get_ports CH_Z]
set_property PACKAGE_PIN R16 [get_ports CH_B]
set_property PACKAGE_PIN R14 [get_ports CH_A]
set_property PACKAGE_PIN T15 [get_ports M1PWM]
set_property IOSTANDARD LVCMOS33 [get_ports M1PWM]
set_property IOSTANDARD LVCMOS33 [get_ports CH_A]
set_property IOSTANDARD LVCMOS33 [get_ports CH_B]
set_property IOSTANDARD LVCMOS33 [get_ports CH_Z]
set_property PACKAGE_PIN V17 [get_ports {M1CTRL_tri_o[1]}]
set_property PACKAGE_PIN L16 [get_ports {M1CTRL_tri_o[0]}]
set_property IOSTANDARD LVCMOS33 [get_ports {M1CTRL_tri_o[1]}]
set_property IOSTANDARD LVCMOS33 [get_ports {M1CTRL_tri_o[0]}]
Thess XDC definitions allows the following interaction with external signal conditioners
Both boards, Arty S7 and Motor Diver shield are very flexible to test your experiments with signal conditioning and DC power management devices involved.