Revision 12:7a7fe3baf733, committed 2019-01-09

Comitter:

jebradshaw

Date:

Wed Jan 09 13:35:44 2019 +0000

Parent:

11:93d924320ddc

Commit message:

position, velocity, acceleration modes all in radians; Axis_Init function takes encoder counts/revolution

Changed in this revision

--- 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);
+}
+

--- a/Axis.h	Tue Nov 15 15:41:33 2016 +0000
+++ b/Axis.h	Wed Jan 09 13:35:44 2019 +0000
@@ -1,4 +1,6 @@
-
+// Updated Axis Class on 20190108 to include mode change (Position, Velocity, Acceleration)
+//  all in radians, rad/sec, rad/sec/sec
+// J. Bradshaw
 
 #ifndef MBED_AXIS_H
 #define MBED_AXIS_H
@@ -10,17 +12,39 @@
 
 class Axis{
 public:
+    /** Create a closed loop controller connected to the specified pins
+     *
+     * @param _spi address of the spi object for LS7366 encoder IC communication
+     * @param _cs chip select signal used for the LS7366 encoder IC spi addressing
+     * @param _pwm pulse width modulation output pin for motor control signal     
+     * @param _dir DigitalOut pin to control the motor direction pin1
+     * @param _analog analog input pin for monitoring current
+     * @param _limit pointer to integer object for limit switch detection
+     */
     Axis(SPI& _spi, PinName _cs, PinName _pwm, PinName _dir, PinName _analog, int* limit);    
+    /** Create a closed loop controller connected to the specified pins
+     *
+     * @param _spi address of the spi object for LS7366 encoder IC communication
+     * @param _cs chip select signal used for the LS7366 encoder IC spi addressing
+     * @param _pwm pulse width modulation output pin for motor control signal     
+     * @param _dir DigitalOut pin to control the motor direction pin 1
+     * @param _dir2 DigitalOut pin to control the motor direction pin 2
+     * @param _analog analog input pin for monitoring current
+     * @param _limit pointer to integer object for limit switch detection
+     */
+    Axis(SPI& _spi, PinName _cs, PinName _pwm, PinName _dir, PinName _dir2, PinName _analog, int* limit);
     void paramUpdate(void);
-    void center(void);
-    void init(void);
+    void init(float encCountsPerRev);
     void moveTrapezoid(float position, float time);
-    void moveUpdate(void);
+    void moveScurve(float position, float time);
+    void moveUpdateTrapezoid(void);
     float readCurrent(void);
     void axisOff(void);
     void axisOn(void);
     void zero(void);
     void writeEncoderValue(long value);
+    void updatePIDgains(float P, float I, float D);
+    void changeMoveMode(int mode);
     
     long enc;       //used to return the data from the LS7366 encoder chip
     float co;       // = 0.0;
@@ -42,8 +66,11 @@
     int moveState;
     int debug;
     int *ptr_limit;
-    float motCurrent;   //motor current read from readCurrent() function
+    float motI;   //motor current read from readCurrent() function
+    volatile float motI_last;
     float mot_I_lim;    //max current limit
+    float dIdT;
+    float mot_I_max, mot_I_max_last;
     int axisState;
     int motInvert;
     char dataFormat;    //'r'=radians (default), 'd'=degrees, 'e'=encoder counts
@@ -51,6 +78,8 @@
     float pos_deg, vel_deg;  //current position measurement in degrees
     float ctsPerDeg;
     int busyflag;
+    int moveMode;
+    float countsPerRev;   
 
     Ticker update;
     Ticker moveProfile;
@@ -65,6 +94,7 @@
     DigitalOut _cs;
     PwmOut _pwm;
     DigitalOut _dir;
+    DigitalOut _dir2;
     AnalogIn _analog;
 };
 

--- a/MotCon.lib	Tue Nov 15 15:41:33 2016 +0000
+++ b/MotCon.lib	Wed Jan 09 13:35:44 2019 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/jebradshaw/code/MotCon/#709761ee0a14
+https://developer.mbed.org/users/jebradshaw/code/MotCon2/#709761ee0a14

--- a/PID.lib	Tue Nov 15 15:41:33 2016 +0000
+++ b/PID.lib	Wed Jan 09 13:35:44 2019 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/aberk/code/PID/#6e12a3e5af19
+https://mbed.org/users/aberk/code/PID/#324821248e5e