robot

Dependencies:   FastPWM3 mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }