Joseph Bradshaw / Axis

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

Dependencies:   LS7366LIB MotCon2 PID

Dependents:   LPC1768_6axis_Arm

Axis.cpp@9:7bc59203ce98, 2016-08-29 (annotated)

Committer:
jebradshaw
Date:
Mon Aug 29 19:42:07 2016 +0000
Revision:
9:7bc59203ce98
Parent:
8:7e399d7c990d
Child:
10:32faca5a2577
Update to avoid reading/writing during home interrupts

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