Ohjaustekniikka
Pyörimisnopeuden mittaaminen back-EMF-menetelmällä
Jos moottorille halutaan tehdä takaisinkytketty säätö, esim. PID, täytyy pyörimisnopeutta pystyä mittamaan. Oheisessa artikkelissa esitellään eräs tapa tähän: back-EMF-mittaus. Sen etu on, että minkäänlaista mittausanturia ei tarvita.
http://robotti.wikidot.com/local--files/ohjaus/BackEMF.jpg
PID controller using Back EMF as the Control Feedback
Usually when implementing a PID motor controller you would need an external opto-mechanical encoder or magnetic sensor to determine the position of the motor. This project uses no external position detecting parts, but instead uses the back EMF generated by a motor in order to determine position.
Inspired by an article in the August 2004 Circuit Cellar magazine by Rich LeGrand, the following project demonstrates positional PID control of a single motor. It's implemented using a PIC16F88, an L293D four channel driver and a few resistors.
The code was written using the CCS C compiler.
Click here for Back EMF motor source code
Click here for the compiled code in HEX ready to load into a PIC 16F88
The code sequences from a trapezoid position trajectory to either a constant velocity or a holding (locked) state. One of 2 Move() functions can be commented in or out to choose the final state of the motor. The #define END_POSITION constant determines where the motor stops for the trapezoid and hold states.
Back EMF detection is accomplished by floating the inputs to the motor briefly (500us), then measuring the voltage from each side or the motor to ground. The project uses the A/D converters of the 16F88 to read these voltages and calculate the current position. See the EMFfb() function for details.
The Acroname web site has a great article on Back EMF.
See http://www.acroname.com/robotics/info/articles/back-emf/back-emf.html
Microchip also has an application note that goes in to great detail on a Back EMF project.
http://ww1.microchip.com/downloads/en/AppNotes/00893a.pdf
PID control is implemented in PIDfb() and uses the standard proportional, derivative and integral calculations. This project was used to control a Lego(tm) 9V motor and was found to be stable using PGain=3 DGain=10 and IGain=0 (i.e. no integral term). See Mr. LeGrand's excellent article in the Circuit Cellar magazine for more information on how to tune a PID system for a particular motor.
Here are some details on the functions of the C program:
main()
This function disables the HW comparator, sets up the digital I/O directions and initialized two Analog to Digital channels. Next, it sets the hardware PWM to 4.88Khz and stops the motor by applying a low to each side. After a 1 second delay, it calls Move() with a trapezoid request. See the Move() function details for what this does. Then it goes into a while loop with a 5ms delay and a call to Tick() which implements both the Back EMF and PID processes. When the trapezoid function completes, it calls Move() again with either a constant speed or a hold request, depending on which call is commented in. Lastly it goes into an infinite loop which again calls Tick().
Move()
This function takes 4 parameters,
1) the operation requested (trapezoid, hold, constant, etc.)
2) the target velocity
3) the desired acceleration to the target velocity
4) the end position
This function's main job is to copy the parms to global variables needed by Tick(), but it also calculates the point at which the trapezoid function starts to decelerate (tDecPos). The deceleration position is approximated by calculating the ramp up position and subtracting it from the end position. This then gives the ramp down position.
Tick()
This function calls the three main functions in the code. First it calculates the next position for the motor to seek to, using CalcTraj(). Next it determines the motor position by calling EMFfb(). Lastly it calls PIDfb() which generates a new PWM value for the motor.
CalcTraj()
CalcTraj() calculates a new target position every tick based on which function Move() requested (trapezoid, hold, constant, etc.) and puts the results in the genPos variable. For Hold, the target position is simply the end position of Move(). For Constant Velocity, it increments genPos by the velocity requested by Move(). A Trapezoid request by Move() puts genPos in one of three states; either accelerating (the upward slope of the trapezoid), holding constant velocity (the plateau of the trapezoid), or decelerating (the final downward slope of the trapezoid). It also shuts off the motor when the trapezoid function is done by setting the runState to OFF.
EMFfb()
The Back EMF function shuts off the PWM signal to the motor for 500uS, then reads the voltage on each side of the motor using two A/D channels on the 16F88. The PWM is restored and a current position is calculated. This position is simply the position it was before, plus the A/D value from the A side of the motor and minus the A/D value from the B side. By doing this, the currPos variable is increased for clockwise (CW) motor rotation and decreased for counter-clockwise (CCW) rotation. This is due to the voltage on side A being positive for CW rotation and zero for CCW rotation. Similarly side B's voltage is positive for CCW rotation and zero for CW rotation.
PIDfb()
The PID function uses the target position calculated by CalcTraj() and the current position from EMFfb() and generates an error value. This error is used to create a PWM value based on the Proportional difference (pGain * error), the integral (iGain * eInteg), and the derivative (dGain * (error - ePrev)). The Web has many fine examples on why these 3 values are used in feedback control systems. Basically, the proportional part is used to mostly seek to the desired position, the integral is used to get to the exact desired position, and the derivative part is used to keep from overshooting or oscillating at the desired position.
SetPWM()
This function uses a signed PWM value to set the direction of the motor and set the hardware PWM value sent. It also clips the PWM value if it is larger than the hardware can handle.
This project was designed to show how Back EMF and PID control can work on a single motor. It was kept simple in order to show the main concepts needed for Back EMF and PID. For a practical robot control, it would need to be expanded to at least two motors and also allow for negative end positions (moving backward) and/or velocities.
Using this controller, a Lego(tm) motor can be ramped up and down, held static or held at a constant speed despite force in either direction. When I first got it working it was amazing to see it in action. Give it a try!
David Hygh
If you have any questions, send me email through my YahooID - davidhy11
*
Filename: BackEmf.c
// Date: 8/20/04
// Version: 1.0
// Author: David Hygh
*
Processor frequency: 20 MHz
// Notes:
// Demonstrates PID control of a motor, using Back EMF as the
// control feedback. This code implements a positional PID
// controller, i.e. the target is a position or distance.
The code sequences from a trapezoid position trajectory to either
// a constant velocity or to holding.
Implemented in a PIC16F88 using the CCS C compiler
*
#include <16F88.h>
#device ADC=8
#fuses HS,NOWDT,NOPUT,MCLR,NOBROWNOUT,NOLVP,NOCPD,NOWRT,CCPB0,NOPROTECT,NOFCMEN,NOIESO
#use delay (clock=20000000)
#use rs232(baud=19200, xmit=PIN_B5, rcv=PIN_B2, parity=n)
#use fast_io(A)
#use fast_io(B)
#define END_POSITION 40000L
// Function Prototypes
void EMFfb(void);
void PIDfb(void);
void SetPWM(signed long);
void Tick(void);
void Move(int, long, long, int32);
void CalcTraj(void);
enum run_types {
R_OFF,
R_TRAPEZOID,
R_HOLD,
R_CONST };
// Global vars
const long pGain = 3L; // proportional gain
const long dGain = 10L; // derivative gain
const long iGain = 0L; // integral gain
int32 tEndPos; // target end position
long tVel, tAcc; // target velocity, target acceleration
signed long currPos = 0; // current position based on EMF feedback
int32 genPos = 0; // generated postion from trapeziod function
signed long genVel = 0; // generated velocity from trapeziod function
int32 tDecPos; // calculated deceleration position
int runState = R_OFF; // current run state
signed long eInteg = 0L; // integral accumulator
void main()
{
setup_comparator(NC_NC_NC_NC);
setup_vref(0);
set_tris_a(0b11110011); // A2-3 outputs
set_tris_b(0b11010100); // B0-1,B3,B5 outputs
setup_adc_ports(sAN0 | sAN1 | VSS_VDD); // setup A/D inputs
setup_adc( ADC_CLOCK_INTERNAL ); // setup A/D clock
set_adc_channel(0); // init A/D channel
setup_ccp1(CCP_PWM);
setup_timer_2(T2_DIV_BY_4, 255, 1); // 4.88 kHz PWM
set_pwm1_duty(0); // initialize PWM off
output_low(PIN_A2); // motor side A low
output_low(PIN_A3); // motor side B low
delay_ms(1000);
// printf("Starting\n\r"); // debug, only if UART connected
/* Trapeziodal Speed */
Move(R_TRAPEZOID, 120, 1, END_POSITION);
while (runState != R_OFF)
{
delay_ms(5);
Tick();
}
/* Hold or Constant Speed */
Move(R_HOLD, 0, 0, END_POSITION); // hold position
// Move(R_CONST, 30, 0, 0); // constant speed
while (TRUE)
{
delay_ms(5);
Tick();
}
}
/* copies run state, target position, velocity and acceleration to globals */
void Move(int run, long vel, long acc, int32 endPos)
{
tEndPos = endPos;
tVel = vel;
tAcc = acc;
// the deceleration position is approximated by calculating the ramp up
// position and subtracting it from the end position.
// the ramp up position is vel^2/acc*2 + vel/2
// this routine skips '+ vel/2' in order to handle non-evenly divisible
// vel/acc/endPos values
tDecPos = endPos - ((vel * vel)/(acc*2));
runState = run;
//printf("endpos=%lu decpos=%lu\n\r", tEndPos, tDecPos);
}
void Tick(void)
{
CalcTraj(); // calculate the trajectory
EMFfb(); // get motor position using back EMF
PIDfb(); // calculate PID control
}
/* calculate trajectory based on runState */
void CalcTraj(void)
{
switch (runState)
{
case R_OFF:
break;
case R_HOLD:
genPos = tEndPos;
break;
case R_CONST:
genPos += tVel;
break;
case R_TRAPEZOID:
if (genPos >= tDecPos)
genVel -= tAcc; // decelerate
else if (genVel < tVel)
{
genVel += tAcc; // accelerate
if (genVel > tVel) genVel = tVel;
}
// position is at/beyond target endpoint, or velocity was negative
if ((genPos + tAcc) >= tEndPos || genVel < 0)
{
genPos = tEndPos;
genVel = 0;
runState = R_OFF;
}
genPos += genVel;
}
// printf("gtp=%lu gtv=%ld\n\r", genPos, genVel);
}
/* implements EMF feedback */
void EMFfb(void)
{
long adcMa = 0; // holds ADC value from motor side A
long adcMb = 0; // holds ADC value from motor side B
setup_ccp1(CCP_OFF); // shut off PWM
output_low(PIN_B3); // sync signal for scope
delay_us(500);
set_adc_channel( 0 );
delay_us(10);
adcMa = read_adc(); // get ADC value from motor side A
set_adc_channel( 1 );
delay_us(10);
adcMb = read_adc(); // get ADC value from motor side B
output_high(PIN_B3); // sync signal for scope
setup_ccp1(CCP_PWM); // activate PWM
currPos += adcMa - adcMb; // calculate current position
}
/* implements PID control */
void PIDfb(void)
{
signed long pwmSet;
static signed long ePrev;
signed long error;
if (runState != R_OFF) // only calculate PWM values if moving or holding
{
error = genPos - currPos; // error is desired-current position
pwmSet = pGain * error + // standard PID equation
iGain * eInteg +
dGain * (error - ePrev); // derivative is current-previous
eInteg += error; // integral is simply a summation over time
ePrev = error; // save previous for derivative
}
else
pwmSet = 0;
SetPWM(pwmSet); // set PWM value to motor
// printf("e%ld cp%ld 1V%ld 2V%ld\n\r", error, currPos, adcV1, adcV2);
}
/* sets hardware PWM based on sign and value of pwm */
void SetPWM(signed long pwm)
{
long pset;
if (pwm < 0)
{
output_low(PIN_A2); // motor side A low
output_high(PIN_A3); // motor side B high
} else {
output_low(PIN_A3); // motor side B low
output_high(PIN_A2); // motor side A high
}
pset = (abs(pwm)); // take absolute value of pwm
// clip PWM signal to hardware max of 1023
if (pset > 1023L)
{
pset = 1023;
//printf("ps%ld pwm%ld\n\r", pset, pwm);
}
set_pwm1_duty(pset); // CCS function to set hardware PWM
}
The code :
:1000000000308A00532A00003A30840000080319A7
:1000100016280630F800F701F70B0C28F80B0B2810
:100020007B30F700F70B1228800B092800341030C2
:10003000F700FA01C601C30CC20C031C25284408B2
:10004000FA070318C60A4508C607C60CFA0CF90CCD
:10005000F80CF70B1B280034F801F901F701FA013D
:100060004908031D36284808031950281030CA00D3
:100070000310C60DC70DF70DFA0D49087A02031DCE
:10008000432848087702031C4C284808F702031C41
:10009000FA034908FA020314F80DF90DCA0B3828BF
:1000A00000008A1182284108A3004008A2003F08EE
:1000B000A1003E08A0003B08A5003A08A4003D08A6
:1000C000A7003C08A6003B08C3003A08C2003B0852
:1000D000C5003A08C40017207908C3007808C20098
:1000E00003103C0DF9003D0DFA007908C4007A08B0
:1000F000C5004308C7007808C6007A08C900790817
:10010000C8002C2878083E02B0003F08B1007908EA
:10011000031C790FB1024008B2000030031C01300B
:10012000B2024108B3000030031C0130B3023908A9
:10013000B40000343408FC3E03185A29043E5C29FC
:100140005A292308AD002208AC002108AB00200882
:10015000AA005A292408AA0725080318250FAB0767
:10016000003003180130AC07003003180130AD0730
:100170005A2933082D02031CD628031DCF28320824
:100180002C02031CD628031DCF2831082B02031C88
:10019000D628031DCF2830082A02031CD62826089B
:1001A000AE022708031C270FAF020229AF1FDB286E
:1001B000A51FE728DD28A51B02292F082502031CFF
:1001C0000229031DE72824082E0203180229260805
:1001D000AE070318AF0A2708AF07A51FF228AF1F05
:1001E000FE28F428AF1B022925082F02031C022930
:1001F000031DFE282E082402031802292508AF003B
:100200002408AE0026082A07B9002B08BA002708E0
:100210000318270FBA072C08BB0000300318013061
:10022000BB072D08BC00003003180130BC072308B1
:100230003C02031C2D29031D2F2922083B02031C0D
:100240002D29031D2F2921083A02031C2D29031DE6
:100250002F292008390203182F29AF1F3A29230814
:10026000AD002208AC002108AB002008AA00AF01B5
:10027000AE01B4012E08F7002F08F800F901FA01C9
:10028000AF1F4429F903FA032A08F7072B080318BC
:100290002B0FF8072C0803182C0FF9072D0803184B
:1002A0002D0FFA077A08AD007908AC007808AB008A
:1002B0007708AA008A1175290A148A100A11820780
:1002C000A028B928A128AA280330BE02031C72293D
:1002D0003E308400000803197229702900000000D4
:1002E000800B6E298A1181299A28B901BA01BB01B4
:1002F000BC019701970186110230BD00F730BE00A6
:100300006429BD0B7E290030F8001F08C739780426
:100310009F001030F700F70B8B2900001F151F19E5
:100320008F291E08BA01B9000830F8001F08C73924
:1003300078049F001030F700F70B9C2900001F1570
:100340001F19A0291E08BC01BB00861506100C3021
:1003500097003B083902F7003A08FA003C08031CF2
:100360003C0FFA027708A8070318A90A7A08A90718
:10037000B4080319192A2808F7002908F800F90118
:10038000FA01A91FC529F903FA0377082A02F70021
:100390007808031C780FCE292B08CF292B02F800F0
:1003A0007908031C790FD6292C08D7292C02F900CB
:1003B0007A08031C7A0FDE292D08DF292D02FA00A6
:1003C0007808BC007708BB00C3010330C2003C08BA
:1003D000C5003B08C40017207908BE007808BD009E
:1003E00037083B02F7003C08FA003808031C380FB6
:1003F000FA027708BF007A08C000C3010A30C200C1
:100400007A08C5007708C400172078083D07B900AE
:100410003E08BA000318BA0A7908BA073B08B507BC
:100420000318B60A3C08B6073C08B8003B08B700FA
:100430001B2ABA01B9013A08BE003908BD00BE1F27
:10044000242A05118515262A851105153E08FA006E
:100450003D08BE1F342A3D08003CF700FA013E0863
:10046000031C3E0FFA027708BF007A08C00040085C
:10047000033C03183F2A0330C000FF30BF00400890
:10048000F9003F08F800F90CF80CF90CF80CF90C1D
:1004900078089500F90C790C3039F7001708CF3936
:1004A00077049700003484011F30830583161F12E0
:1004B0009F121B0880390F389B00073083129F0062
:1004C00040308316990026309800903083129800AF
:1004D000A801A901AA01AB01AC01AD01AE01AF01B8
:1004E000B401B501B601B701B801073083169C000D
:1004F00005081030F700F70B7B2A00001C08831258
:100500000D1383169D01F3306500D43066001F1271
:100510009F121B08803903389B001F1383121F177B
:100520009F1783169F1383121F140030F8001F08B3
:10053000C73978049F0006100C3097000030F8008F
:1005400005389200FF308316920083129501051141
:1005500085110430B900FA30BA000420B90BAB2A77
:100560000130B900BB017830BA00BD010130BC00D8
:10057000C101C0019C30BF004030BE005320B40810
:100580000319C72A0530BA0004207421BF2A02309B
:10059000B900BB01BA01BD01BC01C101C0019C3061
:1005A000BF004030BE0053200530BA000420742143
:0405B000D42A6300E6
:04400E002A3FFC3F0A
:00000001FF
;PIC16F88
