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@0:cf7192f9f99a, 2015-08-31 (annotated)

Committer:
jebradshaw
Date:
Mon Aug 31 17:14:20 2015 +0000
Revision:
0:cf7192f9f99a
Child:
1:cd249816dba8
Used to control an axis on a robotic arm

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