C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: LS7366LIB MotCon2 PID
Diff: Axis.cpp
- Revision:
- 0:d7f0715676f4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Axis.cpp Thu Nov 08 17:54:30 2018 +0000 @@ -0,0 +1,320 @@ + +#include "Axis.h" +#include "LS7366.h" +#include "MotCon.h" +#include "PID.h" + +Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _analog(analog){ + this->_cs = 1; // Initialize chip select as off (high) + this->_pwm = 0.0; + this->_dir = 0; + this->co = 0.0; + this->Tdelay = .01; + this->Pk = 450.0; //120.0; //rough gains, seem to work well but could use tuning + this->Ik = 105.0; //75.0; + this->Dk = 0.0; + this->set_point = 0.0; + this->set_point_last = 0.0; + this->pos = 0.0; + this->vel = 0.0; + this->acc = 0.0; + this->stat = -1; + this->pos_cmd = 0.0; + this->vel_cmd = 0.0; + this->vel_avg_cmd = 0; + this->acc_cmd = 0.0; + this->vel_max = 2700.0 * Tdelay; //counts * Tdelay + this->acc_max = 1200.0 * Tdelay; //counts/sec/sec * Tdelay + this->p_higher = 0.0; + this->p_lower = 0.0; + this->vel_accum = 0.0; + this->moveTime = 0.0; + this->enc = 0; + this->moveStatus = 0; //status flag to indicate state of profile movement + this->moveState = 0; //used for state machine in movement profiles + this->debug = 0; + this->update.attach(this, &Axis::paramUpdate, Tdelay); + this->axisState = 0; + this->mot_I_lim = .35; + this->dIdT = 0.0; + this->motI = 0.0; + this->motI_last = 0.0; + this->mot_I_max = 0.0; + this->mot_I_max_last = 0.0; + this->motInvert = 0; + this->dataFormat = 'r'; //default is radians +// this->ctsPerDeg = cpd; //update counts per degree passed from constructor + + this->pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval + this->ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC + this->motcon = new MotCon(pwm, dir); + this->ptr_limit = limit; + + //start at 0 + this->ls7366->LS7366_reset_counter(); + this->ls7366->LS7366_quad_mode_x4(); + this->ls7366->LS7366_write_DTR(0); + + this->set_point = 0.0; + this->pid->setSetPoint(this->set_point); + this->enc = this->ls7366->LS7366_read_counter(); //update class variable +} + +void Axis::init(void){ + //resets the controllers internals + this->pid->reset(); + + //Encoder counts limit + this->pid->setInputLimits(-55000, 55000); + //Pwm output from 0.0 to 1.0 + this->pid->setOutputLimits(-1.0, 1.0); + //If there's a bias. + this->pid->setBias(0.0); + this->pid->setMode(AUTO_MODE); + + this->pid->setInterval(this->Tdelay); + + //start at 0 + this->ls7366->LS7366_reset_counter(); + this->ls7366->LS7366_quad_mode_x4(); + this->ls7366->LS7366_write_DTR(0); + + this->set_point = 0.0; + this->pid->setSetPoint(this->set_point); + this->enc = this->ls7366->LS7366_read_counter(); //update class variable + + //resets the controllers internals + this->pid->reset(); + //start at 0 + this->set_point = 0.0; + this->pid->setSetPoint(0); + + this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller +} + +void Axis::updatePIDgains(float P, float I, float D){ + this->Pk = P; //120.0; //rough gains, seem to work well but could use tuning + this->Ik = I; //75.0; + this->Dk = D; + this->pid->setTunings(this->Pk, this->Ik, this->Dk); +} + +void Axis::paramUpdate(void){ + //testOut = 1; + this->enc = this->ls7366->LS7366_read_counter(); + this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians + + this->vel = (this->pos - this->pos_last) * this->Tdelay; + this->acc = (this->vel - this->vel_last); + + this->pid->setSetPoint(this->set_point); + + //Update the process variable. + this->pid->setProcessValue(this->pos); + //Set the new output. + this->co = this->pid->compute(); + + if(this->axisState){ + if(this->motInvert==0){ + this->motcon->mot_control(this->co); //send controller output to PWM motor control command + } + else{ + this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command + } + } + else{ + this->co = 0.0; + this->motcon->mot_control(0.0); //turn off motor command + } + + this->pos_last = this->pos; + this->vel_last = this->vel; + this->set_point_last = this->set_point; +} + +void Axis::center(void){ + while((*this->ptr_limit == 1) && (this->readCurrent() < mot_I_lim)){ //limit switch not pressed and mot current not exceeded + this->set_point += 100; + wait(.05); + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d motI=%.3f\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit, this->_analog.read()); + } + wait(.5); + while((*this->ptr_limit == 0)){ //limit switch is pressed + this->set_point -= 10; + wait(.1); + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d motI=%.3f\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit, this->_analog.read()); + } + this->zero(); //zero channel + +// this->set_point = -(totalCounts/2.0); + + if(this->debug) + printf("HOME END:T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d motI=%.3f\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit, this->_analog.read()); +// pc.printf("End Home\r\n\r\n"); +} + +void Axis::moveUpdate(void){ + +/* if(*this->ptr_limit == 0){ + this->moveState = 4; //terminate the move + printf("\r\nLimit reached on axis!\r\n"); + } +*/ + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d motI=%.3f\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit, this->_analog.read()); + + switch(this->moveState){ + case 0: + break; + + //accelerate + case 1: + //testOut = 1; + this->vel_accum += this->acc_cmd * this->Tdelay; //add acceleration to the velocity accumulator + if(this->vel_avg_cmd > 0.0){ //check the sign of the movement + if(this->vel_accum >= this->vel_cmd) //if the accumulator reaches or exceeds the velocity command + this->vel_accum = this->vel_cmd; // only add the velocity command to the accumulator + } + else{ //if the sign was negative + if(this->vel_accum <= this->vel_cmd) + this->vel_accum = this->vel_cmd; + } + //testOut = 0; + + this->set_point += this->vel_accum; + //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt); + //pc.printf("acc_up,%.2f,%.3f,%.1f,%.3f,%.3f,%.2f,%.2f\r\n", this->t.read(), this->set_point, this->pos, this->vel, this->acc, this->vel_accum, this->acc_cmd); + + if(this->t.read()>=(this->moveTime/3.0) || (abs(this->vel_accum) > abs(this->vel_cmd))) + this->moveState = 2; + break; + + //constant velocity + case 2: + //testOut = 1; + //this->vel_accum += this->vel_cmd * this->Tdelay; + this->set_point += this->vel_cmd; + //testOut = 0; + //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt); + //pc.printf("vel_cn,%.2f,%.3f,%.1f,%.3f,%.3f,%.2f,%.2f\r\n", this->t.read(), this->set_point, this->pos, this->vel, this->acc, this->vel_accum, this->acc_cmd); + + if(this->t.read()>=(2.0/3.0 * this->moveTime)) + this->moveState = 3; + break; + + //decelerate + case 3: + this->vel_accum -= this->acc_cmd * this->Tdelay; + + this->set_point += this->vel_accum; //ramp down velocity by acceleration + //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt); + //pc.printf("acc_dn,%.2f,%.3f,%.1f,%.3f,%.3f,%.2f,%.2f\r\n", this->t.read(), this->set_point, this->pos, this->vel, this->acc, this->vel_accum, this->acc_cmd); + + if(this->vel_avg_cmd > 0.0){ + if(this->pos_cmd <= this->pos){ + //finish with position command + this->set_point = this->pos_cmd; + this->moveState = 4; + } + } + else{ + if(this->pos_cmd >= this->pos){ + //finish with position command + //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124 + this->moveState = 4; + } + } + + if(this->t.read()>=this->moveTime){ + //finish with position command + //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124 + this->moveState = 4; + } + break; + + case 4: + this->moveProfile.detach(); //turn off the trapazoidal update ticker + this->t.stop(); + this->moveState = 0; + break; + }//switch moveStatus + return; +} + +// position - encoder position to move to +// time - duration of the movement +void Axis::moveTrapezoid(float positionCmd, float time){ + this->pos_cmd = positionCmd; + this->moveTime = time; + float enc_distance = this->pos_cmd - (float)this->enc;// * 1.0/con0.countsPerDeg * 180.0/PI; + + this->vel_avg_cmd = enc_distance / time; + this->vel_cmd = 1.5 * this->vel_avg_cmd * this->Tdelay; + this->acc_cmd = 4.5 * (enc_distance / (this->moveTime * this->moveTime)) * this->Tdelay; + + //pc.printf("tx=%f encdist=%.3f vAvg=%.3f vMax=%.3f Acc=%.3f \r\n", this->moveTime, enc_distance,this->vel_avg_cmd,this->vel_cmd,this->acc_cmd); + + //establish encoder velocities and accelerations for position control per Tdelay + this->vel_accum = 0.0; + +// this->set_point = this->pos; + this->moveState = 1; + this->t.reset(); + this->t.start(); + this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay); +} + +float Axis::readCurrent(void){ + this->motI = (this->_analog.read() * 3.3) / .525; //525mV per amp + if(abs(this->motI) > this->mot_I_max){ //update the max motor current measured + this->mot_I_max = this->motI; + } + this->dIdT = motI - motI_last; + this->motI_last = motI; + return this->motI; +} + +void Axis::axisOff(void){ + this->co = 0; + this->axisState = 0; + this->update.detach(); +} + +void Axis::axisOn(void){ + this->co = 0.0; + this->pid->reset(); + //start at 0 if not already homed + if(this->stat != 0){ + this->set_point = 0.0; + this->pid->setSetPoint(0); + } + + this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller + this->axisState = 1; + this->update.attach(this, &Axis::paramUpdate, this->Tdelay); +} + +void Axis::zero(void){ + + this->ls7366->LS7366_reset_counter(); + this->ls7366->LS7366_quad_mode_x4(); + this->ls7366->LS7366_write_DTR(0); + + this->enc = this->ls7366->LS7366_read_counter(); + + this->pos = 0.0; + this->pid->reset(); //added to avoid possible itegral windup effects on instant position change 20170616 + this->set_point = 0.0; + this->pid->setSetPoint(0); + +} + +void Axis::writeEncoderValue(long value){ + + this->ls7366->LS7366_write_DTR(value); + + this->set_point = (float)value; + this->pid->setSetPoint(this->set_point); +} \ No newline at end of file