motor.cpp

00001 #include "motor.h"
00002 
00003 Motor::Motor(Serial *pc, PinName pwm, PinName dir1, PinName dir2, PinName fault, PinName encA, PinName encB) :
00004     _pwm(pwm),
00005     _dir1(dir1),
00006     _dir2(dir2),
00007     _fault(fault),
00008     _encA(encA),
00009     _encB(encB),
00010     _pc(pc)
00011 {
00012     _encA.mode(PullNone);
00013     _encB.mode(PullNone);
00014 
00015     ticks = 0;
00016     encNow = 0;
00017     encLast = 0;
00018 
00019     enc_last = 0;
00020 
00021     pid_on = 1;
00022     currentPWM = 0;
00023 
00024     encTicks = 0;
00025     motor_polarity = 0;
00026 
00027     pidSpeed = 0;
00028     pidError = 0;
00029     pidErrorPrev = 0;
00030     pidSetpoint = 0;
00031 
00032     i = 0;
00033     counter = 0;
00034 
00035     _pwm.period_us(PWM_PERIOD_US);
00036 
00037     _encA.rise(this, &Motor::encTick);
00038     _encA.fall(this, &Motor::encTick);
00039     _encB.rise(this, &Motor::encTick);
00040     _encB.fall(this, &Motor::encTick);
00041 
00042     //pidTicker.attach(this, &Motor::pidTick, 1/PID_FREQ);
00043 
00044     dir = 0;
00045     motor_polarity = 0;
00046     pgain = 0.02;
00047     igain = 0.005;
00048     dgain = 0;
00049     pwm_min = 25;
00050     intgrl = 0;
00051     //count = 0;
00052     speed = 0;
00053     err = 0;
00054 }
00055 
00056 void Motor::forward(float pwm) {
00057     if (dir) {
00058         _dir1 = 0;
00059         _dir2 = 1;
00060     } else {
00061         _dir1 = 1;
00062         _dir2 = 0;
00063     }
00064 
00065     _pwm = pwm;
00066     currentPWM = pwm;
00067 }
00068 
00069 void Motor::backward(float pwm) {
00070     if (dir) {
00071         _dir1 = 1;
00072         _dir2 = 0;
00073     } else {
00074         _dir1 = 0;
00075         _dir2 = 1;
00076     }
00077 
00078     _pwm = pwm;
00079     currentPWM = -pwm;
00080 }
00081 
00082 void Motor::pid2(int16_t encTicks) {
00083     speed = encTicks;
00084 
00085     if (!pid_on) return;
00086     pidError = pidSetpoint - speed;
00087     float p = pgain * pidError;
00088     i += igain * pidError;
00089     float d = dgain * (pidError - pidErrorPrev);
00090     pidSpeed = p + i + d;
00091 
00092     if (i > 1) {
00093       i = 1;
00094     } else if (i < -1) {
00095       i = -1;
00096     }
00097 
00098     pidErrorPrev = pidError;
00099 
00100     if (pidSpeed > 1) pidSpeed = 1;
00101     if (pidSpeed < -1) pidSpeed = -1;
00102 
00103     if (pidSpeed > 0) forward(pidSpeed);
00104     else backward(-pidSpeed);
00105 
00106     if (pidSetpoint == 0) {
00107         forward(0);
00108         pidError = 0;
00109         pidSpeed = 0;
00110     }
00111 }
00112 
00113 void Motor::reset_pid() {
00114     err = 0;
00115     err_prev = 0;
00116     i = 0;
00117     der = 0;
00118     sp = 0;
00119     sp_pid = 0;
00120     forward(0);
00121 }
00122 
00123 int16_t Motor::getSpeed() {
00124     return speed;
00125 }
00126 
00127 void Motor::setSpeed(int16_t speed) {
00128     sp_pid = speed;
00129     pidSetpoint = speed;
00130     if (sp_pid == 0) reset_pid();
00131     fail_counter = 0;
00132 }
00133 
00134 void Motor::getPIDGain(char *gain) {
00135     sprintf(gain, "PID:%f,%f,%f", pgain, igain, dgain);
00136 }
00137 
00138 void Motor::pidTick() {
00139     pid2(ticks);
00140     ticks = 0;
00141 }
00142 
00143 void Motor::encTick() {
00144     uint8_t enc_dir;
00145     encNow = _encA.read() | (_encB.read() << 1);
00146     enc_dir = (encLast & 1) ^ ((encNow & 2) >> 1);
00147     encLast = encNow;
00148 
00149     if (enc_dir & 1) ticks++;
00150     else ticks--;
00151 }