Diff: Axis.cpp

Revision:

0:cf7192f9f99a

Child:

1:cd249816dba8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis.cpp	Mon Aug 31 17:14:20 2015 +0000
@@ -0,0 +1,255 @@
+
+#include "Axis.h"
+#include "LS7366.h"
+#include "MotCon.h"
+#include "PID.h"
+
+Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir), _limit(limit) {
+    _cs = 1;           // Initialize chip select as off (high)
+    _pwm = 0.0;
+    _dir = 0;
+    co = 0.0;
+    Tdelay = .01;
+    Pk = 120.0; //80.0;      //rough gains, seem to work well but could use tuning   
+    Ik = 55.0;  //35.0;
+    Dk = 0.0;
+    set_point = 0.0;
+    set_point_last = 0.0;
+    pos = 0.0;
+    vel = 0.0;
+    acc = 0.0;
+    pos_cmd = 0.0;
+    vel_cmd = 0.0;
+    vel_avg_cmd = 0;
+    acc_cmd = 0.0;
+    vel_max = 2700.0 * Tdelay; //counts * Tdelay
+    acc_max = 1200.0 * Tdelay; //counts/sec/sec  * Tdelay
+    p_higher = 0.0;
+    p_lower = 0.0;
+    vel_accum = 0.0;
+    moveTime = 0.0;
+    countsPerDeg = 0.0;
+    enc = 0;
+    moveStatus = 0;     //status flag to indicate state of profile movement
+    moveState = 0;      //used for state machine in movement profiles
+    debug = 0;   
+    update.attach(this, &Axis::paramUpdate, Tdelay);    
+    
+    pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval
+    ls7366 = new LS7366(spi, cs);  //LS7366 encoder interface IC
+    motcon = new MotCon(pwm, dir);
+    
+    //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(float cpd){
+    _limit.mode(PullUp);
+    this->countsPerDeg = cpd;    //90.0/10680.0;
+    //resets the controllers internals
+    this->pid->reset();
+
+    //Encoder counts limit
+    this->pid->setInputLimits(-20000.0, 20000.0); 
+    //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::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();
+
+    this->motcon->mot_control(this->co); //send controller output to PWM motor control command
+    
+    this->pos_last = this->pos;
+    this->vel_last = this->vel;
+    this->set_point_last = this->set_point;
+    //testOut = 0;
+}
+
+void Axis::home(long halfcounts){
+    this->pid->setInputLimits(-20000.0, 20000.0); 
+    while(this->_limit == 1){
+       this->set_point += 100;
+       this->pid->setSetPoint(this->set_point);
+       wait(.05);
+       if(this->debug)
+            printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);
+    }
+    this->set_point -= 1000;
+    this->pid->setSetPoint(this->set_point);
+    wait(.5);
+    while(this->_limit == 1){
+       this->set_point += 10;
+       this->pid->setSetPoint(this->set_point);
+       wait(.02);
+       if(this->debug)     
+        printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);
+    }
+
+    this->ls7366->LS7366_write_DTR(0);      //zero encoder channel
+    this->set_point = 0.0;
+    this->pid->setSetPoint(this->set_point);
+
+    this->pid->setInputLimits(-17000.0, 0.0); //reset span limits
+    for(int positionCmd = 0;positionCmd > -halfcounts;positionCmd-=30){
+        this->set_point = positionCmd;                    //move arm to center
+        wait(.01);
+        if(this->debug)
+            printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);
+    }
+    this->set_point = -halfcounts;
+    this->pid->setSetPoint(this->set_point);
+    
+    //let PID settle to set point
+    while((this->enc > (this->set_point + 100)) || (this->enc < (this->set_point - 100))){
+        //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);
+        wait(.01);
+    }
+    //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);
+    
+    this->ls7366->LS7366_write_DTR(0);      //zero encoder channel
+    this->set_point = 0.0;
+    this->pid->setSetPoint(this->set_point);
+
+    this->pid->setInputLimits(-6500.0, 6500.0); //reset span limits
+    this->pid->setSetPoint(this->set_point);
+        
+//    pc.printf("Home:T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f Pk=%.2f Ik=%.2f vel=%.3f acc=%.3f\r\n", t.read(), this->set_point, this->co, this->enc, this->pos, this->Pk, this->Ik, this->vel, this->acc);    
+//    pc.printf("End Home\r\n\r\n");
+}
+
+void Axis::moveUpdate(void){                    
+    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)
+                    this->moveState = 4;
+            }
+            else{
+                if(this->pos_cmd >= this->pos)
+                    this->moveState = 4;                
+            }
+        
+            if(this->t.read()>=this->moveTime){
+                this->moveState = 4;    
+            }
+        break;
+        
+        case 4:        
+            //finish with position command
+            this->set_point = this->pos_cmd;
+            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 = 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);
+}