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?
User | Revision | Line number | New 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 |