Diff: Axis.cpp

Revision:

2:653433f4ee72

Parent:

1:cd249816dba8

Child:

3:71447d4fb4f0

--- a/Axis.cpp	Thu Sep 24 16:47:02 2015 +0000
+++ b/Axis.cpp	Wed Sep 30 16:20:45 2015 +0000
@@ -4,7 +4,7 @@
 #include "MotCon.h"
 #include "PID.h"
 
-Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) {
+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;
@@ -28,12 +28,13 @@
     p_lower = 0.0;
     vel_accum = 0.0;
     moveTime = 0.0;
-    countsPerDeg = 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);    
+    update.attach(this, &Axis::paramUpdate, Tdelay);
+    axisState = 1;    
     
     pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval
     ls7366 = new LS7366(spi, cs);  //LS7366 encoder interface IC
@@ -50,14 +51,13 @@
     this->enc = this->ls7366->LS7366_read_counter();  //update class variable    
 }
 
-void Axis::init(float cpd){
+void Axis::init(void){
     //_limit.mode(PullUp);
-    this->countsPerDeg = cpd;    //90.0/10680.0;
     //resets the controllers internals
     this->pid->reset();
 
     //Encoder counts limit
-    this->pid->setInputLimits(-20000.0, 20000.0); 
+    this->pid->setInputLimits(-this->totalCounts, this->totalCounts); 
     //Pwm output from 0.0 to 1.0
     this->pid->setOutputLimits(-1.0, 1.0);
     //If there's a bias.
@@ -99,7 +99,8 @@
     //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    
+    this->motcon->mot_control(this->co); //send controller output to PWM motor control command    
     
     this->pos_last = this->pos;
     this->vel_last = this->vel;
@@ -107,44 +108,47 @@
     //testOut = 0;
 }
 
-void Axis::home(long halfcounts){
-    this->pid->setInputLimits(-20000.0, 20000.0); 
-    while(*this->ptr_limit == 1){
+void Axis::center(void){
+    this->pid->setInputLimits(-this->totalCounts, this->totalCounts); 
+    
+    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 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);
+            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(.5);
+    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\r\n", t.read(), this->set_point, this->co, this->pos, this->vel, this->acc,*this->ptr_limit);
+        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(-17000.0, 0.0); //reset span limits
-    for(int positionCmd = 0;positionCmd > -halfcounts;positionCmd-=30){
+    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)
-            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);
+            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 = -halfcounts;
+    this->set_point = -(this->totalCounts)/2.0;
     this->pid->setSetPoint(this->set_point);
     
     //let PID settle to set point
     while((this->enc > (this->set_point + 100)) || (this->enc < (this->set_point - 100))){
-        //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);
+        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);
@@ -153,14 +157,23 @@
     this->set_point = 0.0;
     this->pid->setSetPoint(this->set_point);
 
-    this->pid->setInputLimits(-6500.0, 6500.0); //reset span limits
+    this->pid->setInputLimits(-(this->totalCounts)/2.0, (this->totalCounts)/2.0); //reset span limits
     this->pid->setSetPoint(this->set_point);
         
-//    pc.printf("Home: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);    
+    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());
 //    pc.printf("End Home\r\n\r\n");
 }
 
-void Axis::moveUpdate(void){                    
+void Axis::moveUpdate(void){               
+
+    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());
+
     switch(this->moveState){
         case 0:        
         break;
@@ -261,3 +274,18 @@
     this->t.start();
     this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay);
 }
+
+float Axis::readCurrent(void){    
+    motCurrent = _analog.read() * 3.3;
+    return motCurrent;
+}
+
+void Axis::axisOff(void){
+    update.detach();
+    this->axisState = 0;
+}
+
+void Axis::axisOn(void){
+    update.attach(this, &Axis::paramUpdate, Tdelay);
+    this->axisState = 1;
+}
\ No newline at end of file