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 }