C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: PID LS7366LIB MotCon2
Diff: Axis.cpp
- Revision:
- 1:cd249816dba8
- Parent:
- 0:cf7192f9f99a
- Child:
- 2:653433f4ee72
--- a/Axis.cpp Mon Aug 31 17:14:20 2015 +0000 +++ b/Axis.cpp Thu Sep 24 16:47:02 2015 +0000 @@ -4,7 +4,7 @@ #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) { +Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) { _cs = 1; // Initialize chip select as off (high) _pwm = 0.0; _dir = 0; @@ -38,6 +38,7 @@ 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); + ptr_limit = limit; //start at 0 this->ls7366->LS7366_reset_counter(); @@ -50,7 +51,7 @@ } void Axis::init(float cpd){ - _limit.mode(PullUp); + //_limit.mode(PullUp); this->countsPerDeg = cpd; //90.0/10680.0; //resets the controllers internals this->pid->reset(); @@ -108,22 +109,23 @@ void Axis::home(long halfcounts){ this->pid->setInputLimits(-20000.0, 20000.0); - while(this->_limit == 1){ + 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 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); + 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); } this->set_point -= 1000; this->pid->setSetPoint(this->set_point); wait(.5); - while(this->_limit == 1){ + + 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 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); + 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); } this->ls7366->LS7366_write_DTR(0); //zero encoder channel @@ -135,7 +137,7 @@ 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); + 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); } this->set_point = -halfcounts; this->pid->setSetPoint(this->set_point); @@ -207,22 +209,28 @@ //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) + 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) - this->moveState = 4; + if(this->pos_cmd >= this->pos){ + //finish with position command + this->set_point = this->pos_cmd; + this->moveState = 4; + } } if(this->t.read()>=this->moveTime){ + //finish with position command + this->set_point = this->pos_cmd; 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;