C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: LS7366LIB MotCon2 PID
Diff: Axis.cpp
- Revision:
- 0:cf7192f9f99a
- Child:
- 1:cd249816dba8
diff -r 000000000000 -r cf7192f9f99a Axis.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Axis.cpp Mon Aug 31 17:14:20 2015 +0000 @@ -0,0 +1,255 @@ + +#include "Axis.h" +#include "LS7366.h" +#include "MotCon.h" +#include "PID.h" + +Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir), _limit(limit) { + _cs = 1; // Initialize chip select as off (high) + _pwm = 0.0; + _dir = 0; + co = 0.0; + Tdelay = .01; + Pk = 120.0; //80.0; //rough gains, seem to work well but could use tuning + Ik = 55.0; //35.0; + Dk = 0.0; + set_point = 0.0; + set_point_last = 0.0; + pos = 0.0; + vel = 0.0; + acc = 0.0; + pos_cmd = 0.0; + vel_cmd = 0.0; + vel_avg_cmd = 0; + acc_cmd = 0.0; + vel_max = 2700.0 * Tdelay; //counts * Tdelay + acc_max = 1200.0 * Tdelay; //counts/sec/sec * Tdelay + p_higher = 0.0; + p_lower = 0.0; + vel_accum = 0.0; + moveTime = 0.0; + countsPerDeg = 0.0; + enc = 0; + moveStatus = 0; //status flag to indicate state of profile movement + moveState = 0; //used for state machine in movement profiles + debug = 0; + update.attach(this, &Axis::paramUpdate, Tdelay); + + pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval + ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC + motcon = new MotCon(pwm, dir); + + //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(float cpd){ + _limit.mode(PullUp); + this->countsPerDeg = cpd; //90.0/10680.0; + //resets the controllers internals + this->pid->reset(); + + //Encoder counts limit + this->pid->setInputLimits(-20000.0, 20000.0); + //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::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(); + + this->motcon->mot_control(this->co); //send controller output to PWM motor control command + + this->pos_last = this->pos; + this->vel_last = this->vel; + this->set_point_last = this->set_point; + //testOut = 0; +} + +void Axis::home(long halfcounts){ + this->pid->setInputLimits(-20000.0, 20000.0); + while(this->_limit == 1){ + this->set_point += 100; + this->pid->setSetPoint(this->set_point); + wait(.05); + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); + } + this->set_point -= 1000; + this->pid->setSetPoint(this->set_point); + wait(.5); + while(this->_limit == 1){ + this->set_point += 10; + this->pid->setSetPoint(this->set_point); + wait(.02); + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); + } + + this->ls7366->LS7366_write_DTR(0); //zero encoder channel + this->set_point = 0.0; + this->pid->setSetPoint(this->set_point); + + this->pid->setInputLimits(-17000.0, 0.0); //reset span limits + for(int positionCmd = 0;positionCmd > -halfcounts;positionCmd-=30){ + this->set_point = positionCmd; //move arm to center + wait(.01); + if(this->debug) + printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); + } + this->set_point = -halfcounts; + this->pid->setSetPoint(this->set_point); + + //let PID settle to set point + while((this->enc > (this->set_point + 100)) || (this->enc < (this->set_point - 100))){ + //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); + wait(.01); + } + //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); + + this->ls7366->LS7366_write_DTR(0); //zero encoder channel + this->set_point = 0.0; + this->pid->setSetPoint(this->set_point); + + this->pid->setInputLimits(-6500.0, 6500.0); //reset span limits + this->pid->setSetPoint(this->set_point); + +// pc.printf("Home:T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc); +// pc.printf("End Home\r\n\r\n"); +} + +void Axis::moveUpdate(void){ + 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) + this->moveState = 4; + } + else{ + if(this->pos_cmd >= this->pos) + this->moveState = 4; + } + + if(this->t.read()>=this->moveTime){ + this->moveState = 4; + } + break; + + case 4: + //finish with position command + this->set_point = this->pos_cmd; + 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 = 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); +}