Joseph Bradshaw / Axis_version2

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

Dependencies:   PID LS7366LIB MotCon2

Axis.cpp@12:7a7fe3baf733, 2019-01-09 (annotated)

Committer:
jebradshaw
Date:
Wed Jan 09 13:35:44 2019 +0000
Revision:
12:7a7fe3baf733
Parent:
11:93d924320ddc 
position, velocity, acceleration modes all in radians; Axis_Init function takes encoder counts/revolution

Who changed what in which revision?

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