577 lines
18 KiB
C++
577 lines
18 KiB
C++
// Click in this box, then
|
|
// CTRL-A to select all
|
|
// CTRL-C to copy
|
|
// CTRL-V to paste into Arduino IDE
|
|
|
|
// File: Lab6_Pos_PI_PZ_DEV
|
|
// Written: Sept 21, 2020 Clark Hochgraf
|
|
// Revised:
|
|
// Desc:
|
|
// Closed loop P+I control of motor speed.
|
|
// Assumes fixed 5v 2amp power supply on Motor Driver
|
|
|
|
volatile byte quadratureCode, oldquadratureCode;
|
|
volatile float quadPos = 0.0;
|
|
volatile bool isEncoderChange = false;
|
|
float lastquadPos = 0;
|
|
|
|
bool isModuleEn = false, prevModuleEn = false, isPrinting = false;
|
|
bool isProfileEn = false, oldProfileEn = false, isShowStats = false;
|
|
|
|
const byte TSAMP_MSEC = 20;
|
|
int TIC_MSEC = TSAMP_MSEC;
|
|
long timeElapsedTicks = 0;
|
|
const float TSAMP = 0.001 * TSAMP_MSEC;
|
|
volatile float adcReading = 0;
|
|
|
|
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. / 255.0; // motor drive voltage per pwm command
|
|
const float PWM_PER_VOLT = 1 / VOLTS_PER_PWM; // pwm value to achieve 1 volt output
|
|
|
|
float Vctl, Varm;
|
|
float mtrVel = 0.0, mtrPos = 0.0, errVel = 0.0, errPos = 0.0;
|
|
float refAcc = 0.0, refVel = 0.0, refPos = 0.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;
|
|
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 D_DRAG = 0*1.117e-6;
|
|
const float SYS_A = 1795;//1795; //2040//1717
|
|
const float SYS_B = 6.21;
|
|
|
|
boolean isFrictionCompensated = true; boolean isVff = true;
|
|
const float V_FRICTION = 0.20;
|
|
float Vffwd = 0;
|
|
float Vdist = 0;
|
|
|
|
static float lastmicroseconds=0;
|
|
|
|
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);//Initialize the serial port ** Set Serial Monitor to same 115200
|
|
Serial.println(F("Lab6_Pos_PI_PZ_DEV"));
|
|
|
|
displayMenu();
|
|
initMotorPins();
|
|
initEncoderInterrupts();
|
|
initPWMtimer2();
|
|
}
|
|
|
|
// ##################################################################
|
|
void loop()
|
|
{ manageMenu();
|
|
syncSample();
|
|
lastmicroseconds=micros();
|
|
|
|
isProfileEn = isModuleEn;
|
|
if (isModuleEn) {
|
|
isTrapezoid=false;
|
|
if (isTrapezoid) { // Choose speed reference
|
|
trapRefVel(700.0, 600.0, 1500.0); //displacement (radians), veloc (rad/sec), acceler (rad/sec/sec)
|
|
// good values for trapRefVel are 700, 600, 1500
|
|
} else {
|
|
stepRefPos(50);
|
|
}
|
|
// Choose which control method to use by uncommenting it below
|
|
calculateError(); // location of function call for efficient software execution
|
|
//delayMicroseconds(20000); // To model delay of digital controller, add delay here
|
|
//closedLoopVelPI(isFrictionCompensated, isVff, 0.004, 0.01); // friction enabled? Vff enabled? Kp gain
|
|
closedLoopPosPI(isFrictionCompensated, isVff, 0.16, 0.1); // friction enabled? Vff enabled? Kp gain
|
|
//closedLoopPosPI_PZ(isFrictionCompensated, isVff, 0.16,0.1,5,20); // friction enabled? Vff enabled? Kp,Ki,CompC,CompD
|
|
//calculateError(); // if the calculateError() is called here system stability is severely degraded.
|
|
}
|
|
else { // stop motor by sending zero voltage command to pwm
|
|
OCR2B = 0; OCR2A = 0;
|
|
timeElapsedTicks=0;
|
|
quadPos=0;
|
|
lastquadPos = quadPos;
|
|
}
|
|
|
|
if (isModuleEn && (timeElapsedTicks * TSAMP_MSEC) <= 4000) //stop print after 4 seconds
|
|
{
|
|
//isModuleEn = false;
|
|
printResults();
|
|
}
|
|
|
|
if (isPrinting) printResults();
|
|
|
|
//lastmicroseconds=micros();
|
|
//Serial.println(micros()-lastmicroseconds);
|
|
if (isModuleEn) timeElapsedTicks++;
|
|
prevModuleEn = isModuleEn;
|
|
} // End main loop
|
|
// ##################################################################
|
|
|
|
|
|
|
|
//**************************************************************
|
|
float PD_compensate(float x, float c, float d)
|
|
{
|
|
// Proportional plus Derivative compensator
|
|
// Cancel pole at -c, replace with a pole at -d.
|
|
// Implement (s+c)/(s+d) as 1 + (c-d)/(s+d).
|
|
// and post scale by d/c to get unity gain at DC.
|
|
|
|
// PD section
|
|
static float yd = 0.0;
|
|
yd += ((c - d) * x - d * yd) * TSAMP;
|
|
float ypd = x + yd;
|
|
ypd = ypd * d / c; // correct scaling to unity gain at DC.
|
|
return ypd;
|
|
}
|
|
|
|
|
|
//********************************************************************
|
|
float VfrictionVelocity(float Vcmd)
|
|
{ // calculates voltage required to overcome friction.
|
|
// direction of voltage depends on commanded direction of velocity
|
|
float frictionV = 0.0;
|
|
//Coulomb friction
|
|
if (Vcmd > 0.001) frictionV = V_FRICTION;
|
|
if (Vcmd < -0.001) frictionV = -V_FRICTION;
|
|
//static friction
|
|
if ((mtrVel < 1) && (mtrVel > -1) && (Vcmd > 0.001)) frictionV += 0.4;
|
|
if ((mtrVel < 1) && (mtrVel > -1) && (Vcmd < -0.001)) frictionV -= 0.4;
|
|
return frictionV;
|
|
}
|
|
|
|
|
|
//********************************************************************
|
|
void calculateError(void)
|
|
{
|
|
mtrVel = (quadPos - lastquadPos) / (TSAMP_MSEC * 0.001); // radians per time interval (rad/sec)
|
|
lastquadPos = quadPos;
|
|
mtrPos = quadPos;
|
|
errVel = refVel - mtrVel;
|
|
errPos = refPos - mtrPos;
|
|
}
|
|
|
|
//********************************************************************
|
|
void printResults(void)
|
|
{ if (isModuleEn != prevModuleEn)
|
|
{
|
|
//print header
|
|
Serial.print("time (msec): ");
|
|
Serial.print("\tmotorspeed: (rad/sec)");
|
|
Serial.print("\tquadPos: (rad)");
|
|
Serial.print("\t errVel: (rad/sec)");
|
|
Serial.print("\t Vctrl (V)");
|
|
Serial.print("\t Wref (rad/sec)");
|
|
Serial.print("\t Posref (rad)");
|
|
Serial.println();
|
|
//lastquadPos = quadPos;
|
|
}
|
|
Serial.print(timeElapsedTicks * TSAMP_MSEC); Serial.print("\t");
|
|
Serial.print(mtrVel); Serial.print("\t");
|
|
Serial.print(quadPos); Serial.print("\t");
|
|
Serial.print(errVel); Serial.print("\t");
|
|
Serial.print(Vctl); Serial.print("\t");
|
|
Serial.print(refVel); Serial.print("\t");
|
|
Serial.print(refPos);
|
|
Serial.println();
|
|
}
|
|
|
|
|
|
//********************************************************************
|
|
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
|
|
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();
|
|
}
|
|
|
|
//********************************************************************
|
|
ISR(PCINT1_vect) // vector to quadrature decoder
|
|
{
|
|
//digitalWrite(ALED,!digitalRead(ALED));
|
|
isEncoderChange = true;
|
|
decodeEncoder32();
|
|
}
|
|
|
|
//********************************************************************
|
|
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); // inner tracks
|
|
|
|
// 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 TIC_MSEC
|
|
const unsigned long TIC_USEC = TIC_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 (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
|
|
|
|
//********************************************************************
|
|
void displayMenu()
|
|
{
|
|
Serial.println("\nEnter 'e' to toggle module enable.");
|
|
//Serial.println("Enter 'g' to go.");
|
|
}
|
|
|
|
//********************************************************************
|
|
void manageMenu()
|
|
{
|
|
char inChar = Serial.read();
|
|
if (inChar == 'e')
|
|
{
|
|
isModuleEn = !isModuleEn;
|
|
//digitalWrite(PWMVAL_PIN, isModuleEn); // PWM disable low
|
|
if (isModuleEn) {
|
|
Serial.println(F("Module ENABLED"));
|
|
timeElapsedTicks = 0;
|
|
oldProfileEn=false; isProfileEn=true;
|
|
}
|
|
else {
|
|
Serial.println(F("Module DISABLED"));
|
|
quadPos=0;
|
|
lastquadPos=quadPos;
|
|
}
|
|
|
|
}
|
|
else if (inChar == 'g')
|
|
{
|
|
timeElapsedTicks = 0;
|
|
dir = 1.0;
|
|
isProfileEn = true;
|
|
}
|
|
else if (inChar == 'p')
|
|
{
|
|
isPrinting = !isPrinting;
|
|
}
|
|
else if (inChar == 'f')
|
|
{
|
|
isFrictionCompensated = !isFrictionCompensated;
|
|
if (isFrictionCompensated) Serial.println(F("Stiction Comp Enabled"));
|
|
}
|
|
else if (inChar == 't')
|
|
{
|
|
isTrapezoid = !isTrapezoid;
|
|
if (isTrapezoid) {
|
|
Serial.println(F("Trapezoidal Reference"));
|
|
}
|
|
else {
|
|
Serial.println(F("Step Reference"));
|
|
}
|
|
}
|
|
}
|
|
|
|
//********************************************************************
|
|
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 stepRefPos(float pos_rad) // step input velocity reference levels
|
|
{
|
|
static float refPos_last_time = 0;
|
|
oldProfileEn = isProfileEn;
|
|
if (timeElapsedTicks < 0) refVel = 0.0;
|
|
else if (timeElapsedTicks < 40000) refPos = pos_rad;
|
|
else if (timeElapsedTicks < 80) refPos = -pos_rad;
|
|
else if (timeElapsedTicks < 100) refPos = 0.0;
|
|
else isProfileEn = false;
|
|
//refPos += refVel_last_time * TSAMP;
|
|
//refVel_last_time = refVel;
|
|
isShowStats = (oldProfileEn && !isProfileEn);
|
|
}
|
|
|
|
//*********************************************************************
|
|
void trapRefVel(float specDisp_rad, float specVel, float specAcc) // trap velocity profile
|
|
{
|
|
if (!oldProfileEn && isProfileEn)
|
|
{
|
|
//specDisp_rad = 300.0; specVel = 150.0; specAcc = 800.0;
|
|
trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
dwellStartT = 10; dwellEndT = 10; specVdist = 0.0;
|
|
|
|
//specDisp_rad = 300.0, specVel = 100.0, specAcc = 200.0;
|
|
//trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
//dwellStartT = 50, dwellEndT = 200; specVdist = 2.0;
|
|
|
|
//specDisp_rad = 206.9, specVel = 110.0, specAcc = 180.0;
|
|
//trapBuildSymAcc(specDisp_rad, specVel, specAcc);
|
|
//dwellGoT = 300, dwellUpT = 500, dwellEndT = 200; specVdist = abs(V_DIST);
|
|
|
|
// Times when speed changes
|
|
T0 = dwellStartT;
|
|
T1 = T0 + accT; T2 = T1 + platT; T3 = T2 + accT; T4 = T3 + dwellEndT;
|
|
T5 = T4 + accT; T6 = T5 + platT; T7 = T6 + accT; T8 = T7 + dwellEndT;
|
|
|
|
//T0 = dwellGoT;
|
|
//T1 = T0 + accT, T2 = T1 + platT, T3 = T2 + accT, T4 = T3 + dwellUpT;
|
|
//T5 = T4 + accT, T6 = T5 + platT, T7 = T6 + accT, T8 = T7 + dwellEndT;
|
|
|
|
// Times when disturbances are applied and values
|
|
D1 = T3 + 100; D2 = D1 + 200;
|
|
dD1 = specVdist; dD2 = -specVdist;
|
|
|
|
//simJumpOn_simJumpOff();
|
|
//simJumpOn_simFileOff();
|
|
//weightOn_simJumpOff();
|
|
//weightOn_simFileOff();
|
|
}
|
|
tick = timeElapsedTicks;
|
|
// ---- Command profile -------------------------------------
|
|
oldProfileEn = isProfileEn;
|
|
if (tick < T0) refAcc = 0.0;
|
|
else if (tick < T1) refAcc = dir * trapAcc;
|
|
else if (tick < T2) refAcc = 0.0;
|
|
else if (tick < T3) refAcc = -dir * trapAcc;
|
|
else if (tick < T4) refAcc = 0.0;
|
|
//else if (tick == T4) isProfileEn = false;
|
|
|
|
else if (tick < T5) refAcc = -dir * trapAcc;
|
|
else if (tick < T6) refAcc = 0.0;
|
|
else if (tick < T7) refAcc = dir * trapAcc;
|
|
else if (tick < T8) refAcc = 0.0;
|
|
else if (tick == T8) isProfileEn = false;
|
|
|
|
//---- Disturbance profile -----------------------------------
|
|
if (tick < D1) Vdist = 0.0;
|
|
else if (tick == D1) Vdist += dD1 * specVdist; // module, sim load
|
|
else if (tick == D2) Vdist += dD2 * specVdist;
|
|
else if (tick == D3) Vdist += dD3 * specVdist;
|
|
else if (tick == D4) Vdist += dD4 * specVdist;
|
|
else if (tick == D5) Vdist += dD5 * specVdist;
|
|
else if (tick == D6) Vdist += dD6 * specVdist;
|
|
else if (tick == D7) Vdist += dD7 * specVdist;
|
|
else if (tick == D8) Vdist += dD8 * specVdist;
|
|
|
|
//---- Profile integration -----------------------------------
|
|
if (isProfileEn) {
|
|
refVel += refAcc * TSAMP;
|
|
refPos += refVel * TSAMP;
|
|
}
|
|
else {
|
|
refAcc = 0.0;
|
|
refVel = 0.0;
|
|
}
|
|
|
|
isShowStats = (oldProfileEn && !isProfileEn);
|
|
}
|
|
|
|
//**************************************************************
|
|
void trapBuildSymAcc(float Disp_rad, float Vplat, float Acc)
|
|
{
|
|
float tAcc = Vplat / Acc;
|
|
float dAcc = Vplat * tAcc;
|
|
float dPlat = Disp_rad - dAcc;
|
|
float tPlat = dPlat / Vplat;
|
|
|
|
trapAcc = Acc; // acceleration rate
|
|
accT = int(round(tAcc / TSAMP)); // time duration of acceleration
|
|
platT = int(round(tPlat / TSAMP)); // time duration of constant speed
|
|
float dTrap = (accT + platT) * TSAMP * Vplat; // total displacement during trapezoid profile
|
|
|
|
// Serial.println();
|
|
// Serial.print("tAcc = "); Serial.println(tAcc);
|
|
// Serial.print("dAcc = "); Serial.println(dAcc);
|
|
// Serial.print("dPlat = "); Serial.println(dPlat);
|
|
// Serial.print("tPlat = "); Serial.println(tPlat);
|
|
// Serial.print("accT = "); Serial.println(accT);
|
|
// Serial.print("platT = "); Serial.println(platT);
|
|
// Serial.print("dTrap = "); Serial.println(dTrap);
|
|
}
|
|
|
|
//********************************************************************
|
|
void closedLoopVelPI(boolean isFrictionCompensated, boolean isVff, float KpGain, float KiGain)
|
|
{
|
|
float Kp = KpGain;
|
|
float Ki = KiGain;
|
|
|
|
Vffwd = ((SYS_B / SYS_A) * refVel + (1.0 / SYS_A) * refAcc);
|
|
Vctl = PI_compensate(errVel, Kp, Ki); // PI control
|
|
|
|
if (isFrictionCompensated) {
|
|
Vctl += VfrictionVelocity(refVel);
|
|
}
|
|
if (isVff) {
|
|
Vctl += Vffwd;
|
|
}
|
|
Varm = Vctl;
|
|
if (isModuleEn) driveMotor(Varm, mtrVel, mtrPos);
|
|
}
|
|
|
|
//********************************************************************
|
|
void closedLoopPosPI(boolean isFrictionCompensated, boolean isVff, float KpGain, float KiGain)
|
|
{
|
|
float Kp = KpGain;
|
|
float Ki = KiGain;
|
|
|
|
Vffwd = ((SYS_B / SYS_A) * refVel + (1.0 / SYS_A) * refAcc);
|
|
Vctl = PI_compensate(errPos, Kp, Ki); // PI control
|
|
|
|
if (isFrictionCompensated) {
|
|
Vctl += VfrictionVelocity(refVel);
|
|
}
|
|
if (isVff) {
|
|
Vctl += Vffwd;
|
|
}
|
|
Varm = Vctl;
|
|
if (isModuleEn) driveMotor(Varm, mtrVel, mtrPos);
|
|
}
|
|
|
|
//********************************************************************
|
|
void closedLoopPosPI_PZ(boolean isFrictionCompensated, boolean isVff, float KpGain, float KiGain, float CompC, float CompD)
|
|
{
|
|
float Kp = KpGain;
|
|
float Ki = KiGain;
|
|
|
|
Vffwd = ((SYS_B / SYS_A) * refVel + (1.0 / SYS_A) * refAcc);
|
|
Vctl = PI_compensate(errPos, Kp, Ki); // PI control
|
|
Vctl = PD_compensate(Vctl, CompC, CompD); // PD control, signal, CompC, CompD)
|
|
if (isFrictionCompensated) {
|
|
Vctl += VfrictionVelocity(refVel);
|
|
}
|
|
if (isVff) {
|
|
Vctl += Vffwd;
|
|
}
|
|
Varm = Vctl;
|
|
if (isModuleEn) driveMotor(Varm, mtrVel, mtrPos);
|
|
//Serial.println(micros()-lastmicroseconds);
|
|
}
|
|
|
|
//**************************************************************
|
|
float PI_compensate(float x, float Kp, float Ki)
|
|
{
|
|
// Implement (Kp s+Ki)/s as Kp + Ki/s.
|
|
static float Integralx = 0.0;
|
|
Integralx += x * TSAMP;
|
|
float ypi = Kp * x + Ki * Integralx;
|
|
// Apply integral compensator windup clipping.
|
|
return ypi;
|
|
}
|
|
|
|
|
|
//********************************************************************
|
|
void driveMotor(float Varm, float &vel, float &disp_rad)
|
|
{
|
|
// 8 bit PWM, ~5 volt rail
|
|
// note: reversed sign of Varm to get positive speed for positive volts
|
|
float pwm_command = -Varm * PWM_PER_VOLT;
|
|
|
|
if (pwm_command < 0) { // negative case -- set direction CW
|
|
pwm_command = - int(pwm_command);
|
|
OCR2A = 0;
|
|
OCR2B = constrain(pwm_command, 0, 255);
|
|
//Serial.println(OCR2B);
|
|
}
|
|
else
|
|
{ //positive case -- set direction CW
|
|
pwm_command = int(pwm_command);
|
|
OCR2B = 0;
|
|
OCR2A = constrain(pwm_command, 0, 255);
|
|
//Serial.println(OCR2A);
|
|
}
|
|
}
|
|
|
|
|