Diff: Axis.cpp

Revision:

1:cd249816dba8

Parent:

0:cf7192f9f99a

Child:

2:653433f4ee72

--- a/Axis.cpp	Mon Aug 31 17:14:20 2015 +0000
+++ b/Axis.cpp	Thu Sep 24 16:47:02 2015 +0000
@@ -4,7 +4,7 @@
 #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) {
+Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) {
     _cs = 1;           // Initialize chip select as off (high)
     _pwm = 0.0;
     _dir = 0;
@@ -38,6 +38,7 @@
     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;
     
     //start at 0    
     this->ls7366->LS7366_reset_counter();
@@ -50,7 +51,7 @@
 }
 
 void Axis::init(float cpd){
-    _limit.mode(PullUp);
+    //_limit.mode(PullUp);
     this->countsPerDeg = cpd;    //90.0/10680.0;
     //resets the controllers internals
     this->pid->reset();
@@ -108,22 +109,23 @@
 
 void Axis::home(long halfcounts){
     this->pid->setInputLimits(-20000.0, 20000.0); 
-    while(this->_limit == 1){
+    while(*this->ptr_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);
+            printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit);
     }
     this->set_point -= 1000;
     this->pid->setSetPoint(this->set_point);
     wait(.5);
-    while(this->_limit == 1){
+    
+    while(*this->ptr_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);
+        printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit);
     }
 
     this->ls7366->LS7366_write_DTR(0);      //zero encoder channel
@@ -135,7 +137,7 @@
         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);
+            printf("T=%.2f SP=%.3f co=%.3f pos=%.3f vel=%.3f acc=%.3f limit=%d\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit);
     }
     this->set_point = -halfcounts;
     this->pid->setSetPoint(this->set_point);
@@ -207,22 +209,28 @@
             //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)
+                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)
-                    this->moveState = 4;                
+                if(this->pos_cmd >= this->pos){
+                    //finish with position command
+                    this->set_point = this->pos_cmd;
+                    this->moveState = 4;
+                }
             }
         
             if(this->t.read()>=this->moveTime){
+                //finish with position command
+                this->set_point = this->pos_cmd;
                 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;