Diff: Axis.cpp

Revision:

9:7bc59203ce98

Parent:

8:7e399d7c990d

Child:

10:32faca5a2577

diff -r 7e399d7c990d -r 7bc59203ce98 Axis.cpp
--- a/Axis.cpp	Mon Jul 11 18:26:26 2016 +0000
+++ b/Axis.cpp	Mon Aug 29 19:42:07 2016 +0000
@@ -4,7 +4,9 @@
 #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){
+#define RETRIES_MAX 10000
+
+Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit, int *busy): _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;
@@ -39,7 +41,10 @@
     this->motInvert = 0;
     this->dataFormat = 'r'; //default is radians
 //    this->ctsPerDeg = cpd;  //update counts per degree passed from constructor
+    this->ptr_busy = busy;
     
+    while(*this->ptr_busy);
+    *this->ptr_busy=1;
     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);
@@ -53,9 +58,21 @@
     this->set_point = 0.0;
     this->pid->setSetPoint(this->set_point);
     this->enc = this->ls7366->LS7366_read_counter();  //update class variable    
+    *this->ptr_busy=0;
 }
 
 void Axis::init(void){
+    int retries=0;
+    DigitalOut led3(P1_20);
+    while(*this->ptr_busy==1){        
+        if(retries>=RETRIES_MAX){
+            led3=1;
+        }
+        wait_us(5);
+        retries++;
+    }
+    //now set the busy flag
+    *this->ptr_busy=1;   
     //resets the controllers internals
     this->pid->reset();
 
@@ -85,9 +102,22 @@
     this->pid->setSetPoint(0); 
                                                         
     this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller    
+    
+    *this->ptr_busy=0;   //clear the busy flag
 }
     
-void Axis::paramUpdate(void){
+void Axis::paramUpdate(void){    
+    int retries=0;
+    DigitalOut led3(P1_20);
+    while(*this->ptr_busy==1){        
+        if(retries>=RETRIES_MAX){
+            led3=1;
+        }
+        wait_us(5);
+        retries++;
+    }
+    
+    *this->ptr_busy=1;   //set the busy flag
     //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
@@ -119,6 +149,8 @@
     this->vel_last = this->vel;
     this->set_point_last = this->set_point;
     //testOut = 0;
+    
+    *this->ptr_busy=0;   //clear the busy flag
 }
 
 void Axis::center(void){    
@@ -135,6 +167,7 @@
        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());
     }
+    while(*this->ptr_busy==1);
     this->zero();   //zero channel        
     
 //    this->set_point = -(totalCounts/2.0);
@@ -263,9 +296,10 @@
 void Axis::axisOff(void){
     this->co = 0;
     this->axisState = 0;
+    this->update.detach();
 }
 
-void Axis::axisOn(void){
+void Axis::axisOn(void){    
     this->co = 0.0;
     this->pid->reset();              
     //start at 0
@@ -274,20 +308,52 @@
                                                         
     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){
+    int retries=0;
+    DigitalOut led3(P1_20);
+    while(*this->ptr_busy==1){        
+        if(retries>=RETRIES_MAX){
+            led3=!led3;
+            retries=0;
+        }
+        wait_us(5);
+        retries++;
+    }    
+    *this->ptr_busy=1;   //set the busy flag        
+
     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->ptr_busy=0;   //clear the busy flag
+    
+    this->pos = 0.0;
     this->set_point = 0.0;
     this->pid->setSetPoint(0);    
+    
 }
 
 void Axis::writeEncoderValue(long value){
+    int retries=0;
+    DigitalOut led3(P1_20);
+    while(*this->ptr_busy==1){
+        if(retries>=RETRIES_MAX){
+            led3=1;
+        }
+        wait_us(5);
+        retries++;
+    }
+    *this->ptr_busy=1;   //set the busy flag
+    
     this->ls7366->LS7366_write_DTR(value);
     
+    *this->ptr_busy=0;   //clear the busy flag
+        
     this->set_point = (float)value;
-    this->pid->setSetPoint(this->set_point);    
+    this->pid->setSetPoint(this->set_point);        
 }
\ No newline at end of file