Diff: Axis.cpp

Revision:

4:4079f92f9c26

Parent:

3:71447d4fb4f0

Child:

5:79dcaa63700c

--- a/Axis.cpp	Thu Oct 01 14:55:54 2015 +0000
+++ b/Axis.cpp	Wed Oct 07 17:32:13 2015 +0000
@@ -3,44 +3,43 @@
 #include "LS7366.h"
 #include "MotCon.h"
 #include "PID.h"
-#define MIN_MOT_COL .06
 
 Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit, float totalCnts): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _analog(analog){
-    _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;
-    totalCounts = totalCnts;
-    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);
-    axisState = 0;    
+    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 = 120.0; //80.0;      //rough gains, seem to work well but could use tuning   
+    this->Ik = 55.0;  //35.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->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->totalCounts = totalCnts;
+    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;    
     
-    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);
-    ptr_limit = limit;
+    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();
@@ -53,7 +52,6 @@
 }
 
 void Axis::init(void){
-    //_limit.mode(PullUp);
     //resets the controllers internals
     this->pid->reset();
 
@@ -100,9 +98,13 @@
     //Set the new output.
     this->co = this->pid->compute();
 
-    //this->motcon->mot_control(-(this->co)); //send controller output to PWM motor control command    
-    this->motcon->mot_control(this->co); //send controller output to PWM motor control command    
-    
+    if(this->axisState)
+        this->motcon->mot_control(this->co); //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;
@@ -258,7 +260,7 @@
 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;
+    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;
@@ -276,17 +278,23 @@
     this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay);
 }
 
-float Axis::readCurrent(void){
-    this->motCurrent = this->_analog.read() * 3.3;
-    return this->motCurrent;
+float Axis::readCurrent(void){    
+    motCurrent = this->_analog.read() * 3.3;
+    return motCurrent;
 }
 
 void Axis::axisOff(void){
-    update.detach();
+    this->co = 0;
     this->axisState = 0;
 }
 
 void Axis::axisOn(void){
-    update.attach(this, &Axis::paramUpdate, Tdelay);
+    this->co = 0.0;
+    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
     this->axisState = 1;
 }
\ No newline at end of file