motors
Fork of motor by
Embed:
(wiki syntax)
Show/hide line numbers
motor.cpp
00001 #include "motor.h" 00002 00003 Motor::Motor() { 00004 00005 } 00006 00007 Motor::Motor(USBSerial *pc, PwmOut *pwm, DigitalOut *dir1, DigitalOut *dir2, DigitalIn *fault) { 00008 _pc = pc; 00009 _pwm = pwm; 00010 _dir1 = dir1; 00011 _dir2 = dir2; 00012 _fault = fault; 00013 00014 enc_last = 0; 00015 00016 pid_on = 1; 00017 currentPWM = 0; 00018 00019 encTicks = 0; 00020 motor_polarity = 0; 00021 00022 pidSpeed0 = 0; 00023 pidSpeed1 = 0; 00024 pidError0 = 0; 00025 pidError1 = 0; 00026 pidError2 = 0; 00027 pidSetpoint = 0; 00028 00029 _pwm->period_us(PWM_PERIOD_US); 00030 } 00031 00032 void Motor::forward(float pwm) { 00033 if (dir) { 00034 *_dir1 = 0; 00035 *_dir2 = 1; 00036 } else { 00037 *_dir1 = 1; 00038 *_dir2 = 0; 00039 } 00040 00041 *_pwm = pwm; 00042 currentPWM = pwm; 00043 } 00044 00045 void Motor::backward(float pwm) { 00046 if (dir) { 00047 *_dir1 = 1; 00048 *_dir2 = 0; 00049 } else { 00050 *_dir1 = 0; 00051 *_dir2 = 1; 00052 } 00053 00054 *_pwm = pwm; 00055 currentPWM = -pwm; 00056 } 00057 00058 void Motor::pid2(int16_t encTicks) { 00059 speed = encTicks; 00060 float pidDiv = 100.0; 00061 00062 if (!pid_on) return; 00063 pidError2 = pidError1; 00064 pidError1 = pidError0; 00065 pidError0 = pidSetpoint - speed; 00066 pidSpeed1 = pidSpeed0; 00067 float p = pgain / pidDiv * (pidError0 - pidError1); 00068 float i = igain / pidDiv * (pidError0 + pidError1) / 2.0; 00069 float d = dgain / pidDiv * (pidError0 - 2.0 * pidError1 + pidError2); 00070 pidSpeed0 = pidSpeed1 + p + i + d; 00071 00072 00073 if (pidSpeed0 > 255) pidSpeed0 = 255; 00074 if (pidSpeed0 < -255) pidSpeed0 = -255; 00075 00076 if (pidSpeed0 > 0) forward((pidSpeed0) / 255.0); 00077 else backward((pidSpeed0) / -255.0); 00078 00079 if (pidSetpoint == 0) { 00080 forward(0); 00081 pidError0 = 0; 00082 pidError1 = 0; 00083 pidError2 = 0; 00084 pidSpeed0 = 0; 00085 pidSpeed1 = 0; 00086 } 00087 } 00088 00089 void Motor::pid(int16_t encTicks) { 00090 speed = encTicks; 00091 encTicks = 0; 00092 00093 if (!pid_on) return; 00094 00095 err_prev = err; 00096 err = sp_pid - speed; 00097 00098 if (stallLevel != 2) { 00099 intgrl += (err * pid_multi) / igain; 00100 00101 //constrain integral 00102 if (intgrl < -imax) intgrl = -imax; 00103 if (intgrl > imax) intgrl = imax; 00104 00105 if (sp == 0) pwmmin = 0; 00106 else if (sp < 0) pwmmin = -pwm_min; 00107 else pwmmin = pwm_min; 00108 00109 pwm = pwmmin + err*pgain + intgrl/pid_multi; 00110 //constrain pwm 00111 if (pwm < -255) pwm = -255; 00112 if (pwm > 255) pwm = 255; 00113 00114 prevStallCount = stallCount; 00115 if (((speed < 5 && currentPWM == 255) || (speed > -5 && currentPWM == -255)) && stallCount < stallErrorLimit) { 00116 stallCount++; 00117 } else if (stallCount > 0) { 00118 stallCount--; 00119 } 00120 00121 _pc->printf("pwm:%d\n", pwm); 00122 00123 if (pwm < 0) { 00124 pwm *= -1; 00125 backward(pwm/255.0); 00126 } else { 00127 forward(pwm/255.0); 00128 } 00129 00130 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) { 00131 stallLevel = 0; 00132 stallChanged = 1; 00133 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) { 00134 stallLevel = 1; 00135 stallChanged = 1; 00136 } else if (stallCount == stallErrorLimit) { 00137 stallLevel = 2; 00138 stallChanged = 1; 00139 reset_pid(); 00140 } 00141 } else { 00142 stallCount--; 00143 if (stallCount == 0) { 00144 stallLevel = 0; 00145 stallChanged = 1; 00146 } 00147 } 00148 00149 if ((fail_counter == 100) && failsafe) { 00150 sp_pid = 0; 00151 reset_pid(); 00152 forward(0); 00153 } 00154 00155 if (stallChanged) { 00156 _pc->printf("<stall:%d>\n", stallLevel); 00157 stallChanged = 0; 00158 } 00159 00160 if ((speed < 10) && (pwm > 250)) { 00161 stall_counter++; 00162 } else { 00163 stall_counter = 0; 00164 } 00165 00166 fail_counter++; 00167 00168 if ((fail_counter == 100) && failsafe) { 00169 sp_pid = 0; 00170 reset_pid(); 00171 forward(0); 00172 } 00173 } 00174 00175 void Motor::reset_pid() { 00176 err = 0; 00177 err_prev = 0; 00178 intgrl = 0; 00179 der = 0; 00180 sp = 0; 00181 sp_pid = 0; 00182 forward(0); 00183 } 00184 00185 00186 void Motor::setup() { 00187 pid_on = 0; 00188 forward(50); 00189 encTicks = 0; 00190 wait_ms(500); 00191 00192 if (speed < 0) { 00193 motor_polarity ^= 1; 00194 } 00195 00196 reset_pid(); 00197 forward(0); 00198 pid_on = 1; 00199 } 00200 00201 void Motor::init() { 00202 dir = 0; 00203 motor_polarity = 0; 00204 pgain = 50; 00205 igain = 10; 00206 dgain = 0; 00207 pid_multi = 32; 00208 imax = 255*pid_multi; 00209 err_max = 4000; 00210 pwm_min = 25; 00211 intgrl = 0; 00212 //count = 0; 00213 speed = 50; 00214 err = 0; 00215 } 00216 00217 int16_t Motor::getSpeed() { 00218 return speed; 00219 } 00220 00221 void Motor::setSpeed(int16_t speed) { 00222 sp_pid = speed; 00223 pidSetpoint = speed; 00224 if (sp_pid == 0) reset_pid(); 00225 fail_counter = 0; 00226 } 00227 00228 void Motor::getPIDGain(char *gain) { 00229 sprintf(gain, "PID:%d,%d,%d", pgain, igain, dgain); 00230 }
Generated on Mon Oct 2 2023 09:25:36 by
1.7.2
