Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
cmd_mode.cpp
00001 #include "mbed.h" 00002 #include "CommandProcessor.h" 00003 #include "PreferenceWriter.h" 00004 00005 #include "prefs.h" 00006 #include "globals.h" 00007 00008 #include "hardware.h" 00009 #include "defaults.h" 00010 #include "derived.h" 00011 00012 #include "errors.h" 00013 #include "Calibration.h" 00014 00015 void cmd_setp(Serial *pc, char *buf) { 00016 if (BREMS_src == CMD_SRC_TERMINAL) { 00017 float x = atof(buf); 00018 if (BREMS_op == OP_DRIVING && x < 0.0f) x = 0.0f; 00019 control.user_cmd = x; 00020 if (!checks_passed() && control.user_cmd != 0.0f) { 00021 pc->printf("%s\n", "Errors present, setting to 0.0"); 00022 control.user_cmd = 0.0f; 00023 } 00024 } 00025 } 00026 00027 void cmdf_setp(Serial *pc, char c) { 00028 if (BREMS_src == CMD_SRC_TERMINAL) { 00029 float x = (float)(c - 127) / 128.0f; 00030 if (BREMS_op == OP_DRIVING && x < 0.0f) x = 0.0f; 00031 control.user_cmd = x; 00032 if (!checks_passed() && control.user_cmd != 0.0f) { 00033 pc->printf("%s\n", "Errors present, setting to 0.0"); 00034 control.user_cmd = 0.0f; 00035 } 00036 } 00037 } 00038 00039 void cmd_mode(Serial *pc, char *buf) { 00040 if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) { 00041 pc->printf("%s\n", "Turn off motor first"); 00042 return; 00043 } 00044 int n = str_to_mode(buf); 00045 if (n < 0) { 00046 pc->printf("%s\n", "Invalid Mode"); 00047 return; 00048 } 00049 BREMS_mode = n; 00050 00051 //ZERO and CHR are special modes - they execute run-once functions 00052 //and override commutation, then exit to CFG 00053 if (BREMS_mode == MODE_ZERO) { 00054 calibrate_position(&io); 00055 control.user_cmd = 0.0f; 00056 BREMS_mode = MODE_CFG; 00057 } 00058 else if (BREMS_mode == MODE_CHR) { 00059 control.user_cmd = 0.0f; 00060 BREMS_mode = MODE_CFG; 00061 } 00062 else 00063 { 00064 pc->printf("Set mode to %s\n", mode_to_str(n)); 00065 } 00066 } 00067 00068 void cmd_src(Serial *pc, char *buf) { 00069 if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) { 00070 pc->printf("%s\n", "Turn off motor first"); 00071 return; 00072 } 00073 int n = str_to_src(buf); 00074 if (n < 0) { 00075 pc->printf("%s\n", "Invalid Source"); 00076 return; 00077 } 00078 BREMS_src = n; 00079 pc->printf("Set source to %s\n", src_to_str(n)); 00080 } 00081 00082 void cmd_op(Serial *pc, char *buf) { 00083 if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) { 00084 pc->printf("%s\n", "Turn off motor first"); 00085 return; 00086 } 00087 int n = str_to_op(buf); 00088 if (n < 0) { 00089 pc->printf("%s\n", "Invalid operation"); 00090 return; 00091 } 00092 BREMS_op = n; 00093 pc->printf("Set operation to %s\n", op_to_str(n)); 00094 } 00095 00096 void cmd_exit(Serial *pc) { 00097 if (control.user_cmd != 0.0f || control.torque_percent != 0.0f) { 00098 pc->printf("%s\n", "Turn off motor first"); 00099 return; 00100 } 00101 BREMS_mode = MODE_RUN; 00102 pc->printf("%s\n", "Run mode ON"); 00103 }
Generated on Tue Jul 12 2022 17:58:39 by 1.7.2