Diff: Axis.cpp

Revision:

12:7a7fe3baf733

Parent:

11:93d924320ddc

--- a/Axis.cpp	Tue Nov 15 15:41:33 2016 +0000
+++ b/Axis.cpp	Wed Jan 09 13:35:44 2019 +0000
@@ -4,14 +4,73 @@
 #include "MotCon.h"
 #include "PID.h"
 
-Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit): _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) , _dir2(NC), _analog(analog){
     this->_cs = 1;           // Initialize chip select as off (high)
     this->_pwm = 0.0;
     this->_dir = 0;
+    this->_dir2 = 0;
     this->co = 0.0;
     this->Tdelay = .01;
-    this->Pk = 140.0; //120.0;      //rough gains, seem to work well but could use tuning   
-    this->Ik = 95.0;  //75.0;
+    this->Pk = 450.0; //120.0;      //rough gains, seem to work well but could use tuning   
+    this->Ik = 105.0;  //75.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;
+    this->acc_cmd = 0.0;
+    this->vel_max = 2700.0 * Tdelay; //counts * Tdelay
+    this->acc_max = 1200.0 * Tdelay; //counts/sec/sec  * Tdelay
+    this->p_higher = 0.0;
+    this->p_lower = 0.0;
+    this->vel_accum = 0.0;
+    this->moveTime = 0.0;
+    this->enc = 0;
+    this->moveStatus = 0;     //status flag to indicate state of profile movement
+    this->moveState = 0;      //used for state machine in movement profiles
+    this->debug = 0;   
+    this->update.attach(this, &Axis::paramUpdate, this->Tdelay);
+    this->axisState = 0;
+    this->mot_I_lim = .35;    
+    this->dIdT = 0.0;
+    this->motI = 0.0;
+    this->motI_last = 0.0;
+    this->mot_I_max = 0.0;
+    this->mot_I_max_last = 0.0;
+    this->motInvert = 0;
+    this->dataFormat = 'r'; //default is radians
+//    this->ctsPerDeg = cpd;  //update counts per degree passed from constructor
+    this->moveMode=0;   //0 is position (default), 1 is velocity, 2 is acceleration
+    
+    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
+    this->motcon = new MotCon(pwm, dir);
+    this->ptr_limit = limit;
+    
+    //start at 0    
+    this->ls7366->LS7366_reset_counter();
+    this->ls7366->LS7366_quad_mode_x4();       
+    this->ls7366->LS7366_write_DTR(0);
+    
+    this->set_point = 0.0;
+    this->pid->setSetPoint(this->set_point);
+    this->enc = this->ls7366->LS7366_read_counter();  //update class variable    
+}
+
+Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName dir2, PinName analog, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _dir2(dir2),_analog(analog){
+    this->_cs = 1;           // Initialize chip select as off (high)
+    this->_pwm = 0.0;
+    this->_dir = 0;
+    this->_dir2 = 0;
+    this->co = 0.0;
+    this->Tdelay = .01;
+    this->Pk = 450.0; //120.0;      //rough gains, seem to work well but could use tuning   
+    this->Ik = 105.0;  //75.0;
     this->Dk = 0.0;
     this->set_point = 0.0;
     this->set_point_last = 0.0;
@@ -36,13 +95,19 @@
     this->update.attach(this, &Axis::paramUpdate, Tdelay);
     this->axisState = 0;
     this->mot_I_lim = .35;    
+    this->dIdT = 0.0;
+    this->motI = 0.0;
+    this->motI_last = 0.0;
+    this->mot_I_max = 0.0;
+    this->mot_I_max_last = 0.0;
     this->motInvert = 0;
     this->dataFormat = 'r'; //default is radians
 //    this->ctsPerDeg = cpd;  //update counts per degree passed from constructor
+    this->moveMode=0;   //0 is position (default), 1 is velocity, 2 is acceleration
     
-    this->pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval
+    this->pid = new PID(0.0,0.0,0.0,this->Tdelay); //Kc, Ti, Td, interval
     this->ls7366 = new LS7366(spi, cs);  //LS7366 encoder interface IC
-    this->motcon = new MotCon(pwm, dir);
+    this->motcon = new MotCon(pwm, dir, dir2);
     this->ptr_limit = limit;
     
     //start at 0    
@@ -55,7 +120,7 @@
     this->enc = this->ls7366->LS7366_read_counter();  //update class variable    
 }
 
-void Axis::init(void){ 
+void Axis::init(float encCountsPerRev){ 
     //resets the controllers internals
     this->pid->reset();
 
@@ -69,6 +134,9 @@
     
     this->pid->setInterval(this->Tdelay);
       
+    //set the encoder counts per revolution/linear throw
+    this->countsPerRev=encCountsPerRev;
+      
     //start at 0    
     this->ls7366->LS7366_reset_counter();
     this->ls7366->LS7366_quad_mode_x4();       
@@ -86,19 +154,33 @@
                                                         
     this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller    
 }
-    
+
+void Axis::updatePIDgains(float P, float I, float D){
+    this->Pk = P; //120.0;      //rough gains, seem to work well but could use tuning   
+    this->Ik = I;  //75.0;
+    this->Dk = D;
+    this->pid->setTunings(this->Pk, this->Ik, this->Dk);
+}
+
 void Axis::paramUpdate(void){    
     //testOut = 1;    
     this->enc = this->ls7366->LS7366_read_counter();
-    this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians
-        
-    this->vel = (this->pos - this->pos_last) * this->Tdelay;
-    this->acc = (this->vel - this->vel_last);
+    //this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians
+    
+    this->pos =  ((float)this->enc / this->countsPerRev) * 6.28319; //Conver pos to radians
+    this->vel = (this->pos - this->pos_last) * (1.0/this->Tdelay); //current vel in radians
+    this->acc = (this->vel - this->vel_last) * (1.0/this->Tdelay);
 
     this->pid->setSetPoint(this->set_point);    
     
     //Update the process variable.
-    this->pid->setProcessValue(this->pos);
+    if(this->moveMode == 0)
+        this->pid->setProcessValue(this->pos);
+    if(this->moveMode == 1)
+        this->pid->setProcessValue(this->vel);
+    if(this->moveMode == 2)
+        this->pid->setProcessValue(this->acc);
+        
     //Set the new output.
     this->co = this->pid->compute();
 
@@ -120,30 +202,7 @@
     this->set_point_last = this->set_point;
 }
 
-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);
-       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(.5);
-    while((*this->ptr_limit == 0)){ //limit switch is pressed
-       this->set_point -= 10;
-       wait(.1);
-       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());
-    }
-    this->zero();   //zero channel        
-    
-//    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());
-//    pc.printf("End Home\r\n\r\n");
-}
-
-void Axis::moveUpdate(void){               
+void Axis::moveUpdateTrapezoid(void){               
 
 /*    if(*this->ptr_limit == 0){
         this->moveState = 4;    //terminate the move
@@ -251,12 +310,17 @@
     this->moveState = 1;
     this->t.reset();
     this->t.start();
-    this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay);
+    this->moveProfile.attach(this, &Axis::moveUpdateTrapezoid, this->Tdelay);
 }
 
 float Axis::readCurrent(void){    
-    motCurrent = (this->_analog.read() * 3.3) / .525;  //525mV per amp
-    return motCurrent;
+    this->motI = (this->_analog.read() * 3.3) / .525;  //525mV per amp
+    if(this->motI > this->mot_I_max){
+        this->mot_I_max = this->motI;
+    }
+    this->dIdT = motI - motI_last;
+    this->motI_last = motI;
+    return this->motI;
 }
 
 void Axis::axisOff(void){
@@ -267,10 +331,12 @@
 
 void Axis::axisOn(void){    
     this->co = 0.0;
-    this->pid->reset();              
-    //start at 0
-    this->set_point = 0.0;
-    this->pid->setSetPoint(0); 
+    this->pid->reset();
+    //start at 0 if not already homed   (commented out on 20181217 to accomodate vel mode
+//    if(this->stat != 0){
+//        this->set_point = 0.0;
+//        this->pid->setSetPoint(0); 
+//    }
                                                         
     this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller
     this->axisState = 1;
@@ -286,6 +352,7 @@
     this->enc = this->ls7366->LS7366_read_counter();
     
     this->pos = 0.0;
+    this->pid->reset();     //added to avoid possible itegral windup effects on instant position change 20170616
     this->set_point = 0.0;
     this->pid->setSetPoint(0);    
     
@@ -297,4 +364,27 @@
         
     this->set_point = (float)value;
     this->pid->setSetPoint(this->set_point);        
-}
\ No newline at end of file
+}
+
+//mode = 0 position, 1 - volocity, 2 - acceleration
+void Axis::changeMoveMode(int mode){    
+        
+    if(mode == 0){
+        this->Tdelay = .01;
+        //Encoder counts limit
+        this->moveMode = 0;
+    }
+    else if(mode == 1){
+        this->Tdelay = .05;
+        //this->pid->setInputLimits(-100.0, 100.0);
+        this->moveMode = 1;        
+    }
+    else if(mode == 2){
+        this->Tdelay = .05;
+        //this->pid->setInputLimits(-100.0, 100.0);
+        this->moveMode = 2;  
+    }
+    this->pid->setInterval(this->Tdelay);
+    this->update.attach(this, &Axis::paramUpdate, this->Tdelay);
+}
+