cmd_set_get.cpp

00001 #include "mbed.h"
00002 #include "CommandProcessor.h"
00003 #include "PreferenceWriter.h"
00004 
00005 #include "globals.h"
00006 #include "errors.h"
00007 #include "prefs.h"
00008 
00009 #include "tables.h"
00010 #include "defaults.h"
00011 #include "derived.h"
00012 
00013 void cmd_ls(Serial *pc) {
00014     if (control.enabled) {
00015         pc->printf("%s\n", "Turn motor off first");
00016         return;
00017     }
00018     
00019     DPRINT(PREFS_VALID);
00020     DPRINT(TORQUE_MODE);
00021     FPRINT(MAX_TQPCT_PLUS);
00022     FPRINT(MAX_TQPCT_MINUS);
00023     FPRINT(TORQUE_MAX);
00024     FPRINT(W_MAX);
00025     FPRINT(BUS_VOLTAGE);
00026     FPRINT(F_SW);
00027     FPRINT(K_LOOP_D);
00028     FPRINT(KI_BASE_D);
00029     FPRINT(K_LOOP_Q);
00030     FPRINT(KI_BASE_Q);
00031     FPRINT2(KP_D);
00032     FPRINT2(KI_D);
00033     FPRINT2(KP_Q);
00034     FPRINT2(KI_Q);
00035     FPRINT(F_SLOW_LOOP);
00036     FPRINT(INTEGRAL_MAX);
00037     FPRINT(OVERMODULATION_FACTOR);
00038     FPRINT(V_PHASE_SWIZZLE);
00039     DPRINT(W_FILTER_WINDOW);
00040     FPRINT(DQ_FILTER_STRENGTH);
00041     DPRINT(THROTTLE_FILTER_WINDOW);
00042     FPRINT(POLE_PAIRS);
00043     FPRINT(POS_OFFSET);
00044     FPRINT(RESOLVER_LOBES);
00045     DPRINT(CPR);
00046     FPRINT(Ld);
00047     FPRINT(Lq);
00048     FPRINT(FLUX_LINKAGE);
00049     FPRINT(Rs);
00050     FPRINT(KT);
00051     FPRINT(W_SAFE);
00052     FPRINT(W_CRAZY);
00053     DPRINT(TH_LIMIT_LOW);
00054     DPRINT(TH_LIMIT_HIGH);
00055     DPRINT2(ROWS);
00056     DPRINT2(COLUMNS);
00057     FPRINT(W_STEP);
00058     DPRINT(ENABLE_LOGGING);
00059     DPRINT(LOG_PAGE_SIZE);
00060     DPRINT(LOG_HEADER_SIZE);
00061     DPRINT(LOG_PACKET_SIZE);
00062     DPRINT(LOG_BAUD_RATE);
00063     FPRINT(K_LOOP_W);
00064     FPRINT(KI_BASE_W);
00065     FPRINT(W_SETPOINT_MAX);
00066     FPRINT(W_LOOP_MAX_TQ);
00067 }
00068 
00069 #define ls_specialf(a) if (strcmp(buf, #a) == 0) {pc->printf("%s: %f\n", #a, a); return;}
00070 #define ls_speciald(a) if (strcmp(buf, #a) == 0) {pc->printf("%s: %d\n", #a, a); return;}
00071 void cmd_ls2(Serial *pc, char *buf) {    
00072     if (strcmp(buf, "mode") == 0) {
00073         pc->printf("%s\n", mode_to_str(BREMS_mode));
00074         return;
00075     }
00076     if (strcmp(buf, "src") == 0) {
00077         pc->printf("%s\n", src_to_str(BREMS_src));
00078         return;
00079     }
00080     if (strcmp(buf, "op") == 0) {
00081         pc->printf("%s\n", op_to_str(BREMS_op));
00082         return;
00083     }
00084     
00085     ls_specialf(KP_D);
00086     ls_specialf(KP_Q);
00087     ls_specialf(KP_W);
00088     ls_specialf(KI_D);
00089     ls_specialf(KI_Q);
00090     ls_specialf(KI_W);
00091     
00092     ls_speciald(ROWS);
00093     ls_speciald(COLUMNS);
00094     
00095     float *fptr = checkf(buf);
00096     if (fptr != NULL) pc->printf("%s: %f\n", buf, *fptr);
00097     int *nptr = NULL;
00098     if (fptr == NULL) nptr = checkn(buf);
00099     if (nptr != NULL) pc->printf("%s: %d\n", buf, *nptr);
00100     if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
00101 }
00102 
00103 void cmd_defaults(Serial *pc) {
00104     if (BREMS_mode == MODE_RUN || BREMS_mode == MODE_ZERO || BREMS_mode == MODE_CHR) {
00105         pc->printf("%s\n", "Can only do that in CFG mode");
00106         return;
00107     }
00108     
00109     DEFAULT(TORQUE_MODE);
00110     DEFAULT(MAX_TQPCT_PLUS);
00111     DEFAULT(MAX_TQPCT_MINUS);
00112     DEFAULT(TORQUE_MAX);
00113     DEFAULT(W_MAX);
00114     DEFAULT(BUS_VOLTAGE);
00115     DEFAULT(F_SW);
00116     DEFAULT(K_LOOP_D);
00117     DEFAULT(KI_BASE_D);
00118     DEFAULT(K_LOOP_Q);
00119     DEFAULT(KI_BASE_Q);
00120     DEFAULT(F_SLOW_LOOP);
00121     DEFAULT(INTEGRAL_MAX);
00122     DEFAULT(OVERMODULATION_FACTOR);
00123     DEFAULT(V_PHASE_SWIZZLE);
00124     DEFAULT(W_FILTER_WINDOW);
00125     DEFAULT(DQ_FILTER_STRENGTH);
00126     DEFAULT(THROTTLE_FILTER_WINDOW);
00127     DEFAULT(POLE_PAIRS);
00128     DEFAULT(POS_OFFSET);
00129     DEFAULT(RESOLVER_LOBES);
00130     DEFAULT(CPR);
00131     DEFAULT(Ld);
00132     DEFAULT(Lq);
00133     DEFAULT(FLUX_LINKAGE);
00134     DEFAULT(Rs);
00135     DEFAULT(KT);
00136     DEFAULT(W_SAFE);
00137     DEFAULT(W_CRAZY);
00138     DEFAULT(TH_LIMIT_LOW);
00139     DEFAULT(TH_LIMIT_HIGH);
00140     DEFAULT(W_STEP);
00141     DEFAULT(ENABLE_LOGGING);
00142     DEFAULT(LOG_PAGE_SIZE);
00143     DEFAULT(LOG_HEADER_SIZE);
00144     DEFAULT(LOG_PACKET_SIZE);
00145     DEFAULT(LOG_BAUD_RATE);
00146     DEFAULT(K_LOOP_W);
00147     DEFAULT(KI_BASE_W);
00148     DEFAULT(W_SETPOINT_MAX);
00149     DEFAULT(W_LOOP_MAX_TQ);
00150     pc->printf("Defaults Loaded\n");
00151 }
00152 
00153 void cmd_reload(Serial *pc, PreferenceWriter *pref) {
00154     if (BREMS_mode == MODE_RUN || BREMS_mode == MODE_ZERO || BREMS_mode == MODE_CHR) {
00155         pc->printf("%s\n", "Can only do that in CFG mode");
00156         return;
00157     }
00158     
00159     pref->load();
00160     if (_PREFS_VALID) pc->printf("Flash Values Loaded\n");
00161 }
00162 
00163 void cmd_set(Serial *pc, char *buf, char *val) {
00164     float *fptr = checkf(buf);
00165     if (fptr != NULL) *fptr = (float) (atof(val));
00166     int *nptr = NULL;
00167     if (fptr == NULL) nptr = checkn(buf);
00168     if (nptr != NULL) *nptr = (int) (atoi(val));
00169     if (nptr != NULL || fptr != NULL) cmd_ls2(pc, buf);
00170     if (nptr == NULL && fptr == NULL) pc->printf("%s\n", "No Such Parameter");
00171 }
00172 
00173 void cmd_flush(Serial *pc, PreferenceWriter *pref) {
00174     if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) {
00175         pc->printf("%s\n", "Turn off motor first");
00176         return;
00177     }
00178     io.logger->disable();
00179     if (!pref->ready()) pref->open();
00180     _PREFS_VALID = 1;
00181     pref->flush();
00182     io.logger->enable();
00183 }
00184 
00185 void cmd_query_err(Serial *pc) {
00186     if ((moded_errors & (1 << ERR_THROTTLE_DISABLED)) != 0) pc->printf("%s\n", "Throttle disabled");
00187     if ((moded_errors & (1 << ERR_POS_INVALID)) != 0) pc->printf("%s\n", "No encoder index");
00188     if ((moded_errors & (1 << ERR_NOT_DRIVING)) != 0) pc->printf("%s\n", "Not driving");
00189     if (moded_errors == 0) pc->printf("%s\n", "No errors");
00190 }
00191 
00192 void cmd_query(Serial *pc, char *buf) {
00193     if (strcmp(buf, "pos") == 0) pc->printf("%f\n", foc.p);
00194     else if (strcmp(buf, "w") == 0) pc->printf("%f\n", read.w);
00195     else if (strcmp(buf, "cmd") == 0) pc->printf("%f\n", control.user_cmd);
00196     else if (strcmp(buf, "d") == 0) pc->printf("%f\n", foc.d);
00197     else if (strcmp(buf, "q") == 0) pc->printf("%f\n", foc.q);
00198     else if (strcmp(buf, "d_ref") == 0) pc->printf("%f\n", control.d_ref);
00199     else if (strcmp(buf, "q_ref") == 0) pc->printf("%f\n", control.q_ref);
00200     else if (strcmp(buf, "vd") == 0) pc->printf("%f\n", foc.vd);
00201     else if (strcmp(buf, "vq") == 0) pc->printf("%f\n", foc.vq);
00202     else if (strcmp(buf, "tq") == 0) pc->printf("%f\n", control.torque_percent);
00203     else if (strcmp(buf, "ia") == 0) pc->printf("%f\n", foc.ia);
00204     else if (strcmp(buf, "ib") == 0) pc->printf("%f\n", foc.ib);
00205     else if (strcmp(buf, "ic") == 0) pc->printf("%f\n", -foc.ia - foc.ib);
00206     else if (strcmp(buf, "errors") == 0) cmd_query_err(pc);
00207     else pc->printf("%s\n", "No such parameter");
00208 }
00209 
00210 void cmdf_w(Serial *pc) {
00211     union {int16_t s; uint16_t u;};
00212     s = (int)(read.w);
00213     uint8_t h, l;
00214     h = (u & 0xff00) >> 8;
00215     l = (u & 0x00ff);
00216     pc->putc(h);
00217     pc->putc(l);
00218 }
00219 
00220 void cmdf_data(Serial *pc) {
00221     char packet[8];
00222     
00223     packet[0] = (char)(read.w / 8.0f);
00224     packet[1] = (char)(control.d_ref / 2.0f + 128.0f);
00225     packet[2] = (char)(control.d_filtered / 2.0f + 128.0f);
00226     packet[3] = (char)(control.q_ref / 2.0f + 128.0f);
00227     packet[4] = (char)(control.q_filtered / 2.0f + 128.0f);
00228     packet[5] = (char)(255.0f * control.torque_percent);
00229     packet[6] = (char)(255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f);
00230     packet[7] = (char)(255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f);
00231     
00232     pc->putc(packet[0]); pc->putc(packet[1]); pc->putc(packet[2]); pc->putc(packet[3]);
00233     pc->putc(packet[4]); pc->putc(packet[5]); pc->putc(packet[6]); pc->putc(packet[7]);
00234 }