349 lines
11 KiB
C++
349 lines
11 KiB
C++
/* Click in this box, then
|
|
CTRL-A to select all
|
|
CTRL-C to copy
|
|
CTRL-V to paste into Arduino IDE
|
|
|
|
File: Lab4_Velocity_PI_control
|
|
Written: Mar 1, 2021 Clark Hochgraf
|
|
Revised:
|
|
Desc:
|
|
Closed loop PI speed control of dc motor with Trapezoidal reference
|
|
Speed of the motor is measured using a quadrature encoder.
|
|
|
|
Assumes fixed 5v 2amp power supply on Motor Driver
|
|
*/
|
|
volatile byte quadratureCode, oldquadratureCode;
|
|
volatile float mtrPos = 0.0;
|
|
float lastmtrPos = 0;
|
|
float motorspeedRPS = 0;
|
|
|
|
bool isModuleEn = false, prevModuleEn = false, isPrinting = false;
|
|
bool isProfileEn = false, oldProfileEn = false, isShowStats = false;
|
|
|
|
|
|
const byte TSAMP_MSEC = 20;
|
|
long timeElapsedTicks = 0;
|
|
const float TSAMP = 0.001 * TSAMP_MSEC;
|
|
long TOTAL_RUN_TIME_MSEC = 4000000; // in millisec
|
|
volatile float adcReading = 0;
|
|
|
|
float pwm_value_commanded;// command supplied to the motor PWM value.
|
|
|
|
const int ENC2 = A2; // d16, PC2 PCINT10 (green wire)
|
|
const int ENC3 = A3; // d17, PC3 PCINT11 (blue wire
|
|
const int PWM_M1A_PIN = 3; // d3, PD3 PCINT19 digital output OCR2B (green wire)
|
|
const int PWM_M1B_PIN = 11; // d11, PB4 digital output OCR2A (blue wire)
|
|
|
|
const float VOLTS_PER_PWM = 5.0 / 255.0; // motor drive voltage per pwm command
|
|
const float PWM_PER_VOLT = 1 / VOLTS_PER_PWM; // pwm value to achieve 1 volt output
|
|
const float RADPERSEC_PERVOLT = 356.0; // from datasheet calculations
|
|
|
|
|
|
|
|
float Varm = 0, Vff = 0;
|
|
float mtrVel = 0.0, errVel = 0.0, errPos = 0.0, errDist = 0.0;
|
|
float refAcc = 0.0, refVel = 0.0, refPos = 0.0, refDist = 200.0;
|
|
float refAccT = 0.0, refVelT = 0.0, refPosT = 0.0, refDistT = 200.0;
|
|
unsigned int tick = 0;
|
|
|
|
float dir = 1.0;
|
|
float disp_rad = 0;
|
|
float specAcc = 0.0, specVel, specDisp_rad, specVdist;
|
|
float trapAcc = 0.0, trapVel = 0.0, trapDisp_rad = 0.0;
|
|
float trapAccT = 0.0;
|
|
int dwellStartT, accT, platT, dwellEndT;
|
|
int T0, T1, T2, T3, T4, T5, T6, T7, T8;
|
|
int D0, D1, D2, D3, D4, D5, D6, D7, D8;
|
|
float dD1, dD2, dD3, dD4, dD5, dD6, dD7, dD8;
|
|
boolean isTrapezoid = true;
|
|
|
|
|
|
//---------------------------------------------------------------------
|
|
// Motor constants: DF robot motor with encoder model FIT0458
|
|
const float K_EMF = 0.00285; //(V/(160*120))*60.0/TWO_PI; // V/rad/sec from V/1000RPM
|
|
const float K_TRQ = 0.008 / 2.8; // N-m/Amp
|
|
const float R_ARM = 2.14; // ohms
|
|
const float GEAR_RATIO = 120; // motor turns per output shaft turns
|
|
const float D_DRAG = 2.45e-6;
|
|
const float SYS_A = 2491.5;
|
|
const float SYS_B = 8.3333;
|
|
const float MaxTrqmN = 1000 * (5.0 / R_ARM) * K_TRQ;
|
|
const float V_FRICTION = 0.0;
|
|
|
|
float trapeAccel = 2 * 256;
|
|
static float trapeVel = 0;
|
|
float Vdist = 0;
|
|
|
|
#define signof(x) ((x) < 0 ? -1 : ((x) > 0 ? 1 : 0))
|
|
|
|
#include "trapeRef.h" // for the PID classes (not a library)
|
|
|
|
// ##################################################################
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);//Initialize the serial port ** Set Serial Monitor to same 115200
|
|
Serial.println(F("Lab4_Velocity_PI_control"));
|
|
Serial.println(F("runs for four seconds and then stops"));
|
|
displayMenu();
|
|
initMotorPins();
|
|
initEncoderInterrupts();
|
|
initPWMtimer2();
|
|
}
|
|
|
|
// ##################################################################
|
|
void loop()
|
|
{ manageMenu();
|
|
syncSample();
|
|
|
|
if (isModuleEn) {
|
|
|
|
boolean useTrapezoidalReference = true;
|
|
if (useTrapezoidalReference) {
|
|
isProfileEn = true;
|
|
float maxAccel = 1300; float maxVelocity = 700; float totalDisplacement = 900; float Ncycles = 1;
|
|
trapRefVel(totalDisplacement, maxVelocity, maxAccel, Ncycles);
|
|
//displacement (radians), veloc (rad/sec), acceleration (rad/sec/sec), Ncycles
|
|
refVel = refVelT; refPos = refPosT;
|
|
}
|
|
else { // use step reference for velocity
|
|
refVel = 300;
|
|
}
|
|
calculateMotorVelocity();
|
|
calculateError();
|
|
Vff = refAccT / SYS_A + refVel * K_EMF + V_FRICTION * signof(refVel) + mtrVel * D_DRAG * R_ARM / K_TRQ;
|
|
//Vff = 0;
|
|
Varm = Vff + closedLoopPI(errVel, 0.0125, 0.195);
|
|
driveMotorVolts(Varm);
|
|
printResults(); // only print while motor running
|
|
|
|
}
|
|
else { //send zero voltage command to pwm if not enabled
|
|
Varm = 0;
|
|
driveMotorVolts(Varm);
|
|
}
|
|
|
|
if (timeElapsedTicks * TSAMP_MSEC > TOTAL_RUN_TIME_MSEC) //stop motor after 2 seconds
|
|
{
|
|
isModuleEn = false; isProfileEn = false;
|
|
}
|
|
prevModuleEn = isModuleEn;
|
|
|
|
} // End main loop
|
|
// ##################################################################
|
|
|
|
//*********************************************************************
|
|
float closedLoopPI(float errorIn, float Kp, float Ki)
|
|
{
|
|
static float PI_integralError=0;
|
|
PI_integralError+= errorIn*TSAMP;
|
|
return Kp * errorIn+ Ki*PI_integralError;
|
|
}
|
|
|
|
|
|
//*********************************************************************
|
|
void stepRefVel(float vel_rad_per_sec) // step input velocity reference levels
|
|
{
|
|
if (timeElapsedTicks < 0) refVel = 0.0;
|
|
else if (timeElapsedTicks < 400) refVel = vel_rad_per_sec;
|
|
else if (timeElapsedTicks < 800) refVel = -vel_rad_per_sec;
|
|
else if (timeElapsedTicks < 1000) refVel = 0.0;
|
|
else isModuleEn = false;
|
|
}
|
|
|
|
//********************************************************************
|
|
void calculateMotorVelocity(void)
|
|
{
|
|
mtrVel = (mtrPos - lastmtrPos) / (TSAMP_MSEC * 0.001); // radians per time interval (rad/sec)
|
|
lastmtrPos = mtrPos;
|
|
}
|
|
|
|
//********************************************************************
|
|
void calculateError(void)
|
|
{
|
|
lastmtrPos = mtrPos;
|
|
errVel = refVel - mtrVel;
|
|
}
|
|
|
|
//********************************************************************
|
|
void driveMotorVolts(float Vmotor)
|
|
{
|
|
int pwm_command = (int)(Vmotor * PWM_PER_VOLT);
|
|
|
|
if (pwm_command < 0) { // negative case -- set direction CW
|
|
pwm_command = - int(pwm_command);
|
|
OCR2B = 0;
|
|
OCR2A = constrain(pwm_command, 0, 255); //Serial.println(OCR2A);
|
|
}
|
|
else
|
|
{ //positive case -- set direction CW
|
|
pwm_command = int(pwm_command);
|
|
OCR2A = 0;
|
|
OCR2B = constrain(pwm_command, 0, 255); //Serial.println(OCR2B);
|
|
}
|
|
}
|
|
|
|
//********************************************************************
|
|
void printResults(void)
|
|
{ if ((isModuleEn != prevModuleEn) && isModuleEn)
|
|
{
|
|
//print header
|
|
Serial.print("time (msec): "); Serial.print(",");
|
|
Serial.print("motorspeed: (rad/sec) "); Serial.print(",");
|
|
Serial.print("mtrPos: (rad) "); Serial.print(",");
|
|
Serial.print("pwm_command: () "); Serial.print(",");
|
|
Serial.print("Varm (V) "); Serial.print(",");
|
|
Serial.print("refVel (rad/sec)");
|
|
Serial.println();
|
|
mtrPos = 0;
|
|
lastmtrPos = mtrPos;
|
|
}
|
|
motorspeedRPS = mtrVel;
|
|
Serial.print(timeElapsedTicks * TSAMP_MSEC); Serial.print(",");
|
|
Serial.print(motorspeedRPS); Serial.print(",");
|
|
Serial.print(mtrPos); Serial.print(",");
|
|
Serial.print(pwm_value_commanded); Serial.print(",");
|
|
Serial.print(Varm); Serial.print(",");
|
|
Serial.print(refVel);
|
|
Serial.println();
|
|
timeElapsedTicks++;
|
|
}
|
|
|
|
//********************************************************************
|
|
void displayMenu()
|
|
{
|
|
Serial.println("\nEnter 'e' to toggle module enable.");
|
|
}
|
|
|
|
//********************************************************************
|
|
void manageMenu()
|
|
{
|
|
char inChar = Serial.read();
|
|
if (inChar == 'e')
|
|
{
|
|
isModuleEn = !isModuleEn;
|
|
if (isModuleEn) {
|
|
Serial.println(F("Module ENABLED"));
|
|
timeElapsedTicks = 0;
|
|
}
|
|
else Serial.println(F("Module DISABLED"));
|
|
}
|
|
}
|
|
|
|
//********************************************************************
|
|
void initMotorPins()
|
|
{ // configure pins as input or output
|
|
pinMode(ENC2, INPUT); // Encoder A
|
|
pinMode(ENC3, INPUT); // Encoder B
|
|
pinMode(PWM_M1A_PIN, OUTPUT); // set motor PWM signal to output
|
|
pinMode(PWM_M1B_PIN, OUTPUT); // set motor direction pin to output
|
|
}
|
|
//********************************************************************
|
|
void initEncoderInterrupts(void)
|
|
{
|
|
// Position encoder ISR setup
|
|
// PCINT1_vect ISR triggered for enabled bit changes on PCMSK1
|
|
|
|
cli(); // disable global interrupts
|
|
PCMSK1 = 0b00001100; // ENC3,2,1,0 -> A3,A2,A1,A0 -> d17,16,15,14
|
|
PCICR = (1 << PCIE1); // enable pin change interrupts 8..14
|
|
sei(); // enable global interrupts
|
|
}
|
|
|
|
//********************************************************************
|
|
void initPWMtimer2(void)
|
|
{
|
|
//-----------------------------------------------------------------
|
|
// Use Timer 2 for direct motor drive PWM generation.
|
|
// Prescale = 1, FAST PWM, 8-bit (Mode 3) -> 62.5 kHz PWM
|
|
// Output pins OC2B (d3~) and OC2A (d11~) driven by counter hardware.
|
|
cli();
|
|
TCCR2B = 0;
|
|
TCCR2A = 0;
|
|
TCCR2B = (0 << WGM22); // start FAST PWM mode 3 setup
|
|
TCCR2A = (1 << WGM21) | (1 << WGM20); // finish FAST PWM setup
|
|
TCCR2B |= (0 << CS22) | (1 << CS21) | (1 << CS20); // clock prescale = 32
|
|
TCCR2A |= (1 << COM2B1) | (0 << COM2B0); // OCR2B pin (d3~) noninverting PWM
|
|
TCCR2A |= (1 << COM2A1) | (0 << COM2A0); // OCR2A pin (d11~) noninverting PWM
|
|
OCR2B = 1; OCR2A = 1;
|
|
sei();
|
|
}
|
|
|
|
//********************************************************************
|
|
void decodeEncoder32(void) // 2 bit quad decoder
|
|
{
|
|
const float MTR_RAD_PER_TICK = TWO_PI / 32;
|
|
static byte oldquadratureCode = 0;
|
|
|
|
oldquadratureCode = quadratureCode;
|
|
quadratureCode = (PINC & 0b00001100);
|
|
|
|
// Quadrature sequence: 0,8,12,4 (update ?CW facing end)
|
|
switch (quadratureCode)
|
|
{
|
|
case 0:
|
|
if (oldquadratureCode == 4) mtrPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 8) mtrPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 8:
|
|
if (oldquadratureCode == 0) mtrPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 12) mtrPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 12:
|
|
if (oldquadratureCode == 8) mtrPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 4) mtrPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 4:
|
|
if (oldquadratureCode == 12) mtrPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 0) mtrPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
}
|
|
|
|
} // decodeEncoder32( )
|
|
|
|
|
|
//********************************************************************
|
|
void syncSample() // set the sample rate for ADC and therefore for the main loop
|
|
{ // sample interval time is set by TSAMP_MSEC
|
|
const unsigned long TIC_USEC = TSAMP_MSEC * 1000UL;
|
|
const byte ADCSRA_ISR = 0b11101111; // auto ISR, clkdiv = 128
|
|
static unsigned long tic, stake = 0;
|
|
static boolean first_run = true;
|
|
if (first_run) {
|
|
stake = micros(); // only runs first time to set stake
|
|
first_run = false;
|
|
}
|
|
|
|
while ((tic - stake) < TIC_USEC) tic = micros(); // wait here until
|
|
stake = tic;
|
|
ADCSRA = ADCSRA_ISR; // start oversample-average series
|
|
}
|
|
|
|
//********************************************************************
|
|
ISR(PCINT1_vect) // if pin change occurs, update quadrature decoder
|
|
{
|
|
decodeEncoder32();
|
|
}
|
|
|
|
//********************************************************************
|
|
ISR (ADC_vect)
|
|
{
|
|
const byte N_ADC_AVE = 80;
|
|
const float INV_N_ADC = 1.0 / N_ADC_AVE;
|
|
static byte nConv = 0;
|
|
static unsigned int loAccum = 0, hiAccum = 0;
|
|
|
|
//SET_TP0_HI;
|
|
loAccum += ADCL; // lower 8 bits: must read before ADCH per Atmel
|
|
hiAccum += ADCH; // upper 2 bits
|
|
|
|
if (++nConv >= N_ADC_AVE)
|
|
{
|
|
//SET_TP1_HI;
|
|
adcReading = INV_N_ADC * (256UL * hiAccum + loAccum);
|
|
hiAccum = 0; loAccum = 0;
|
|
nConv = 0;
|
|
ADCSRA &= ~bit(ADIE); // stop auto conversions
|
|
//SET_TP1_LO;
|
|
}
|
|
//SET_TP0_LO;
|
|
} // end of ADC_vect
|