C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: PID LS7366LIB MotCon2
Diff: Axis.cpp
- Revision:
- 2:653433f4ee72
- Parent:
- 1:cd249816dba8
- Child:
- 3:71447d4fb4f0
diff -r cd249816dba8 -r 653433f4ee72 Axis.cpp --- a/Axis.cpp Thu Sep 24 16:47:02 2015 +0000 +++ b/Axis.cpp Wed Sep 30 16:20:45 2015 +0000 @@ -4,7 +4,7 @@ #include "MotCon.h" #include "PID.h" -Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) { +Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit, float totalCnts): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _analog(analog){ _cs = 1; // Initialize chip select as off (high) _pwm = 0.0; _dir = 0; @@ -28,12 +28,13 @@ p_lower = 0.0; vel_accum = 0.0; moveTime = 0.0; - countsPerDeg = 0.0; + totalCounts = totalCnts; 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); + update.attach(this, &Axis::paramUpdate, Tdelay); + axisState = 1; pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC @@ -50,14 +51,13 @@ this->enc = this->ls7366->LS7366_read_counter(); //update class variable } -void Axis::init(float cpd){ +void Axis::init(void){ //_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); + this->pid->setInputLimits(-this->totalCounts, this->totalCounts); //Pwm output from 0.0 to 1.0 this->pid->setOutputLimits(-1.0, 1.0); //If there's a bias. @@ -99,7 +99,8 @@ //Set the new output. this->co = this->pid->compute(); - this->motcon->mot_control(this->co); //send controller output to PWM motor control command + //this->motcon->mot_control(-(this->co)); //send controller output to PWM motor control command + this->motcon->mot_control(this->co); //send controller output to PWM motor control command this->pos_last = this->pos; this->vel_last = this->vel; @@ -107,44 +108,47 @@ //testOut = 0; } -void Axis::home(long halfcounts){ - this->pid->setInputLimits(-20000.0, 20000.0); - while(*this->ptr_limit == 1){ +void Axis::center(void){ + this->pid->setInputLimits(-this->totalCounts, this->totalCounts); + + while(*this->ptr_limit == 1){ // && ({ this->set_point += 100; this->pid->setSetPoint(this->set_point); wait(.05); if(this->debug) - printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit); + 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(.2); this->set_point -= 1000; this->pid->setSetPoint(this->set_point); - wait(.5); + wait(.6); while(*this->ptr_limit == 1){ this->set_point += 10; this->pid->setSetPoint(this->set_point); wait(.02); if(this->debug) - printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit); + 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->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->pid->setInputLimits(-(this->totalCounts)/2.0, 0.0); //reset span limits + for(int positionCmd = 0;positionCmd > -this->totalCounts/2.0;positionCmd-=30){ this->set_point = positionCmd; //move arm to center wait(.01); if(this->debug) - printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit); + 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->set_point = -halfcounts; + this->set_point = -(this->totalCounts)/2.0; 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); + 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(.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); @@ -153,14 +157,23 @@ this->set_point = 0.0; this->pid->setSetPoint(this->set_point); - this->pid->setInputLimits(-6500.0, 6500.0); //reset span limits + this->pid->setInputLimits(-(this->totalCounts)/2.0, (this->totalCounts)/2.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); + 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){ +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; @@ -261,3 +274,18 @@ this->t.start(); this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay); } + +float Axis::readCurrent(void){ + motCurrent = _analog.read() * 3.3; + return motCurrent; +} + +void Axis::axisOff(void){ + update.detach(); + this->axisState = 0; +} + +void Axis::axisOn(void){ + update.attach(this, &Axis::paramUpdate, Tdelay); + this->axisState = 1; +} \ No newline at end of file