C++ class for controlling DC motor with encoder feedback. Dependencies include LS7366LIB, MotCon, and PID.
Dependencies: LS7366LIB MotCon2 PID
Axis.cpp@3:71447d4fb4f0, 2015-10-01 (annotated)
- Committer:
- jebradshaw
- Date:
- Thu Oct 01 14:55:54 2015 +0000
- Revision:
- 3:71447d4fb4f0
- Parent:
- 2:653433f4ee72
- Child:
- 4:4079f92f9c26
Axis 2 problems with moving uncommanded.; ; PID? polarity? What did I change?
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 | 3:71447d4fb4f0 | 6 | #define MIN_MOT_COL .06 |
jebradshaw | 0:cf7192f9f99a | 7 | |
jebradshaw | 2:653433f4ee72 | 8 | 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){ |
jebradshaw | 0:cf7192f9f99a | 9 | _cs = 1; // Initialize chip select as off (high) |
jebradshaw | 0:cf7192f9f99a | 10 | _pwm = 0.0; |
jebradshaw | 0:cf7192f9f99a | 11 | _dir = 0; |
jebradshaw | 0:cf7192f9f99a | 12 | co = 0.0; |
jebradshaw | 0:cf7192f9f99a | 13 | Tdelay = .01; |
jebradshaw | 0:cf7192f9f99a | 14 | Pk = 120.0; //80.0; //rough gains, seem to work well but could use tuning |
jebradshaw | 0:cf7192f9f99a | 15 | Ik = 55.0; //35.0; |
jebradshaw | 0:cf7192f9f99a | 16 | Dk = 0.0; |
jebradshaw | 0:cf7192f9f99a | 17 | set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 18 | set_point_last = 0.0; |
jebradshaw | 0:cf7192f9f99a | 19 | pos = 0.0; |
jebradshaw | 0:cf7192f9f99a | 20 | vel = 0.0; |
jebradshaw | 0:cf7192f9f99a | 21 | acc = 0.0; |
jebradshaw | 0:cf7192f9f99a | 22 | pos_cmd = 0.0; |
jebradshaw | 0:cf7192f9f99a | 23 | vel_cmd = 0.0; |
jebradshaw | 0:cf7192f9f99a | 24 | vel_avg_cmd = 0; |
jebradshaw | 0:cf7192f9f99a | 25 | acc_cmd = 0.0; |
jebradshaw | 0:cf7192f9f99a | 26 | vel_max = 2700.0 * Tdelay; //counts * Tdelay |
jebradshaw | 0:cf7192f9f99a | 27 | acc_max = 1200.0 * Tdelay; //counts/sec/sec * Tdelay |
jebradshaw | 0:cf7192f9f99a | 28 | p_higher = 0.0; |
jebradshaw | 0:cf7192f9f99a | 29 | p_lower = 0.0; |
jebradshaw | 0:cf7192f9f99a | 30 | vel_accum = 0.0; |
jebradshaw | 0:cf7192f9f99a | 31 | moveTime = 0.0; |
jebradshaw | 2:653433f4ee72 | 32 | totalCounts = totalCnts; |
jebradshaw | 0:cf7192f9f99a | 33 | enc = 0; |
jebradshaw | 0:cf7192f9f99a | 34 | moveStatus = 0; //status flag to indicate state of profile movement |
jebradshaw | 0:cf7192f9f99a | 35 | moveState = 0; //used for state machine in movement profiles |
jebradshaw | 0:cf7192f9f99a | 36 | debug = 0; |
jebradshaw | 3:71447d4fb4f0 | 37 | // update.attach(this, &Axis::paramUpdate, Tdelay); |
jebradshaw | 3:71447d4fb4f0 | 38 | axisState = 0; |
jebradshaw | 0:cf7192f9f99a | 39 | |
jebradshaw | 0:cf7192f9f99a | 40 | pid = new PID(0.0,0.0,0.0,Tdelay); //Kc, Ti, Td, interval |
jebradshaw | 0:cf7192f9f99a | 41 | ls7366 = new LS7366(spi, cs); //LS7366 encoder interface IC |
jebradshaw | 0:cf7192f9f99a | 42 | motcon = new MotCon(pwm, dir); |
jebradshaw | 1:cd249816dba8 | 43 | ptr_limit = limit; |
jebradshaw | 0:cf7192f9f99a | 44 | |
jebradshaw | 0:cf7192f9f99a | 45 | //start at 0 |
jebradshaw | 0:cf7192f9f99a | 46 | this->ls7366->LS7366_reset_counter(); |
jebradshaw | 0:cf7192f9f99a | 47 | this->ls7366->LS7366_quad_mode_x4(); |
jebradshaw | 0:cf7192f9f99a | 48 | this->ls7366->LS7366_write_DTR(0); |
jebradshaw | 0:cf7192f9f99a | 49 | |
jebradshaw | 0:cf7192f9f99a | 50 | this->set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 51 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 52 | this->enc = this->ls7366->LS7366_read_counter(); //update class variable |
jebradshaw | 0:cf7192f9f99a | 53 | } |
jebradshaw | 0:cf7192f9f99a | 54 | |
jebradshaw | 2:653433f4ee72 | 55 | void Axis::init(void){ |
jebradshaw | 1:cd249816dba8 | 56 | //_limit.mode(PullUp); |
jebradshaw | 0:cf7192f9f99a | 57 | //resets the controllers internals |
jebradshaw | 0:cf7192f9f99a | 58 | this->pid->reset(); |
jebradshaw | 0:cf7192f9f99a | 59 | |
jebradshaw | 0:cf7192f9f99a | 60 | //Encoder counts limit |
jebradshaw | 2:653433f4ee72 | 61 | this->pid->setInputLimits(-this->totalCounts, this->totalCounts); |
jebradshaw | 0:cf7192f9f99a | 62 | //Pwm output from 0.0 to 1.0 |
jebradshaw | 0:cf7192f9f99a | 63 | this->pid->setOutputLimits(-1.0, 1.0); |
jebradshaw | 0:cf7192f9f99a | 64 | //If there's a bias. |
jebradshaw | 0:cf7192f9f99a | 65 | this->pid->setBias(0.0); |
jebradshaw | 0:cf7192f9f99a | 66 | this->pid->setMode(AUTO_MODE); |
jebradshaw | 0:cf7192f9f99a | 67 | |
jebradshaw | 0:cf7192f9f99a | 68 | this->pid->setInterval(this->Tdelay); |
jebradshaw | 0:cf7192f9f99a | 69 | |
jebradshaw | 0:cf7192f9f99a | 70 | //start at 0 |
jebradshaw | 0:cf7192f9f99a | 71 | this->ls7366->LS7366_reset_counter(); |
jebradshaw | 0:cf7192f9f99a | 72 | this->ls7366->LS7366_quad_mode_x4(); |
jebradshaw | 0:cf7192f9f99a | 73 | this->ls7366->LS7366_write_DTR(0); |
jebradshaw | 0:cf7192f9f99a | 74 | |
jebradshaw | 0:cf7192f9f99a | 75 | this->set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 76 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 77 | this->enc = this->ls7366->LS7366_read_counter(); //update class variable |
jebradshaw | 0:cf7192f9f99a | 78 | |
jebradshaw | 0:cf7192f9f99a | 79 | //resets the controllers internals |
jebradshaw | 0:cf7192f9f99a | 80 | this->pid->reset(); |
jebradshaw | 0:cf7192f9f99a | 81 | //start at 0 |
jebradshaw | 0:cf7192f9f99a | 82 | this->set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 83 | this->pid->setSetPoint(0); |
jebradshaw | 0:cf7192f9f99a | 84 | |
jebradshaw | 0:cf7192f9f99a | 85 | this->pid->setTunings(this->Pk, this->Ik, this->Dk); //turns on controller |
jebradshaw | 0:cf7192f9f99a | 86 | } |
jebradshaw | 0:cf7192f9f99a | 87 | |
jebradshaw | 0:cf7192f9f99a | 88 | void Axis::paramUpdate(void){ |
jebradshaw | 0:cf7192f9f99a | 89 | //testOut = 1; |
jebradshaw | 0:cf7192f9f99a | 90 | this->enc = this->ls7366->LS7366_read_counter(); |
jebradshaw | 0:cf7192f9f99a | 91 | this->pos = (float)this->enc; // * this->countsPerDeg * PI/180.0; //times counts/degree and convert to radians |
jebradshaw | 0:cf7192f9f99a | 92 | |
jebradshaw | 0:cf7192f9f99a | 93 | this->vel = (this->pos - this->pos_last) * this->Tdelay; |
jebradshaw | 0:cf7192f9f99a | 94 | this->acc = (this->vel - this->vel_last); |
jebradshaw | 0:cf7192f9f99a | 95 | |
jebradshaw | 0:cf7192f9f99a | 96 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 97 | |
jebradshaw | 0:cf7192f9f99a | 98 | //Update the process variable. |
jebradshaw | 0:cf7192f9f99a | 99 | this->pid->setProcessValue(this->pos); |
jebradshaw | 0:cf7192f9f99a | 100 | //Set the new output. |
jebradshaw | 0:cf7192f9f99a | 101 | this->co = this->pid->compute(); |
jebradshaw | 0:cf7192f9f99a | 102 | |
jebradshaw | 2:653433f4ee72 | 103 | //this->motcon->mot_control(-(this->co)); //send controller output to PWM motor control command |
jebradshaw | 2:653433f4ee72 | 104 | this->motcon->mot_control(this->co); //send controller output to PWM motor control command |
jebradshaw | 0:cf7192f9f99a | 105 | |
jebradshaw | 0:cf7192f9f99a | 106 | this->pos_last = this->pos; |
jebradshaw | 0:cf7192f9f99a | 107 | this->vel_last = this->vel; |
jebradshaw | 0:cf7192f9f99a | 108 | this->set_point_last = this->set_point; |
jebradshaw | 0:cf7192f9f99a | 109 | //testOut = 0; |
jebradshaw | 0:cf7192f9f99a | 110 | } |
jebradshaw | 0:cf7192f9f99a | 111 | |
jebradshaw | 2:653433f4ee72 | 112 | void Axis::center(void){ |
jebradshaw | 2:653433f4ee72 | 113 | this->pid->setInputLimits(-this->totalCounts, this->totalCounts); |
jebradshaw | 2:653433f4ee72 | 114 | |
jebradshaw | 2:653433f4ee72 | 115 | while(*this->ptr_limit == 1){ // && ({ |
jebradshaw | 0:cf7192f9f99a | 116 | this->set_point += 100; |
jebradshaw | 0:cf7192f9f99a | 117 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 118 | wait(.05); |
jebradshaw | 0:cf7192f9f99a | 119 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 120 | 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:cf7192f9f99a | 121 | } |
jebradshaw | 2:653433f4ee72 | 122 | wait(.2); |
jebradshaw | 0:cf7192f9f99a | 123 | this->set_point -= 1000; |
jebradshaw | 0:cf7192f9f99a | 124 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 2:653433f4ee72 | 125 | wait(.6); |
jebradshaw | 1:cd249816dba8 | 126 | |
jebradshaw | 1:cd249816dba8 | 127 | while(*this->ptr_limit == 1){ |
jebradshaw | 0:cf7192f9f99a | 128 | this->set_point += 10; |
jebradshaw | 0:cf7192f9f99a | 129 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 130 | wait(.02); |
jebradshaw | 0:cf7192f9f99a | 131 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 132 | 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:cf7192f9f99a | 133 | } |
jebradshaw | 0:cf7192f9f99a | 134 | |
jebradshaw | 0:cf7192f9f99a | 135 | this->ls7366->LS7366_write_DTR(0); //zero encoder channel |
jebradshaw | 0:cf7192f9f99a | 136 | this->set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 137 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 138 | |
jebradshaw | 2:653433f4ee72 | 139 | this->pid->setInputLimits(-(this->totalCounts)/2.0, 0.0); //reset span limits |
jebradshaw | 2:653433f4ee72 | 140 | for(int positionCmd = 0;positionCmd > -this->totalCounts/2.0;positionCmd-=30){ |
jebradshaw | 0:cf7192f9f99a | 141 | this->set_point = positionCmd; //move arm to center |
jebradshaw | 0:cf7192f9f99a | 142 | wait(.01); |
jebradshaw | 0:cf7192f9f99a | 143 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 144 | 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:cf7192f9f99a | 145 | } |
jebradshaw | 2:653433f4ee72 | 146 | this->set_point = -(this->totalCounts)/2.0; |
jebradshaw | 0:cf7192f9f99a | 147 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 148 | |
jebradshaw | 0:cf7192f9f99a | 149 | //let PID settle to set point |
jebradshaw | 0:cf7192f9f99a | 150 | while((this->enc > (this->set_point + 100)) || (this->enc < (this->set_point - 100))){ |
jebradshaw | 2:653433f4ee72 | 151 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 152 | 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:cf7192f9f99a | 153 | wait(.01); |
jebradshaw | 0:cf7192f9f99a | 154 | } |
jebradshaw | 0:cf7192f9f99a | 155 | //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); |
jebradshaw | 0:cf7192f9f99a | 156 | |
jebradshaw | 0:cf7192f9f99a | 157 | this->ls7366->LS7366_write_DTR(0); //zero encoder channel |
jebradshaw | 0:cf7192f9f99a | 158 | this->set_point = 0.0; |
jebradshaw | 0:cf7192f9f99a | 159 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 160 | |
jebradshaw | 2:653433f4ee72 | 161 | this->pid->setInputLimits(-(this->totalCounts)/2.0, (this->totalCounts)/2.0); //reset span limits |
jebradshaw | 0:cf7192f9f99a | 162 | this->pid->setSetPoint(this->set_point); |
jebradshaw | 0:cf7192f9f99a | 163 | |
jebradshaw | 2:653433f4ee72 | 164 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 165 | 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:cf7192f9f99a | 166 | // pc.printf("End Home\r\n\r\n"); |
jebradshaw | 0:cf7192f9f99a | 167 | } |
jebradshaw | 0:cf7192f9f99a | 168 | |
jebradshaw | 2:653433f4ee72 | 169 | void Axis::moveUpdate(void){ |
jebradshaw | 2:653433f4ee72 | 170 | |
jebradshaw | 2:653433f4ee72 | 171 | if(*this->ptr_limit == 0){ |
jebradshaw | 2:653433f4ee72 | 172 | this->moveState = 4; //terminate the move |
jebradshaw | 2:653433f4ee72 | 173 | printf("\r\nLimit reached on axis!\r\n"); |
jebradshaw | 2:653433f4ee72 | 174 | } |
jebradshaw | 2:653433f4ee72 | 175 | if(this->debug) |
jebradshaw | 2:653433f4ee72 | 176 | 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 | 177 | |
jebradshaw | 0:cf7192f9f99a | 178 | switch(this->moveState){ |
jebradshaw | 0:cf7192f9f99a | 179 | case 0: |
jebradshaw | 0:cf7192f9f99a | 180 | break; |
jebradshaw | 0:cf7192f9f99a | 181 | |
jebradshaw | 0:cf7192f9f99a | 182 | //accelerate |
jebradshaw | 0:cf7192f9f99a | 183 | case 1: |
jebradshaw | 0:cf7192f9f99a | 184 | //testOut = 1; |
jebradshaw | 0:cf7192f9f99a | 185 | this->vel_accum += this->acc_cmd * this->Tdelay; //add acceleration to the velocity accumulator |
jebradshaw | 0:cf7192f9f99a | 186 | if(this->vel_avg_cmd > 0.0){ //check the sign of the movement |
jebradshaw | 0:cf7192f9f99a | 187 | if(this->vel_accum >= this->vel_cmd) //if the accumulator reaches or exceeds the velocity command |
jebradshaw | 0:cf7192f9f99a | 188 | this->vel_accum = this->vel_cmd; // only add the velocity command to the accumulator |
jebradshaw | 0:cf7192f9f99a | 189 | } |
jebradshaw | 0:cf7192f9f99a | 190 | else{ //if the sign was negative |
jebradshaw | 0:cf7192f9f99a | 191 | if(this->vel_accum <= this->vel_cmd) |
jebradshaw | 0:cf7192f9f99a | 192 | this->vel_accum = this->vel_cmd; |
jebradshaw | 0:cf7192f9f99a | 193 | } |
jebradshaw | 0:cf7192f9f99a | 194 | //testOut = 0; |
jebradshaw | 0:cf7192f9f99a | 195 | |
jebradshaw | 0:cf7192f9f99a | 196 | this->set_point += this->vel_accum; |
jebradshaw | 0:cf7192f9f99a | 197 | //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 | 198 | //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 | 199 | |
jebradshaw | 0:cf7192f9f99a | 200 | if(this->t.read()>=(this->moveTime/3.0) || (abs(this->vel_accum) > abs(this->vel_cmd))) |
jebradshaw | 0:cf7192f9f99a | 201 | this->moveState = 2; |
jebradshaw | 0:cf7192f9f99a | 202 | break; |
jebradshaw | 0:cf7192f9f99a | 203 | |
jebradshaw | 0:cf7192f9f99a | 204 | //constant velocity |
jebradshaw | 0:cf7192f9f99a | 205 | case 2: |
jebradshaw | 0:cf7192f9f99a | 206 | //testOut = 1; |
jebradshaw | 0:cf7192f9f99a | 207 | //this->vel_accum += this->vel_cmd * this->Tdelay; |
jebradshaw | 0:cf7192f9f99a | 208 | this->set_point += this->vel_cmd; |
jebradshaw | 0:cf7192f9f99a | 209 | //testOut = 0; |
jebradshaw | 0:cf7192f9f99a | 210 | //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 | 211 | //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 | 212 | |
jebradshaw | 0:cf7192f9f99a | 213 | if(this->t.read()>=(2.0/3.0 * this->moveTime)) |
jebradshaw | 0:cf7192f9f99a | 214 | this->moveState = 3; |
jebradshaw | 0:cf7192f9f99a | 215 | break; |
jebradshaw | 0:cf7192f9f99a | 216 | |
jebradshaw | 0:cf7192f9f99a | 217 | //decelerate |
jebradshaw | 0:cf7192f9f99a | 218 | case 3: |
jebradshaw | 0:cf7192f9f99a | 219 | this->vel_accum -= this->acc_cmd * this->Tdelay; |
jebradshaw | 0:cf7192f9f99a | 220 | |
jebradshaw | 0:cf7192f9f99a | 221 | this->set_point += this->vel_accum; //ramp down velocity by acceleration |
jebradshaw | 0:cf7192f9f99a | 222 | //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 | 223 | //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 | 224 | |
jebradshaw | 0:cf7192f9f99a | 225 | if(this->vel_avg_cmd > 0.0){ |
jebradshaw | 1:cd249816dba8 | 226 | if(this->pos_cmd <= this->pos){ |
jebradshaw | 1:cd249816dba8 | 227 | //finish with position command |
jebradshaw | 1:cd249816dba8 | 228 | this->set_point = this->pos_cmd; |
jebradshaw | 0:cf7192f9f99a | 229 | this->moveState = 4; |
jebradshaw | 1:cd249816dba8 | 230 | } |
jebradshaw | 0:cf7192f9f99a | 231 | } |
jebradshaw | 0:cf7192f9f99a | 232 | else{ |
jebradshaw | 1:cd249816dba8 | 233 | if(this->pos_cmd >= this->pos){ |
jebradshaw | 1:cd249816dba8 | 234 | //finish with position command |
jebradshaw | 1:cd249816dba8 | 235 | this->set_point = this->pos_cmd; |
jebradshaw | 1:cd249816dba8 | 236 | this->moveState = 4; |
jebradshaw | 1:cd249816dba8 | 237 | } |
jebradshaw | 0:cf7192f9f99a | 238 | } |
jebradshaw | 0:cf7192f9f99a | 239 | |
jebradshaw | 0:cf7192f9f99a | 240 | if(this->t.read()>=this->moveTime){ |
jebradshaw | 1:cd249816dba8 | 241 | //finish with position command |
jebradshaw | 1:cd249816dba8 | 242 | this->set_point = this->pos_cmd; |
jebradshaw | 0:cf7192f9f99a | 243 | this->moveState = 4; |
jebradshaw | 0:cf7192f9f99a | 244 | } |
jebradshaw | 0:cf7192f9f99a | 245 | break; |
jebradshaw | 0:cf7192f9f99a | 246 | |
jebradshaw | 0:cf7192f9f99a | 247 | case 4: |
jebradshaw | 0:cf7192f9f99a | 248 | this->moveProfile.detach(); //turn off the trapazoidal update ticker |
jebradshaw | 0:cf7192f9f99a | 249 | this->t.stop(); |
jebradshaw | 0:cf7192f9f99a | 250 | this->moveState = 0; |
jebradshaw | 0:cf7192f9f99a | 251 | break; |
jebradshaw | 0:cf7192f9f99a | 252 | }//switch moveStatus |
jebradshaw | 0:cf7192f9f99a | 253 | return; |
jebradshaw | 0:cf7192f9f99a | 254 | } |
jebradshaw | 0:cf7192f9f99a | 255 | |
jebradshaw | 0:cf7192f9f99a | 256 | // position - encoder position to move to |
jebradshaw | 0:cf7192f9f99a | 257 | // time - duration of the movement |
jebradshaw | 0:cf7192f9f99a | 258 | void Axis::moveTrapezoid(float positionCmd, float time){ |
jebradshaw | 0:cf7192f9f99a | 259 | this->pos_cmd = positionCmd; |
jebradshaw | 0:cf7192f9f99a | 260 | this->moveTime = time; |
jebradshaw | 0:cf7192f9f99a | 261 | float enc_distance = pos_cmd - (float)this->enc;// * 1.0/con0.countsPerDeg * 180.0/PI; |
jebradshaw | 0:cf7192f9f99a | 262 | |
jebradshaw | 0:cf7192f9f99a | 263 | this->vel_avg_cmd = enc_distance / time; |
jebradshaw | 0:cf7192f9f99a | 264 | this->vel_cmd = 1.5 * this->vel_avg_cmd * this->Tdelay; |
jebradshaw | 0:cf7192f9f99a | 265 | this->acc_cmd = 4.5 * (enc_distance / (this->moveTime * this->moveTime)) * this->Tdelay; |
jebradshaw | 0:cf7192f9f99a | 266 | |
jebradshaw | 0:cf7192f9f99a | 267 | //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 | 268 | |
jebradshaw | 0:cf7192f9f99a | 269 | //establish encoder velocities and accelerations for position control per Tdelay |
jebradshaw | 0:cf7192f9f99a | 270 | this->vel_accum = 0.0; |
jebradshaw | 0:cf7192f9f99a | 271 | |
jebradshaw | 0:cf7192f9f99a | 272 | // this->set_point = this->pos; |
jebradshaw | 0:cf7192f9f99a | 273 | this->moveState = 1; |
jebradshaw | 0:cf7192f9f99a | 274 | this->t.reset(); |
jebradshaw | 0:cf7192f9f99a | 275 | this->t.start(); |
jebradshaw | 0:cf7192f9f99a | 276 | this->moveProfile.attach(this, &Axis::moveUpdate, this->Tdelay); |
jebradshaw | 0:cf7192f9f99a | 277 | } |
jebradshaw | 2:653433f4ee72 | 278 | |
jebradshaw | 3:71447d4fb4f0 | 279 | float Axis::readCurrent(void){ |
jebradshaw | 3:71447d4fb4f0 | 280 | this->motCurrent = this->_analog.read() * 3.3; |
jebradshaw | 3:71447d4fb4f0 | 281 | return this->motCurrent; |
jebradshaw | 2:653433f4ee72 | 282 | } |
jebradshaw | 2:653433f4ee72 | 283 | |
jebradshaw | 2:653433f4ee72 | 284 | void Axis::axisOff(void){ |
jebradshaw | 2:653433f4ee72 | 285 | update.detach(); |
jebradshaw | 2:653433f4ee72 | 286 | this->axisState = 0; |
jebradshaw | 2:653433f4ee72 | 287 | } |
jebradshaw | 2:653433f4ee72 | 288 | |
jebradshaw | 2:653433f4ee72 | 289 | void Axis::axisOn(void){ |
jebradshaw | 2:653433f4ee72 | 290 | update.attach(this, &Axis::paramUpdate, Tdelay); |
jebradshaw | 2:653433f4ee72 | 291 | this->axisState = 1; |
jebradshaw | 2:653433f4ee72 | 292 | } |