Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 00004 #include "PositionSensor.h" 00005 #include "FastPWM.h" 00006 #include "PwmIn.h" 00007 #include "MathHelpers.h" 00008 #include "Transforms.h" 00009 #include "DQMapper.h" 00010 #include "ThrottleMapper.h" 00011 #include "Calibration.h" 00012 #include "Filter.h" 00013 #include "LedBlinker.h" 00014 00015 #include "BREMSStructs.h" 00016 #include "BREMSConfig.h" 00017 00018 #include "prefs.h" 00019 #include "hardware.h" 00020 #include "derived.h" 00021 #include "main.h" 00022 00023 #include "errors.h" 00024 00025 IOStruct io; 00026 ReadDataStruct read; 00027 FOCStruct foc; 00028 ControlStruct control; 00029 00030 DQMapper *dq; 00031 ThrottleMapper *th; 00032 00033 int loop_counter = 0; 00034 00035 void commutate() { 00036 /*safety checks, do we do anything this cycle?*/ 00037 if (checks_passed()) { 00038 go_enabled(); 00039 } 00040 00041 /*update velocity, user command*/ 00042 update_velocity(); 00043 if (loop_counter % SLOW_LOOP_COUNTER == 0) { 00044 loop_counter = 0; 00045 slow_loop(); 00046 } 00047 loop_counter++; 00048 io.blink->update(); 00049 00050 /*generate torque command from user command*/ 00051 float w_ref, w_err; 00052 switch (BREMS_op) { 00053 case OP_TORQUE: 00054 control.w_integral = 0.0f; 00055 control.torque_percent = control.user_cmd; 00056 if (control.torque_percent < -1.0f) control.torque_percent = -1.0f; 00057 if (control.torque_percent > 1.0f) control.torque_percent = 1.0f; 00058 break; 00059 case OP_DRIVING: 00060 control.w_integral = 0.0f; 00061 control.torque_percent = th->map(control.user_cmd, read.w); 00062 break; 00063 case OP_SPEED: 00064 w_ref = control.user_cmd * _W_SETPOINT_MAX; 00065 w_err = w_ref - read.w; 00066 control.w_integral += w_err * KI_W; 00067 control.w_integral = constrain(control.w_integral, -_W_LOOP_MAX_TQ, _W_LOOP_MAX_TQ); 00068 control.torque_percent = KP_W * w_err + control.w_integral; 00069 control.torque_percent = constrain(control.torque_percent, -_W_LOOP_MAX_TQ, _W_LOOP_MAX_TQ); 00070 break; 00071 case OP_POSITION: 00072 break; 00073 default: 00074 //this should never happen! 00075 control.torque_percent = 0.0f; 00076 break; 00077 } 00078 00079 /*update references*/ 00080 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref); 00081 00082 /*update position, sin, cos*/ 00083 foc.p = io.pos->GetElecPosition() - _POS_OFFSET; 00084 float sin_p = sinf(foc.p); 00085 float cos_p = cosf(foc.p); 00086 00087 /*scale and offset currents (adval1, 2 are updated in ISR)*/ 00088 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ad1_supp_offset) / I_SCALE; 00089 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ad2_supp_offset) / I_SCALE; 00090 00091 /*compute d, q*/ 00092 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta); 00093 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q); 00094 00095 /*PI controller*/ 00096 control.d_filtered = update_filter(control.d_filtered, foc.d, _DQ_FILTER_STRENGTH); 00097 control.q_filtered = update_filter(control.q_filtered, foc.q, _DQ_FILTER_STRENGTH); 00098 00099 float d_err = control.d_ref - control.d_filtered; 00100 float q_err = control.q_ref - control.q_filtered; 00101 00102 control.d_integral += d_err * KI_D; 00103 control.q_integral += q_err * KI_Q; 00104 00105 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX); 00106 00107 foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f; 00108 foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f; 00109 00110 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f); 00111 00112 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple; 00113 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple; 00114 00115 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR); 00116 00117 float pv = foc.p + read.w / _V_PHASE_SWIZZLE; 00118 float sin_pv = sinf(pv); 00119 float cos_pv = cosf(pv); 00120 00121 /*inverse transforms*/ 00122 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta); 00123 00124 /*set outputs*/ 00125 float va, vb, vc, voff; 00126 00127 invclarke(foc.valpha, foc.vbeta, &va, &vb); 00128 vc = -va - vb; 00129 00130 /*SVPWM*/ 00131 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it 00132 va = va - voff; 00133 vb = vb - voff; 00134 vc = vc - voff; 00135 00136 /*safety checks, reset integral*/ 00137 if (!checks_passed() || !mode_enables_output()) { 00138 go_disabled(); 00139 } 00140 00141 /*log data*/ 00142 if (_ENABLE_LOGGING && mode_enables_logging()) log(); 00143 00144 /*disable outputs if necessary*/ 00145 if (!control.enabled) { 00146 va = 0.0f; vb = 0.0f; vc = 0.0f; 00147 } 00148 00149 //output to timers (some of the calibration modes may want to override this) 00150 //CFG does not override timers - the PWM outputs need to be set to 0 somewhere 00151 if (!mode_overrides_timers()) { 00152 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX); 00153 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); 00154 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); 00155 } 00156 } 00157 00158 void slow_loop() { 00159 switch (BREMS_src) { 00160 case CMD_SRC_RC: 00161 control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle()); 00162 break; 00163 case CMD_SRC_ANALOG: 00164 //handle analog throttle here 00165 break; 00166 case CMD_SRC_TERMINAL: 00167 case CMD_SRC_SERIAL: 00168 case CMD_SRC_CAN: 00169 case CMD_SRC_INTERNAL: 00170 //these sources are updated by callbacks 00171 break; 00172 default: 00173 //this should never happen! 00174 control.user_cmd = 0.0f; 00175 break; 00176 } 00177 } 00178 00179 void log() { 00180 float packet[8]; 00181 packet[0] = read.w / 8.0f; 00182 packet[1] = control.d_ref / 2.0f + 128.0f; 00183 packet[2] = control.d_filtered / 2.0f + 128.0f; 00184 packet[3] = control.q_ref / 2.0f + 128.0f; 00185 packet[4] = control.q_filtered / 2.0f + 128.0f; 00186 packet[5] = 255.0f * control.torque_percent; 00187 packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f; 00188 packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f; 00189 io.logger->log(packet); 00190 } 00191 00192 int main() { 00193 dq = new InterpolatingLutMapper(); 00194 th = new LimitingThrottleMapper(1500.0f); 00195 00196 BREMSInit(&io, &read, &foc, &control, false); 00197 io.pc->attach(rxCallback); 00198 00199 for (;;) { 00200 io.logger->flush(); 00201 } 00202 }
Generated on Tue Jul 12 2022 17:58:39 by 1.7.2