C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: PID LS7366LIB MotCon2
Diff: Axis.cpp
- Revision:
- 8:7e399d7c990d
- Parent:
- 7:d0458137d6e0
- Child:
- 9:7bc59203ce98
diff -r d0458137d6e0 -r 7e399d7c990d Axis.cpp --- a/Axis.cpp Thu May 19 12:41:53 2016 +0000 +++ b/Axis.cpp Mon Jul 11 18:26:26 2016 +0000 @@ -4,20 +4,21 @@ #include "MotCon.h" #include "PID.h" -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){ +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 = 120.0; //80.0; //rough gains, seem to work well but could use tuning - this->Ik = 55.0; //35.0; + this->Ik = 75.0; //35.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; @@ -28,7 +29,6 @@ this->p_lower = 0.0; this->vel_accum = 0.0; this->moveTime = 0.0; - this->totalCounts = totalCnts; this->enc = 0; this->moveStatus = 0; //status flag to indicate state of profile movement this->moveState = 0; //used for state machine in movement profiles @@ -38,6 +38,7 @@ this->mot_I_lim = .35; 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 @@ -59,7 +60,7 @@ this->pid->reset(); //Encoder counts limit - this->pid->setInputLimits(-this->totalCounts, this->totalCounts); + 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. @@ -101,15 +102,12 @@ //Set the new output. this->co = this->pid->compute(); -DigitalOut led3(P1_21); if(this->axisState){ if(this->motInvert==0){ this->motcon->mot_control(this->co); //send controller output to PWM motor control command - led3=1; } else{ this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command - led3=0; } } else{ @@ -123,9 +121,7 @@ //testOut = 0; } -void Axis::center(void){ - this->pid->setInputLimits(-this->totalCounts, this->totalCounts); - +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); @@ -141,9 +137,7 @@ } this->zero(); //zero channel - this->set_point = -(totalCounts/2.0); - wait(5.0); - this->zero(); +// 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()); @@ -262,7 +256,7 @@ } float Axis::readCurrent(void){ - motCurrent = this->_analog.read() * 3.3; + motCurrent = (this->_analog.read() * 3.3)/ .525; //525mV per amp return motCurrent; } @@ -289,4 +283,11 @@ 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