Diff: Axis.cpp

Revision:

5:79dcaa63700c

Parent:

4:4079f92f9c26

Child:

7:d0458137d6e0

--- a/Axis.cpp	Wed Oct 07 17:32:13 2015 +0000
+++ b/Axis.cpp	Thu Dec 17 19:06:27 2015 +0000
@@ -34,7 +34,8 @@
     this->moveState = 0;      //used for state machine in movement profiles
     this->debug = 0;   
     this->update.attach(this, &Axis::paramUpdate, Tdelay);
-    this->axisState = 0;    
+    this->axisState = 0;
+    this->mot_I_lim = .35;    
     
     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
@@ -114,54 +115,24 @@
 void Axis::center(void){
     this->pid->setInputLimits(-this->totalCounts, this->totalCounts); 
     
-    while(*this->ptr_limit == 1){ // && ({
+    while((*this->ptr_limit == 1) && (this->readCurrent() < mot_I_lim)){ //limit switch not pressed and mot current not exceeded
        this->set_point += 100;
-       this->pid->setSetPoint(this->set_point);
-       wait(.05);
+       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());
     }
-    wait(.2);
-    this->set_point -= 1000;
-    this->pid->setSetPoint(this->set_point);
-    wait(.6);
-    
-    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 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->ls7366->LS7366_write_DTR(0);      //zero encoder channel
-    this->set_point = 0.0;
-    this->pid->setSetPoint(this->set_point);
-
-    this->pid->setInputLimits(-(this->totalCounts)/2.0, 0.0); //reset span limits
-    for(int positionCmd = 0;positionCmd > -this->totalCounts/2.0;positionCmd-=30){
-        this->set_point = positionCmd;                    //move arm to center
-        wait(.01);
-        if(this->debug)
+    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->set_point = -(this->totalCounts)/2.0;
-    this->pid->setSetPoint(this->set_point);
+    this->zero();   //zero channel        
     
-    //let PID settle to set point
-    while((this->enc > (this->set_point + 100)) || (this->enc < (this->set_point - 100))){
-        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(.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(-(this->totalCounts)/2.0, (this->totalCounts)/2.0); //reset span limits
-    this->pid->setSetPoint(this->set_point);
+    this->set_point = -(totalCounts/2.0);
+    wait(5.0);
+    this->zero();
         
     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());
@@ -170,10 +141,11 @@
 
 void Axis::moveUpdate(void){               
 
-    if(*this->ptr_limit == 0){
+/*    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());
 
@@ -234,14 +206,14 @@
             else{
                 if(this->pos_cmd >= this->pos){
                     //finish with position command
-                    this->set_point = this->pos_cmd;
+                    //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;
+                //this->set_point = this->pos_cmd;     //May be causing jerk after multi-axis movements  J Bradshaw 20151124
                 this->moveState = 4;    
             }
         break;
@@ -297,4 +269,13 @@
                                                         
     this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller
     this->axisState = 1;
+}
+
+void Axis::zero(void){
+    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(0);    
 }
\ No newline at end of file