Diff: Axis.cpp

Revision:

0:d7f0715676f4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis.cpp	Thu Nov 08 17:54:30 2018 +0000
@@ -0,0 +1,320 @@
+
+#include "Axis.h"
+#include "LS7366.h"
+#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){
+    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 = 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, 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->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    
+}
+
+void Axis::init(void){ 
+    //resets the controllers internals
+    this->pid->reset();
+
+    //Encoder counts limit
+    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.
+    this->pid->setBias(0.0);
+    this->pid->setMode(AUTO_MODE);
+    
+    this->pid->setInterval(this->Tdelay);
+      
+    //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
+    
+        //resets the controllers internals
+    this->pid->reset();              
+    //start at 0
+    this->set_point = 0.0;
+    this->pid->setSetPoint(0); 
+                                                        
+    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->pid->setSetPoint(this->set_point);    
+    
+    //Update the process variable.
+    this->pid->setProcessValue(this->pos);
+    //Set the new output.
+    this->co = this->pid->compute();
+
+    if(this->axisState){
+        if(this->motInvert==0){
+            this->motcon->mot_control(this->co); //send controller output to PWM motor control command    
+        }
+        else{
+            this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command    
+        }
+    }
+    else{
+        this->co = 0.0;
+        this->motcon->mot_control(0.0);     //turn off motor command
+    }
+        
+    this->pos_last = this->pos;
+    this->vel_last = this->vel;
+    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){               
+
+/*    if(*this->ptr_limit == 0){
+        this->moveState = 4;    //terminate the move
+        printf("\r\nLimit reached on axis!\r\n");
+    }
+*/
+    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());
+
+    switch(this->moveState){
+        case 0:        
+        break;
+            
+        //accelerate
+        case 1:
+            //testOut = 1;
+            this->vel_accum += this->acc_cmd * this->Tdelay;    //add acceleration to the velocity accumulator
+            if(this->vel_avg_cmd > 0.0){                        //check the sign of the movement
+                if(this->vel_accum >= this->vel_cmd)            //if the accumulator reaches or exceeds the velocity command
+                    this->vel_accum = this->vel_cmd;            // only add the velocity command to the accumulator
+            }
+            else{                                               //if the sign was negative
+                if(this->vel_accum <= this->vel_cmd)
+                    this->vel_accum = this->vel_cmd;
+            }
+            //testOut = 0;
+                 
+            this->set_point += this->vel_accum;
+            //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); 
+            //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); 
+
+            if(this->t.read()>=(this->moveTime/3.0) || (abs(this->vel_accum) > abs(this->vel_cmd)))
+                this->moveState = 2;
+        break;
+        
+        //constant velocity
+        case 2:        
+            //testOut = 1;
+            //this->vel_accum += this->vel_cmd * this->Tdelay;
+            this->set_point += this->vel_cmd;
+            //testOut = 0;
+            //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); 
+            //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); 
+            
+            if(this->t.read()>=(2.0/3.0 * this->moveTime))
+                this->moveState = 3;
+        break;
+     
+        //decelerate
+        case 3:                       
+            this->vel_accum -= this->acc_cmd * this->Tdelay;
+        
+            this->set_point += this->vel_accum;             //ramp down velocity by acceleration       
+            //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); 
+            //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); 
+        
+            if(this->vel_avg_cmd > 0.0){
+                if(this->pos_cmd <= this->pos){
+                    //finish with position command
+                    this->set_point = this->pos_cmd;
+                    this->moveState = 4;
+                }
+            }
+            else{
+                if(this->pos_cmd >= this->pos){
+                    //finish with position command
+                    //this->set_point = this->pos_cmd;  //May be causing jerk after multi-axis movements  J Bradshaw 20151124
+                    this->moveState = 4;
+                }
+            }
+        
+            if(this->t.read()>=this->moveTime){
+                //finish with position command
+                //this->set_point = this->pos_cmd;     //May be causing jerk after multi-axis movements  J Bradshaw 20151124
+                this->moveState = 4;    
+            }
+        break;
+        
+        case 4:        
+            this->moveProfile.detach();   //turn off the trapazoidal update ticker
+            this->t.stop();
+            this->moveState = 0;     
+        break;
+    }//switch moveStatus
+    return;
+}
+
+// position - encoder position to move to
+// time - duration of the movement
+void Axis::moveTrapezoid(float positionCmd, float time){
+    this->pos_cmd = positionCmd;
+    this->moveTime = time;
+    float enc_distance = this->pos_cmd - (float)this->enc;// * 1.0/con0.countsPerDeg * 180.0/PI;
+       
+    this->vel_avg_cmd = enc_distance / time;    
+    this->vel_cmd = 1.5 * this->vel_avg_cmd * this->Tdelay;
+    this->acc_cmd = 4.5 * (enc_distance / (this->moveTime * this->moveTime)) * this->Tdelay;
+    
+    //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);
+    
+    //establish encoder velocities and accelerations for position control per Tdelay
+    this->vel_accum = 0.0;
+    
+//  this->set_point = this->pos;
+    this->moveState = 1;
+    this->t.reset();
+    this->t.start();
+    this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay);
+}
+
+float Axis::readCurrent(void){    
+    this->motI = (this->_analog.read() * 3.3) / .525;  //525mV per amp
+    if(abs(this->motI) > this->mot_I_max){   //update the max motor current measured
+        this->mot_I_max = this->motI;
+    }
+    this->dIdT = motI - motI_last;
+    this->motI_last = motI;
+    return this->motI;
+}
+
+void Axis::axisOff(void){
+    this->co = 0;
+    this->axisState = 0;
+    this->update.detach();
+}
+
+void Axis::axisOn(void){    
+    this->co = 0.0;
+    this->pid->reset();
+    //start at 0 if not already homed
+    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;
+    this->update.attach(this, &Axis::paramUpdate, this->Tdelay);        
+}
+
+void Axis::zero(void){       
+
+    this->ls7366->LS7366_reset_counter();
+    this->ls7366->LS7366_quad_mode_x4();       
+    this->ls7366->LS7366_write_DTR(0);
+    
+    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);    
+    
+}
+
+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