C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: LS7366LIB MotCon2 PID
Axis.cpp
00001 00002 #include "Axis.h" 00003 #include "LS7366.h" 00004 #include "MotCon.h" 00005 #include "PID.h" 00006 00007 Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _analog(analog){ 00008 this->_cs = 1; // Initialize chip select as off (high) 00009 this->_pwm = 0.0; 00010 this->_dir = 0; 00011 this->co = 0.0; 00012 this->Tdelay = .01; 00013 this->Pk = 450.0; //120.0; //rough gains, seem to work well but could use tuning 00014 this->Ik = 105.0; //75.0; 00015 this->Dk = 0.0; 00016 this->set_point = 0.0; 00017 this->set_point_last = 0.0; 00018 this->pos = 0.0; 00019 this->vel = 0.0; 00020 this->acc = 0.0; 00021 this->stat = -1; 00022 this->pos_cmd = 0.0; 00023 this->vel_cmd = 0.0; 00024 this->vel_avg_cmd = 0; 00025 this->acc_cmd = 0.0; 00026 this->vel_max = 2700.0 * Tdelay; //counts * Tdelay 00027 this->acc_max = 1200.0 * Tdelay; //counts/sec/sec * Tdelay 00028 this->p_higher = 0.0; 00029 this->p_lower = 0.0; 00030 this->vel_accum = 0.0; 00031 this->moveTime = 0.0; 00032 this->enc = 0; 00033 this->moveStatus = 0; //status flag to indicate state of profile movement 00034 this->moveState = 0; //used for state machine in movement profiles 00035 this->debug = 0; 00036 this->update.attach(this, &Axis::paramUpdate, Tdelay); 00037 this->axisState = 0; 00038 this->mot_I_lim = .35; 00039 this->dIdT = 0.0; 00040 this->motI = 0.0; 00041 this->motI_last = 0.0; 00042 this->mot_I_max = 0.0; 00043 this->mot_I_max_last = 0.0; 00044 this->motInvert = 0; 00045 this->dataFormat = 'r'; //default is radians 00046 // this->ctsPerDeg = cpd; //update counts per degree passed from constructor 00047 00048 this->pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval 00049 this->ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC 00050 this->motcon = new MotCon(pwm, dir); 00051 this->ptr_limit = limit; 00052 00053 //start at 0 00054 this->ls7366->LS7366_reset_counter(); 00055 this->ls7366->LS7366_quad_mode_x4(); 00056 this->ls7366->LS7366_write_DTR(0); 00057 00058 this->set_point = 0.0; 00059 this->pid->setSetPoint(this->set_point); 00060 this->enc = this->ls7366->LS7366_read_counter(); //update class variable 00061 } 00062 00063 void Axis::init(void){ 00064 //resets the controllers internals 00065 this->pid->reset(); 00066 00067 //Encoder counts limit 00068 this->pid->setInputLimits(-55000, 55000); 00069 //Pwm output from 0.0 to 1.0 00070 this->pid->setOutputLimits(-1.0, 1.0); 00071 //If there's a bias. 00072 this->pid->setBias(0.0); 00073 this->pid->setMode(AUTO_MODE); 00074 00075 this->pid->setInterval(this->Tdelay); 00076 00077 //start at 0 00078 this->ls7366->LS7366_reset_counter(); 00079 this->ls7366->LS7366_quad_mode_x4(); 00080 this->ls7366->LS7366_write_DTR(0); 00081 00082 this->set_point = 0.0; 00083 this->pid->setSetPoint(this->set_point); 00084 this->enc = this->ls7366->LS7366_read_counter(); //update class variable 00085 00086 //resets the controllers internals 00087 this->pid->reset(); 00088 //start at 0 00089 this->set_point = 0.0; 00090 this->pid->setSetPoint(0); 00091 00092 this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller 00093 } 00094 00095 void Axis::updatePIDgains(float P, float I, float D){ 00096 this->Pk = P; //120.0; //rough gains, seem to work well but could use tuning 00097 this->Ik = I; //75.0; 00098 this->Dk = D; 00099 this->pid->setTunings(this->Pk, this->Ik, this->Dk); 00100 } 00101 00102 void Axis::paramUpdate(void){ 00103 //testOut = 1; 00104 this->enc = this->ls7366->LS7366_read_counter(); 00105 this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians 00106 00107 this->vel = (this->pos - this->pos_last) * this->Tdelay; 00108 this->acc = (this->vel - this->vel_last); 00109 00110 this->pid->setSetPoint(this->set_point); 00111 00112 //Update the process variable. 00113 this->pid->setProcessValue(this->pos); 00114 //Set the new output. 00115 this->co = this->pid->compute(); 00116 00117 if(this->axisState){ 00118 if(this->motInvert==0){ 00119 this->motcon->mot_control(this->co); //send controller output to PWM motor control command 00120 } 00121 else{ 00122 this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command 00123 } 00124 } 00125 else{ 00126 this->co = 0.0; 00127 this->motcon->mot_control(0.0); //turn off motor command 00128 } 00129 00130 this->pos_last = this->pos; 00131 this->vel_last = this->vel; 00132 this->set_point_last = this->set_point; 00133 } 00134 00135 void Axis::center(void){ 00136 while((*this->ptr_limit == 1) && (this->readCurrent() < mot_I_lim)){ //limit switch not pressed and mot current not exceeded 00137 this->set_point += 100; 00138 wait(.05); 00139 if(this->debug) 00140 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()); 00141 } 00142 wait(.5); 00143 while((*this->ptr_limit == 0)){ //limit switch is pressed 00144 this->set_point -= 10; 00145 wait(.1); 00146 if(this->debug) 00147 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()); 00148 } 00149 this->zero(); //zero channel 00150 00151 // this->set_point = -(totalCounts/2.0); 00152 00153 if(this->debug) 00154 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()); 00155 // pc.printf("End Home\r\n\r\n"); 00156 } 00157 00158 void Axis::moveUpdate(void){ 00159 00160 /* if(*this->ptr_limit == 0){ 00161 this->moveState = 4; //terminate the move 00162 printf("\r\nLimit reached on axis!\r\n"); 00163 } 00164 */ 00165 if(this->debug) 00166 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()); 00167 00168 switch(this->moveState){ 00169 case 0: 00170 break; 00171 00172 //accelerate 00173 case 1: 00174 //testOut = 1; 00175 this->vel_accum += this->acc_cmd * this->Tdelay; //add acceleration to the velocity accumulator 00176 if(this->vel_avg_cmd > 0.0){ //check the sign of the movement 00177 if(this->vel_accum >= this->vel_cmd) //if the accumulator reaches or exceeds the velocity command 00178 this->vel_accum = this->vel_cmd; // only add the velocity command to the accumulator 00179 } 00180 else{ //if the sign was negative 00181 if(this->vel_accum <= this->vel_cmd) 00182 this->vel_accum = this->vel_cmd; 00183 } 00184 //testOut = 0; 00185 00186 this->set_point += this->vel_accum; 00187 //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); 00188 //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); 00189 00190 if(this->t.read()>=(this->moveTime/3.0) || (abs(this->vel_accum) > abs(this->vel_cmd))) 00191 this->moveState = 2; 00192 break; 00193 00194 //constant velocity 00195 case 2: 00196 //testOut = 1; 00197 //this->vel_accum += this->vel_cmd * this->Tdelay; 00198 this->set_point += this->vel_cmd; 00199 //testOut = 0; 00200 //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); 00201 //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); 00202 00203 if(this->t.read()>=(2.0/3.0 * this->moveTime)) 00204 this->moveState = 3; 00205 break; 00206 00207 //decelerate 00208 case 3: 00209 this->vel_accum -= this->acc_cmd * this->Tdelay; 00210 00211 this->set_point += this->vel_accum; //ramp down velocity by acceleration 00212 //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); 00213 //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); 00214 00215 if(this->vel_avg_cmd > 0.0){ 00216 if(this->pos_cmd <= this->pos){ 00217 //finish with position command 00218 this->set_point = this->pos_cmd; 00219 this->moveState = 4; 00220 } 00221 } 00222 else{ 00223 if(this->pos_cmd >= this->pos){ 00224 //finish with position command 00225 //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124 00226 this->moveState = 4; 00227 } 00228 } 00229 00230 if(this->t.read()>=this->moveTime){ 00231 //finish with position command 00232 //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124 00233 this->moveState = 4; 00234 } 00235 break; 00236 00237 case 4: 00238 this->moveProfile.detach(); //turn off the trapazoidal update ticker 00239 this->t.stop(); 00240 this->moveState = 0; 00241 break; 00242 }//switch moveStatus 00243 return; 00244 } 00245 00246 // position - encoder position to move to 00247 // time - duration of the movement 00248 void Axis::moveTrapezoid(float positionCmd, float time){ 00249 this->pos_cmd = positionCmd; 00250 this->moveTime = time; 00251 float enc_distance = this->pos_cmd - (float)this->enc;// * 1.0/con0.countsPerDeg * 180.0/PI; 00252 00253 this->vel_avg_cmd = enc_distance / time; 00254 this->vel_cmd = 1.5 * this->vel_avg_cmd * this->Tdelay; 00255 this->acc_cmd = 4.5 * (enc_distance / (this->moveTime * this->moveTime)) * this->Tdelay; 00256 00257 //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); 00258 00259 //establish encoder velocities and accelerations for position control per Tdelay 00260 this->vel_accum = 0.0; 00261 00262 // this->set_point = this->pos; 00263 this->moveState = 1; 00264 this->t.reset(); 00265 this->t.start(); 00266 this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay); 00267 } 00268 00269 float Axis::readCurrent(void){ 00270 this->motI = (this->_analog.read() * 3.3) / .525; //525mV per amp 00271 if(abs(this->motI) > this->mot_I_max){ //update the max motor current measured 00272 this->mot_I_max = this->motI; 00273 } 00274 this->dIdT = motI - motI_last; 00275 this->motI_last = motI; 00276 return this->motI; 00277 } 00278 00279 void Axis::axisOff(void){ 00280 this->co = 0; 00281 this->axisState = 0; 00282 this->update.detach(); 00283 } 00284 00285 void Axis::axisOn(void){ 00286 this->co = 0.0; 00287 this->pid->reset(); 00288 //start at 0 if not already homed 00289 if(this->stat != 0){ 00290 this->set_point = 0.0; 00291 this->pid->setSetPoint(0); 00292 } 00293 00294 this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller 00295 this->axisState = 1; 00296 this->update.attach(this, &Axis::paramUpdate, this->Tdelay); 00297 } 00298 00299 void Axis::zero(void){ 00300 00301 this->ls7366->LS7366_reset_counter(); 00302 this->ls7366->LS7366_quad_mode_x4(); 00303 this->ls7366->LS7366_write_DTR(0); 00304 00305 this->enc = this->ls7366->LS7366_read_counter(); 00306 00307 this->pos = 0.0; 00308 this->pid->reset(); //added to avoid possible itegral windup effects on instant position change 20170616 00309 this->set_point = 0.0; 00310 this->pid->setSetPoint(0); 00311 00312 } 00313 00314 void Axis::writeEncoderValue(long value){ 00315 00316 this->ls7366->LS7366_write_DTR(value); 00317 00318 this->set_point = (float)value; 00319 this->pid->setSetPoint(this->set_point); 00320 }
Generated on Tue Jul 26 2022 19:48:09 by 1.7.2