309 lines
10 KiB
C++
309 lines
10 KiB
C++
/* Click in this box, then
|
|
CTRL-A to select all
|
|
CTRL-C to copy
|
|
CTRL-V to paste into Arduino IDE
|
|
|
|
File: Lab2_motor_step_v1
|
|
Written: Aug 31, 2020, Clark Hochgraf
|
|
Revised: Sept 17, 2021 - slowed down PWM frequency to 2kHz
|
|
Revised: Feb 8, 2021
|
|
|
|
Desc:
|
|
Closed loop speed control of dc motor.
|
|
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 quadPos = 0.0;
|
|
float lastquadPos = 0;
|
|
float motorspeedRPS = 0;
|
|
|
|
bool isModuleEn = false;
|
|
bool prevModuleEn = false;
|
|
|
|
const byte TSAMP_MSEC = 20;
|
|
long timeElapsedTicks = 0;
|
|
const float TSAMP = 0.001 * TSAMP_MSEC;
|
|
int TOTAL_RUN_TIME_MSEC = 2000; // 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 Wref = 0;
|
|
|
|
//---------------------------------------------------------------------
|
|
// 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 D_DRAG = 6.54e-7;
|
|
const float SYS_A = 1;
|
|
const float SYS_B = 1;
|
|
const float V_FRICTION = 0.2595;
|
|
|
|
float Varm;
|
|
float mtrVel = 0.0, mtrPos = 0.0, errVel = 0.0, errPos = 0.0;
|
|
float refAcc = 0.0, refVel = 0.0, refPos = 0.0;
|
|
|
|
// ##################################################################
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);//Initialize the serial port ** Set Serial Monitor to same 115200
|
|
Serial.println(F("Lab2_closed_loop_vel_v1"));
|
|
Serial.println(F("runs for two seconds and then stops"));
|
|
displayMenu();
|
|
initMotorPins();
|
|
initEncoderInterrupts();
|
|
initPWMtimer2();
|
|
}
|
|
|
|
// ##################################################################
|
|
void loop()
|
|
{ manageMenu();
|
|
syncSample();
|
|
|
|
if (isModuleEn) {
|
|
stepRefVel(500); // input is velocity in radians per sec
|
|
Wref = refVel;
|
|
calculateError();
|
|
//Varm = closedLoopVelP(errVel, 0.1); // error signal, Kp gain // closed loop control
|
|
//Varm = refVel * K_EMF; // uncomment for open loop speed control
|
|
Varm = refVel * K_EMF + V_FRICTION; // uncomment for open loop speed control
|
|
//Varm = refVel * K_EMF + V_FRICTION + mtrVel * D_DRAG * R_ARM / K_TRQ; // uncomment for open loop speed control
|
|
//Varm = Varm + closedLoopVelP(errVel, 0.0125);
|
|
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;
|
|
}
|
|
prevModuleEn = isModuleEn;
|
|
|
|
} // End main loop
|
|
// ##################################################################
|
|
|
|
float closedLoopVelP(float errorIn, float Kp)
|
|
{
|
|
return Kp * errorIn;
|
|
}
|
|
|
|
|
|
//*********************************************************************
|
|
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 calculateError(void)
|
|
{
|
|
mtrVel = (quadPos - lastquadPos) / (TSAMP_MSEC * 0.001); // radians per time interval (rad/sec)
|
|
lastquadPos = quadPos;
|
|
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)
|
|
{
|
|
//print header
|
|
Serial.print("time (msec): "); Serial.print(",");
|
|
Serial.print("motorspeed: (rad/sec) "); Serial.print(",");
|
|
Serial.print("quadPos: (rad) "); Serial.print(",");
|
|
Serial.print("pwm_command: () "); Serial.print(",");
|
|
Serial.print("Varm (V) "); Serial.print(",");
|
|
Serial.print("Wref (rad/sec)");
|
|
Serial.println();
|
|
quadPos = 0;
|
|
lastquadPos = quadPos;
|
|
}
|
|
motorspeedRPS = mtrVel;
|
|
Serial.print(timeElapsedTicks * TSAMP_MSEC); Serial.print(",");
|
|
Serial.print(motorspeedRPS); Serial.print(",");
|
|
Serial.print(quadPos); Serial.print(",");
|
|
Serial.print(pwm_value_commanded); Serial.print(",");
|
|
Serial.print(Varm); Serial.print(",");
|
|
Serial.print(Wref);
|
|
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) | (0 << CS21) | (1 << CS20); // clock prescale = 1 (problem for PWM driver board)
|
|
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) quadPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 8) quadPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 8:
|
|
if (oldquadratureCode == 0) quadPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 12) quadPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 12:
|
|
if (oldquadratureCode == 8) quadPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 4) quadPos -= MTR_RAD_PER_TICK;
|
|
break;
|
|
case 4:
|
|
if (oldquadratureCode == 12) quadPos += MTR_RAD_PER_TICK;
|
|
if (oldquadratureCode == 0) quadPos -= 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
|