Diff: Axis.cpp

Revision:

8:7e399d7c990d

Parent:

7:d0458137d6e0

Child:

9:7bc59203ce98

--- a/Axis.cpp	Thu May 19 12:41:53 2016 +0000
+++ b/Axis.cpp	Mon Jul 11 18:26:26 2016 +0000
@@ -4,20 +4,21 @@
 #include "MotCon.h"
 #include "PID.h"
 
-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){
+Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit): _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;
     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->Ik = 75.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->stat = -1;
     this->pos_cmd = 0.0;
     this->vel_cmd = 0.0;
     this->vel_avg_cmd = 0;
@@ -28,7 +29,6 @@
     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
@@ -38,6 +38,7 @@
     this->mot_I_lim = .35;    
     this->motInvert = 0;
     this->dataFormat = 'r'; //default is radians
+//    this->ctsPerDeg = cpd;  //update counts per degree passed from constructor
     
     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
@@ -59,7 +60,7 @@
     this->pid->reset();
 
     //Encoder counts limit
-    this->pid->setInputLimits(-this->totalCounts, this->totalCounts); 
+    this->pid->setInputLimits(-55000, 55000); 
     //Pwm output from 0.0 to 1.0
     this->pid->setOutputLimits(-1.0, 1.0);
     //If there's a bias.
@@ -101,15 +102,12 @@
     //Set the new output.
     this->co = this->pid->compute();
 
-DigitalOut led3(P1_21);
     if(this->axisState){
         if(this->motInvert==0){
             this->motcon->mot_control(this->co); //send controller output to PWM motor control command    
-            led3=1;
         }
         else{
             this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command    
-            led3=0;
         }
     }
     else{
@@ -123,9 +121,7 @@
     //testOut = 0;
 }
 
-void Axis::center(void){
-    this->pid->setInputLimits(-this->totalCounts, this->totalCounts); 
-    
+void Axis::center(void){    
     while((*this->ptr_limit == 1) && (this->readCurrent() < mot_I_lim)){ //limit switch not pressed and mot current not exceeded
        this->set_point += 100;
        wait(.05);
@@ -141,9 +137,7 @@
     }
     this->zero();   //zero channel        
     
-    this->set_point = -(totalCounts/2.0);
-    wait(5.0);
-    this->zero();
+//    this->set_point = -(totalCounts/2.0);
         
     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());
@@ -262,7 +256,7 @@
 }
 
 float Axis::readCurrent(void){    
-    motCurrent = this->_analog.read() * 3.3;
+    motCurrent = (this->_analog.read() * 3.3)/ .525;  //525mV per amp
     return motCurrent;
 }
 
@@ -289,4 +283,11 @@
     
     this->set_point = 0.0;
     this->pid->setSetPoint(0);    
+}
+
+void Axis::writeEncoderValue(long value){
+    this->ls7366->LS7366_write_DTR(value);
+    
+    this->set_point = (float)value;
+    this->pid->setSetPoint(this->set_point);    
 }
\ No newline at end of file