Joseph Bradshaw / Axis_20181108

C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.

Dependencies:   LS7366LIB MotCon2 PID

Axis.cpp@0:d7f0715676f4, 2018-11-08 (annotated)

Committer:
jebradshaw
Date:
Thu Nov 08 17:54:30 2018 +0000
Revision:
0:d7f0715676f4
20181108 - Committed changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:d7f0715676f4 1
jebradshaw 0:d7f0715676f4 2 #include "Axis.h"
jebradshaw 0:d7f0715676f4 3 #include "LS7366.h"
jebradshaw 0:d7f0715676f4 4 #include "MotCon.h"
jebradshaw 0:d7f0715676f4 5 #include "PID.h"
jebradshaw 0:d7f0715676f4 6
jebradshaw 0:d7f0715676f4 7 Axis::Axis(SPI& spi, PinName cs, PinName pwm, PinName dir, PinName analog, int* limit): _spi(spi), _cs(cs), _pwm(pwm), _dir(dir) , _analog(analog){
jebradshaw 0:d7f0715676f4 8 this->_cs = 1; // Initialize chip select as off (high)
jebradshaw 0:d7f0715676f4 9 this->_pwm = 0.0;
jebradshaw 0:d7f0715676f4 10 this->_dir = 0;
jebradshaw 0:d7f0715676f4 11 this->co = 0.0;
jebradshaw 0:d7f0715676f4 12 this->Tdelay = .01;
jebradshaw 0:d7f0715676f4 13 this->Pk = 450.0; //120.0; //rough gains, seem to work well but could use tuning
jebradshaw 0:d7f0715676f4 14 this->Ik = 105.0; //75.0;
jebradshaw 0:d7f0715676f4 15 this->Dk = 0.0;
jebradshaw 0:d7f0715676f4 16 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 17 this->set_point_last = 0.0;
jebradshaw 0:d7f0715676f4 18 this->pos = 0.0;
jebradshaw 0:d7f0715676f4 19 this->vel = 0.0;
jebradshaw 0:d7f0715676f4 20 this->acc = 0.0;
jebradshaw 0:d7f0715676f4 21 this->stat = -1;
jebradshaw 0:d7f0715676f4 22 this->pos_cmd = 0.0;
jebradshaw 0:d7f0715676f4 23 this->vel_cmd = 0.0;
jebradshaw 0:d7f0715676f4 24 this->vel_avg_cmd = 0;
jebradshaw 0:d7f0715676f4 25 this->acc_cmd = 0.0;
jebradshaw 0:d7f0715676f4 26 this->vel_max = 2700.0 * Tdelay; //counts * Tdelay
jebradshaw 0:d7f0715676f4 27 this->acc_max = 1200.0 * Tdelay; //counts/sec/sec * Tdelay
jebradshaw 0:d7f0715676f4 28 this->p_higher = 0.0;
jebradshaw 0:d7f0715676f4 29 this->p_lower = 0.0;
jebradshaw 0:d7f0715676f4 30 this->vel_accum = 0.0;
jebradshaw 0:d7f0715676f4 31 this->moveTime = 0.0;
jebradshaw 0:d7f0715676f4 32 this->enc = 0;
jebradshaw 0:d7f0715676f4 33 this->moveStatus = 0; //status flag to indicate state of profile movement
jebradshaw 0:d7f0715676f4 34 this->moveState = 0; //used for state machine in movement profiles
jebradshaw 0:d7f0715676f4 35 this->debug = 0;
jebradshaw 0:d7f0715676f4 36 this->update.attach(this, &Axis::paramUpdate, Tdelay);
jebradshaw 0:d7f0715676f4 37 this->axisState = 0;
jebradshaw 0:d7f0715676f4 38 this->mot_I_lim = .35;
jebradshaw 0:d7f0715676f4 39 this->dIdT = 0.0;
jebradshaw 0:d7f0715676f4 40 this->motI = 0.0;
jebradshaw 0:d7f0715676f4 41 this->motI_last = 0.0;
jebradshaw 0:d7f0715676f4 42 this->mot_I_max = 0.0;
jebradshaw 0:d7f0715676f4 43 this->mot_I_max_last = 0.0;
jebradshaw 0:d7f0715676f4 44 this->motInvert = 0;
jebradshaw 0:d7f0715676f4 45 this->dataFormat = 'r'; //default is radians
jebradshaw 0:d7f0715676f4 46 // this->ctsPerDeg = cpd; //update counts per degree passed from constructor
jebradshaw 0:d7f0715676f4 47
jebradshaw 0:d7f0715676f4 48 this->pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval
jebradshaw 0:d7f0715676f4 49 this->ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC
jebradshaw 0:d7f0715676f4 50 this->motcon = new MotCon(pwm, dir);
jebradshaw 0:d7f0715676f4 51 this->ptr_limit = limit;
jebradshaw 0:d7f0715676f4 52
jebradshaw 0:d7f0715676f4 53 //start at 0
jebradshaw 0:d7f0715676f4 54 this->ls7366->LS7366_reset_counter();
jebradshaw 0:d7f0715676f4 55 this->ls7366->LS7366_quad_mode_x4();
jebradshaw 0:d7f0715676f4 56 this->ls7366->LS7366_write_DTR(0);
jebradshaw 0:d7f0715676f4 57
jebradshaw 0:d7f0715676f4 58 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 59 this->pid->setSetPoint(this->set_point);
jebradshaw 0:d7f0715676f4 60 this->enc = this->ls7366->LS7366_read_counter(); //update class variable
jebradshaw 0:d7f0715676f4 61 }
jebradshaw 0:d7f0715676f4 62
jebradshaw 0:d7f0715676f4 63 void Axis::init(void){
jebradshaw 0:d7f0715676f4 64 //resets the controllers internals
jebradshaw 0:d7f0715676f4 65 this->pid->reset();
jebradshaw 0:d7f0715676f4 66
jebradshaw 0:d7f0715676f4 67 //Encoder counts limit
jebradshaw 0:d7f0715676f4 68 this->pid->setInputLimits(-55000, 55000);
jebradshaw 0:d7f0715676f4 69 //Pwm output from 0.0 to 1.0
jebradshaw 0:d7f0715676f4 70 this->pid->setOutputLimits(-1.0, 1.0);
jebradshaw 0:d7f0715676f4 71 //If there's a bias.
jebradshaw 0:d7f0715676f4 72 this->pid->setBias(0.0);
jebradshaw 0:d7f0715676f4 73 this->pid->setMode(AUTO_MODE);
jebradshaw 0:d7f0715676f4 74
jebradshaw 0:d7f0715676f4 75 this->pid->setInterval(this->Tdelay);
jebradshaw 0:d7f0715676f4 76
jebradshaw 0:d7f0715676f4 77 //start at 0
jebradshaw 0:d7f0715676f4 78 this->ls7366->LS7366_reset_counter();
jebradshaw 0:d7f0715676f4 79 this->ls7366->LS7366_quad_mode_x4();
jebradshaw 0:d7f0715676f4 80 this->ls7366->LS7366_write_DTR(0);
jebradshaw 0:d7f0715676f4 81
jebradshaw 0:d7f0715676f4 82 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 83 this->pid->setSetPoint(this->set_point);
jebradshaw 0:d7f0715676f4 84 this->enc = this->ls7366->LS7366_read_counter(); //update class variable
jebradshaw 0:d7f0715676f4 85
jebradshaw 0:d7f0715676f4 86 //resets the controllers internals
jebradshaw 0:d7f0715676f4 87 this->pid->reset();
jebradshaw 0:d7f0715676f4 88 //start at 0
jebradshaw 0:d7f0715676f4 89 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 90 this->pid->setSetPoint(0);
jebradshaw 0:d7f0715676f4 91
jebradshaw 0:d7f0715676f4 92 this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller
jebradshaw 0:d7f0715676f4 93 }
jebradshaw 0:d7f0715676f4 94
jebradshaw 0:d7f0715676f4 95 void Axis::updatePIDgains(float P, float I, float D){
jebradshaw 0:d7f0715676f4 96 this->Pk = P; //120.0; //rough gains, seem to work well but could use tuning
jebradshaw 0:d7f0715676f4 97 this->Ik = I; //75.0;
jebradshaw 0:d7f0715676f4 98 this->Dk = D;
jebradshaw 0:d7f0715676f4 99 this->pid->setTunings(this->Pk, this->Ik, this->Dk);
jebradshaw 0:d7f0715676f4 100 }
jebradshaw 0:d7f0715676f4 101
jebradshaw 0:d7f0715676f4 102 void Axis::paramUpdate(void){
jebradshaw 0:d7f0715676f4 103 //testOut = 1;
jebradshaw 0:d7f0715676f4 104 this->enc = this->ls7366->LS7366_read_counter();
jebradshaw 0:d7f0715676f4 105 this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians
jebradshaw 0:d7f0715676f4 106
jebradshaw 0:d7f0715676f4 107 this->vel = (this->pos - this->pos_last) * this->Tdelay;
jebradshaw 0:d7f0715676f4 108 this->acc = (this->vel - this->vel_last);
jebradshaw 0:d7f0715676f4 109
jebradshaw 0:d7f0715676f4 110 this->pid->setSetPoint(this->set_point);
jebradshaw 0:d7f0715676f4 111
jebradshaw 0:d7f0715676f4 112 //Update the process variable.
jebradshaw 0:d7f0715676f4 113 this->pid->setProcessValue(this->pos);
jebradshaw 0:d7f0715676f4 114 //Set the new output.
jebradshaw 0:d7f0715676f4 115 this->co = this->pid->compute();
jebradshaw 0:d7f0715676f4 116
jebradshaw 0:d7f0715676f4 117 if(this->axisState){
jebradshaw 0:d7f0715676f4 118 if(this->motInvert==0){
jebradshaw 0:d7f0715676f4 119 this->motcon->mot_control(this->co); //send controller output to PWM motor control command
jebradshaw 0:d7f0715676f4 120 }
jebradshaw 0:d7f0715676f4 121 else{
jebradshaw 0:d7f0715676f4 122 this->motcon->mot_control(this->co, 1); //send controller output to PWM motor control command
jebradshaw 0:d7f0715676f4 123 }
jebradshaw 0:d7f0715676f4 124 }
jebradshaw 0:d7f0715676f4 125 else{
jebradshaw 0:d7f0715676f4 126 this->co = 0.0;
jebradshaw 0:d7f0715676f4 127 this->motcon->mot_control(0.0); //turn off motor command
jebradshaw 0:d7f0715676f4 128 }
jebradshaw 0:d7f0715676f4 129
jebradshaw 0:d7f0715676f4 130 this->pos_last = this->pos;
jebradshaw 0:d7f0715676f4 131 this->vel_last = this->vel;
jebradshaw 0:d7f0715676f4 132 this->set_point_last = this->set_point;
jebradshaw 0:d7f0715676f4 133 }
jebradshaw 0:d7f0715676f4 134
jebradshaw 0:d7f0715676f4 135 void Axis::center(void){
jebradshaw 0:d7f0715676f4 136 while((*this->ptr_limit == 1) && (this->readCurrent() < mot_I_lim)){ //limit switch not pressed and mot current not exceeded
jebradshaw 0:d7f0715676f4 137 this->set_point += 100;
jebradshaw 0:d7f0715676f4 138 wait(.05);
jebradshaw 0:d7f0715676f4 139 if(this->debug)
jebradshaw 0:d7f0715676f4 140 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());
jebradshaw 0:d7f0715676f4 141 }
jebradshaw 0:d7f0715676f4 142 wait(.5);
jebradshaw 0:d7f0715676f4 143 while((*this->ptr_limit == 0)){ //limit switch is pressed
jebradshaw 0:d7f0715676f4 144 this->set_point -= 10;
jebradshaw 0:d7f0715676f4 145 wait(.1);
jebradshaw 0:d7f0715676f4 146 if(this->debug)
jebradshaw 0:d7f0715676f4 147 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());
jebradshaw 0:d7f0715676f4 148 }
jebradshaw 0:d7f0715676f4 149 this->zero(); //zero channel
jebradshaw 0:d7f0715676f4 150
jebradshaw 0:d7f0715676f4 151 // this->set_point = -(totalCounts/2.0);
jebradshaw 0:d7f0715676f4 152
jebradshaw 0:d7f0715676f4 153 if(this->debug)
jebradshaw 0:d7f0715676f4 154 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());
jebradshaw 0:d7f0715676f4 155 // pc.printf("End Home\r\n\r\n");
jebradshaw 0:d7f0715676f4 156 }
jebradshaw 0:d7f0715676f4 157
jebradshaw 0:d7f0715676f4 158 void Axis::moveUpdate(void){
jebradshaw 0:d7f0715676f4 159
jebradshaw 0:d7f0715676f4 160 /* if(*this->ptr_limit == 0){
jebradshaw 0:d7f0715676f4 161 this->moveState = 4; //terminate the move
jebradshaw 0:d7f0715676f4 162 printf("\r\nLimit reached on axis!\r\n");
jebradshaw 0:d7f0715676f4 163 }
jebradshaw 0:d7f0715676f4 164 */
jebradshaw 0:d7f0715676f4 165 if(this->debug)
jebradshaw 0:d7f0715676f4 166 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());
jebradshaw 0:d7f0715676f4 167
jebradshaw 0:d7f0715676f4 168 switch(this->moveState){
jebradshaw 0:d7f0715676f4 169 case 0:
jebradshaw 0:d7f0715676f4 170 break;
jebradshaw 0:d7f0715676f4 171
jebradshaw 0:d7f0715676f4 172 //accelerate
jebradshaw 0:d7f0715676f4 173 case 1:
jebradshaw 0:d7f0715676f4 174 //testOut = 1;
jebradshaw 0:d7f0715676f4 175 this->vel_accum += this->acc_cmd * this->Tdelay; //add acceleration to the velocity accumulator
jebradshaw 0:d7f0715676f4 176 if(this->vel_avg_cmd > 0.0){ //check the sign of the movement
jebradshaw 0:d7f0715676f4 177 if(this->vel_accum >= this->vel_cmd) //if the accumulator reaches or exceeds the velocity command
jebradshaw 0:d7f0715676f4 178 this->vel_accum = this->vel_cmd; // only add the velocity command to the accumulator
jebradshaw 0:d7f0715676f4 179 }
jebradshaw 0:d7f0715676f4 180 else{ //if the sign was negative
jebradshaw 0:d7f0715676f4 181 if(this->vel_accum <= this->vel_cmd)
jebradshaw 0:d7f0715676f4 182 this->vel_accum = this->vel_cmd;
jebradshaw 0:d7f0715676f4 183 }
jebradshaw 0:d7f0715676f4 184 //testOut = 0;
jebradshaw 0:d7f0715676f4 185
jebradshaw 0:d7f0715676f4 186 this->set_point += this->vel_accum;
jebradshaw 0:d7f0715676f4 187 //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt);
jebradshaw 0:d7f0715676f4 188 //pc.printf("acc_up,%.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);
jebradshaw 0:d7f0715676f4 189
jebradshaw 0:d7f0715676f4 190 if(this->t.read()>=(this->moveTime/3.0) || (abs(this->vel_accum) > abs(this->vel_cmd)))
jebradshaw 0:d7f0715676f4 191 this->moveState = 2;
jebradshaw 0:d7f0715676f4 192 break;
jebradshaw 0:d7f0715676f4 193
jebradshaw 0:d7f0715676f4 194 //constant velocity
jebradshaw 0:d7f0715676f4 195 case 2:
jebradshaw 0:d7f0715676f4 196 //testOut = 1;
jebradshaw 0:d7f0715676f4 197 //this->vel_accum += this->vel_cmd * this->Tdelay;
jebradshaw 0:d7f0715676f4 198 this->set_point += this->vel_cmd;
jebradshaw 0:d7f0715676f4 199 //testOut = 0;
jebradshaw 0:d7f0715676f4 200 //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt);
jebradshaw 0:d7f0715676f4 201 //pc.printf("vel_cn,%.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);
jebradshaw 0:d7f0715676f4 202
jebradshaw 0:d7f0715676f4 203 if(this->t.read()>=(2.0/3.0 * this->moveTime))
jebradshaw 0:d7f0715676f4 204 this->moveState = 3;
jebradshaw 0:d7f0715676f4 205 break;
jebradshaw 0:d7f0715676f4 206
jebradshaw 0:d7f0715676f4 207 //decelerate
jebradshaw 0:d7f0715676f4 208 case 3:
jebradshaw 0:d7f0715676f4 209 this->vel_accum -= this->acc_cmd * this->Tdelay;
jebradshaw 0:d7f0715676f4 210
jebradshaw 0:d7f0715676f4 211 this->set_point += this->vel_accum; //ramp down velocity by acceleration
jebradshaw 0:d7f0715676f4 212 //pc.printf("T=%.2f SP=%.3f co=%.3f enc=%d pos=%.3f vel=%.3f acc=%.3f vel_accum=%.2f accelCnt=%.2f \r\n", t.read(), con0.set_point, con0.co, con0.enc, con0.pos, con0.vel, con0.acc, vel_accum, accelCnt);
jebradshaw 0:d7f0715676f4 213 //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);
jebradshaw 0:d7f0715676f4 214
jebradshaw 0:d7f0715676f4 215 if(this->vel_avg_cmd > 0.0){
jebradshaw 0:d7f0715676f4 216 if(this->pos_cmd <= this->pos){
jebradshaw 0:d7f0715676f4 217 //finish with position command
jebradshaw 0:d7f0715676f4 218 this->set_point = this->pos_cmd;
jebradshaw 0:d7f0715676f4 219 this->moveState = 4;
jebradshaw 0:d7f0715676f4 220 }
jebradshaw 0:d7f0715676f4 221 }
jebradshaw 0:d7f0715676f4 222 else{
jebradshaw 0:d7f0715676f4 223 if(this->pos_cmd >= this->pos){
jebradshaw 0:d7f0715676f4 224 //finish with position command
jebradshaw 0:d7f0715676f4 225 //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124
jebradshaw 0:d7f0715676f4 226 this->moveState = 4;
jebradshaw 0:d7f0715676f4 227 }
jebradshaw 0:d7f0715676f4 228 }
jebradshaw 0:d7f0715676f4 229
jebradshaw 0:d7f0715676f4 230 if(this->t.read()>=this->moveTime){
jebradshaw 0:d7f0715676f4 231 //finish with position command
jebradshaw 0:d7f0715676f4 232 //this->set_point = this->pos_cmd; //May be causing jerk after multi-axis movements J Bradshaw 20151124
jebradshaw 0:d7f0715676f4 233 this->moveState = 4;
jebradshaw 0:d7f0715676f4 234 }
jebradshaw 0:d7f0715676f4 235 break;
jebradshaw 0:d7f0715676f4 236
jebradshaw 0:d7f0715676f4 237 case 4:
jebradshaw 0:d7f0715676f4 238 this->moveProfile.detach(); //turn off the trapazoidal update ticker
jebradshaw 0:d7f0715676f4 239 this->t.stop();
jebradshaw 0:d7f0715676f4 240 this->moveState = 0;
jebradshaw 0:d7f0715676f4 241 break;
jebradshaw 0:d7f0715676f4 242 }//switch moveStatus
jebradshaw 0:d7f0715676f4 243 return;
jebradshaw 0:d7f0715676f4 244 }
jebradshaw 0:d7f0715676f4 245
jebradshaw 0:d7f0715676f4 246 // position - encoder position to move to
jebradshaw 0:d7f0715676f4 247 // time - duration of the movement
jebradshaw 0:d7f0715676f4 248 void Axis::moveTrapezoid(float positionCmd, float time){
jebradshaw 0:d7f0715676f4 249 this->pos_cmd = positionCmd;
jebradshaw 0:d7f0715676f4 250 this->moveTime = time;
jebradshaw 0:d7f0715676f4 251 float enc_distance = this->pos_cmd - (float)this->enc;// * 1.0/con0.countsPerDeg * 180.0/PI;
jebradshaw 0:d7f0715676f4 252
jebradshaw 0:d7f0715676f4 253 this->vel_avg_cmd = enc_distance / time;
jebradshaw 0:d7f0715676f4 254 this->vel_cmd = 1.5 * this->vel_avg_cmd * this->Tdelay;
jebradshaw 0:d7f0715676f4 255 this->acc_cmd = 4.5 * (enc_distance / (this->moveTime * this->moveTime)) * this->Tdelay;
jebradshaw 0:d7f0715676f4 256
jebradshaw 0:d7f0715676f4 257 //pc.printf("tx=%f encdist=%.3f vAvg=%.3f vMax=%.3f Acc=%.3f \r\n", this->moveTime, enc_distance,this->vel_avg_cmd,this->vel_cmd,this->acc_cmd);
jebradshaw 0:d7f0715676f4 258
jebradshaw 0:d7f0715676f4 259 //establish encoder velocities and accelerations for position control per Tdelay
jebradshaw 0:d7f0715676f4 260 this->vel_accum = 0.0;
jebradshaw 0:d7f0715676f4 261
jebradshaw 0:d7f0715676f4 262 // this->set_point = this->pos;
jebradshaw 0:d7f0715676f4 263 this->moveState = 1;
jebradshaw 0:d7f0715676f4 264 this->t.reset();
jebradshaw 0:d7f0715676f4 265 this->t.start();
jebradshaw 0:d7f0715676f4 266 this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay);
jebradshaw 0:d7f0715676f4 267 }
jebradshaw 0:d7f0715676f4 268
jebradshaw 0:d7f0715676f4 269 float Axis::readCurrent(void){
jebradshaw 0:d7f0715676f4 270 this->motI = (this->_analog.read() * 3.3) / .525; //525mV per amp
jebradshaw 0:d7f0715676f4 271 if(abs(this->motI) > this->mot_I_max){ //update the max motor current measured
jebradshaw 0:d7f0715676f4 272 this->mot_I_max = this->motI;
jebradshaw 0:d7f0715676f4 273 }
jebradshaw 0:d7f0715676f4 274 this->dIdT = motI - motI_last;
jebradshaw 0:d7f0715676f4 275 this->motI_last = motI;
jebradshaw 0:d7f0715676f4 276 return this->motI;
jebradshaw 0:d7f0715676f4 277 }
jebradshaw 0:d7f0715676f4 278
jebradshaw 0:d7f0715676f4 279 void Axis::axisOff(void){
jebradshaw 0:d7f0715676f4 280 this->co = 0;
jebradshaw 0:d7f0715676f4 281 this->axisState = 0;
jebradshaw 0:d7f0715676f4 282 this->update.detach();
jebradshaw 0:d7f0715676f4 283 }
jebradshaw 0:d7f0715676f4 284
jebradshaw 0:d7f0715676f4 285 void Axis::axisOn(void){
jebradshaw 0:d7f0715676f4 286 this->co = 0.0;
jebradshaw 0:d7f0715676f4 287 this->pid->reset();
jebradshaw 0:d7f0715676f4 288 //start at 0 if not already homed
jebradshaw 0:d7f0715676f4 289 if(this->stat != 0){
jebradshaw 0:d7f0715676f4 290 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 291 this->pid->setSetPoint(0);
jebradshaw 0:d7f0715676f4 292 }
jebradshaw 0:d7f0715676f4 293
jebradshaw 0:d7f0715676f4 294 this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller
jebradshaw 0:d7f0715676f4 295 this->axisState = 1;
jebradshaw 0:d7f0715676f4 296 this->update.attach(this, &Axis::paramUpdate, this->Tdelay);
jebradshaw 0:d7f0715676f4 297 }
jebradshaw 0:d7f0715676f4 298
jebradshaw 0:d7f0715676f4 299 void Axis::zero(void){
jebradshaw 0:d7f0715676f4 300
jebradshaw 0:d7f0715676f4 301 this->ls7366->LS7366_reset_counter();
jebradshaw 0:d7f0715676f4 302 this->ls7366->LS7366_quad_mode_x4();
jebradshaw 0:d7f0715676f4 303 this->ls7366->LS7366_write_DTR(0);
jebradshaw 0:d7f0715676f4 304
jebradshaw 0:d7f0715676f4 305 this->enc = this->ls7366->LS7366_read_counter();
jebradshaw 0:d7f0715676f4 306
jebradshaw 0:d7f0715676f4 307 this->pos = 0.0;
jebradshaw 0:d7f0715676f4 308 this->pid->reset(); //added to avoid possible itegral windup effects on instant position change 20170616
jebradshaw 0:d7f0715676f4 309 this->set_point = 0.0;
jebradshaw 0:d7f0715676f4 310 this->pid->setSetPoint(0);
jebradshaw 0:d7f0715676f4 311
jebradshaw 0:d7f0715676f4 312 }
jebradshaw 0:d7f0715676f4 313
jebradshaw 0:d7f0715676f4 314 void Axis::writeEncoderValue(long value){
jebradshaw 0:d7f0715676f4 315
jebradshaw 0:d7f0715676f4 316 this->ls7366->LS7366_write_DTR(value);
jebradshaw 0:d7f0715676f4 317
jebradshaw 0:d7f0715676f4 318 this->set_point = (float)value;
jebradshaw 0:d7f0715676f4 319 this->pid->setSetPoint(this->set_point);
jebradshaw 0:d7f0715676f4 320 }