615 lines
19 KiB
C++
615 lines
19 KiB
C++
#include "Commander.h"
|
|
|
|
Commander::Commander(Stream& serial, char eol, bool echo){
|
|
com_port = &serial;
|
|
this->eol = eol;
|
|
this->echo = echo;
|
|
}
|
|
Commander::Commander(char eol, bool echo){
|
|
this->eol = eol;
|
|
this->echo = echo;
|
|
}
|
|
|
|
|
|
void Commander::add(char id, CommandCallback onCommand, char* label ){
|
|
call_list[call_count] = onCommand;
|
|
call_ids[call_count] = id;
|
|
call_label[call_count] = label;
|
|
call_count++;
|
|
}
|
|
|
|
|
|
void Commander::run(){
|
|
if(!com_port) return;
|
|
run(*com_port, eol);
|
|
}
|
|
|
|
void Commander::run(Stream& serial, char eol){
|
|
Stream* tmp = com_port; // save the serial instance
|
|
char eol_tmp = this->eol;
|
|
this->eol = eol;
|
|
com_port = &serial;
|
|
|
|
// a string to hold incoming data
|
|
while (serial.available()) {
|
|
// get the new byte:
|
|
int ch = serial.read();
|
|
received_chars[rec_cnt++] = (char)ch;
|
|
// end of user input
|
|
if(echo)
|
|
print((char)ch);
|
|
if (isSentinel(ch)) {
|
|
// execute the user command
|
|
run(received_chars);
|
|
|
|
// reset the command buffer
|
|
received_chars[0] = 0;
|
|
rec_cnt=0;
|
|
}
|
|
if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
|
|
received_chars[0] = 0;
|
|
rec_cnt=0;
|
|
}
|
|
}
|
|
|
|
com_port = tmp; // reset the instance to the internal value
|
|
this->eol = eol_tmp;
|
|
}
|
|
|
|
void Commander::run(char* user_input){
|
|
// execute the user command
|
|
char id = user_input[0];
|
|
switch(id){
|
|
case CMD_SCAN:
|
|
for(int i=0; i < call_count; i++){
|
|
print(call_ids[i]);
|
|
print(":");
|
|
if(call_label[i]) println(call_label[i]);
|
|
else println("");
|
|
}
|
|
break;
|
|
case CMD_VERBOSE:
|
|
if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
|
|
printVerbose(F("Verb:"));
|
|
switch (verbose){
|
|
case VerboseMode::nothing:
|
|
println(F("off!"));
|
|
break;
|
|
case VerboseMode::on_request:
|
|
case VerboseMode::user_friendly:
|
|
println(F("on!"));
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_DECIMAL:
|
|
if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
|
|
printVerbose(F("Decimal:"));
|
|
println(decimal_places);
|
|
break;
|
|
default:
|
|
for(int i=0; i < call_count; i++){
|
|
if(id == call_ids[i]){
|
|
call_list[i](&user_input[1]);
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Commander::motor(FOCMotor* motor, char* user_command) {
|
|
|
|
// if target setting
|
|
if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){
|
|
target(motor, user_command);
|
|
return;
|
|
}
|
|
|
|
// parse command letter
|
|
char cmd = user_command[0];
|
|
char sub_cmd = user_command[1];
|
|
// check if there is a subcommand or not
|
|
int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1;
|
|
// check if get command
|
|
bool GET = isSentinel(user_command[value_index]);
|
|
// parse command values
|
|
float value = atof(&user_command[value_index]);
|
|
|
|
|
|
// a bit of optimisation of variable memory for Arduino UNO (atmega328)
|
|
switch(cmd){
|
|
case CMD_C_Q_PID: //
|
|
printVerbose(F("PID curr q| "));
|
|
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]);
|
|
else pid(&motor->PID_current_q,&user_command[1]);
|
|
break;
|
|
case CMD_C_D_PID: //
|
|
printVerbose(F("PID curr d| "));
|
|
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]);
|
|
else pid(&motor->PID_current_d, &user_command[1]);
|
|
break;
|
|
case CMD_V_PID: //
|
|
printVerbose(F("PID vel| "));
|
|
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]);
|
|
else pid(&motor->PID_velocity, &user_command[1]);
|
|
break;
|
|
case CMD_A_PID: //
|
|
printVerbose(F("PID angle| "));
|
|
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]);
|
|
else pid(&motor->P_angle, &user_command[1]);
|
|
break;
|
|
case CMD_LIMITS: //
|
|
printVerbose(F("Limits| "));
|
|
switch (sub_cmd){
|
|
case SCMD_LIM_VOLT: // voltage limit change
|
|
printVerbose(F("volt: "));
|
|
if(!GET) {
|
|
motor->voltage_limit = value;
|
|
motor->PID_current_d.limit = value;
|
|
motor->PID_current_q.limit = value;
|
|
// change velocity pid limit if in voltage mode and no phase resistance set
|
|
if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
|
|
}
|
|
println(motor->voltage_limit);
|
|
break;
|
|
case SCMD_LIM_CURR: // current limit
|
|
printVerbose(F("curr: "));
|
|
if(!GET){
|
|
motor->current_limit = value;
|
|
// if phase resistance specified or the current control is on set the current limit to the velocity PID
|
|
if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
|
|
}
|
|
println(motor->current_limit);
|
|
break;
|
|
case SCMD_LIM_VEL: // velocity limit
|
|
printVerbose(F("vel: "));
|
|
if(!GET){
|
|
motor->velocity_limit = value;
|
|
motor->P_angle.limit = value;
|
|
}
|
|
println(motor->velocity_limit);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_MOTION_TYPE:
|
|
case CMD_TORQUE_TYPE:
|
|
case CMD_STATUS:
|
|
motion(motor, &user_command[0]);
|
|
break;
|
|
case CMD_PWMMOD:
|
|
// PWM modulation change
|
|
printVerbose(F("PWM Mod | "));
|
|
switch (sub_cmd){
|
|
case SCMD_PWMMOD_TYPE: // zero offset
|
|
printVerbose(F("type: "));
|
|
if(!GET) motor->foc_modulation = (FOCModulationType)value;
|
|
switch(motor->foc_modulation){
|
|
case FOCModulationType::SinePWM:
|
|
println(F("SinePWM"));
|
|
break;
|
|
case FOCModulationType::SpaceVectorPWM:
|
|
println(F("SVPWM"));
|
|
break;
|
|
case FOCModulationType::Trapezoid_120:
|
|
println(F("Trap 120"));
|
|
break;
|
|
case FOCModulationType::Trapezoid_150:
|
|
println(F("Trap 150"));
|
|
break;
|
|
}
|
|
break;
|
|
case SCMD_PWMMOD_CENTER: // centered modulation
|
|
printVerbose(F("center: "));
|
|
if(!GET) motor->modulation_centered = value;
|
|
println(motor->modulation_centered);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_RESIST:
|
|
// enable/disable
|
|
printVerbose(F("R phase: "));
|
|
if(!GET){
|
|
motor->phase_resistance = value;
|
|
if(motor->torque_controller==TorqueControlType::voltage){
|
|
motor->voltage_limit = motor->current_limit*value;
|
|
motor->PID_velocity.limit= motor->current_limit;
|
|
}
|
|
}
|
|
if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
|
|
else println(0);
|
|
break;
|
|
case CMD_SENSOR:
|
|
// Sensor zero offset
|
|
printVerbose(F("Sensor | "));
|
|
switch (sub_cmd){
|
|
case SCMD_SENS_MECH_OFFSET: // zero offset
|
|
printVerbose(F("offset: "));
|
|
if(!GET) motor->sensor_offset = value;
|
|
println(motor->sensor_offset);
|
|
break;
|
|
case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch
|
|
printVerbose(F("el. offset: "));
|
|
if(!GET) motor->zero_electric_angle = value;
|
|
println(motor->zero_electric_angle);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_MONITOR: // get current values of the state variables
|
|
printVerbose(F("Monitor | "));
|
|
switch (sub_cmd){
|
|
case SCMD_GET: // get command
|
|
switch((uint8_t)value){
|
|
case 0: // get target
|
|
printVerbose(F("target: "));
|
|
println(motor->target);
|
|
break;
|
|
case 1: // get voltage q
|
|
printVerbose(F("Vq: "));
|
|
println(motor->voltage.q);
|
|
break;
|
|
case 2: // get voltage d
|
|
printVerbose(F("Vd: "));
|
|
println(motor->voltage.d);
|
|
break;
|
|
case 3: // get current q
|
|
printVerbose(F("Cq: "));
|
|
println(motor->current.q);
|
|
break;
|
|
case 4: // get current d
|
|
printVerbose(F("Cd: "));
|
|
println(motor->current.d);
|
|
break;
|
|
case 5: // get velocity
|
|
printVerbose(F("vel: "));
|
|
println(motor->shaft_velocity);
|
|
break;
|
|
case 6: // get angle
|
|
printVerbose(F("angle: "));
|
|
println(motor->shaft_angle);
|
|
break;
|
|
case 7: // get all states
|
|
printVerbose(F("all: "));
|
|
print(motor->target);
|
|
print(";");
|
|
print(motor->voltage.q);
|
|
print(";");
|
|
print(motor->voltage.d);
|
|
print(";");
|
|
print(motor->current.q);
|
|
print(";");
|
|
print(motor->current.d);
|
|
print(";");
|
|
print(motor->shaft_velocity);
|
|
print(";");
|
|
println(motor->shaft_angle);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
break;
|
|
case SCMD_DOWNSAMPLE:
|
|
printVerbose(F("downsample: "));
|
|
if(!GET) motor->monitor_downsample = value;
|
|
println((int)motor->monitor_downsample);
|
|
break;
|
|
case SCMD_CLEAR:
|
|
motor->monitor_variables = (uint8_t) 0;
|
|
println(F("clear"));
|
|
break;
|
|
case SCMD_SET:
|
|
if(!GET) motor->monitor_variables = (uint8_t) 0;
|
|
for(int i = 0; i < 7; i++){
|
|
if(isSentinel(user_command[value_index+i])) break;
|
|
if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i);
|
|
print( (user_command[value_index+i] - '0') );
|
|
}
|
|
println("");
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
break;
|
|
default: // unknown cmd
|
|
printVerbose(F("unknown cmd "));
|
|
printError();
|
|
}
|
|
}
|
|
|
|
void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
|
|
char cmd = user_cmd[0];
|
|
char sub_cmd = user_cmd[1];
|
|
bool GET = isSentinel(user_cmd[1]);
|
|
float value = atof(&user_cmd[(sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1]);
|
|
|
|
switch(cmd){
|
|
case CMD_MOTION_TYPE:
|
|
printVerbose(F("Motion:"));
|
|
switch(sub_cmd){
|
|
case SCMD_DOWNSAMPLE:
|
|
printVerbose(F(" downsample: "));
|
|
if(!GET) motor->motion_downsample = value;
|
|
println((int)motor->motion_downsample);
|
|
break;
|
|
default:
|
|
// change control type
|
|
if(!GET && value >= 0 && (int)value < 5) // if set command
|
|
motor->controller = (MotionControlType)value;
|
|
switch(motor->controller){
|
|
case MotionControlType::torque:
|
|
println(F("torque"));
|
|
break;
|
|
case MotionControlType::velocity:
|
|
println(F("vel"));
|
|
break;
|
|
case MotionControlType::angle:
|
|
println(F("angle"));
|
|
break;
|
|
case MotionControlType::velocity_openloop:
|
|
println(F("vel open"));
|
|
break;
|
|
case MotionControlType::angle_openloop:
|
|
println(F("angle open"));
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_TORQUE_TYPE:
|
|
// change control type
|
|
printVerbose(F("Torque: "));
|
|
if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
|
|
motor->torque_controller = (TorqueControlType)value;
|
|
switch(motor->torque_controller){
|
|
case TorqueControlType::voltage:
|
|
println(F("volt"));
|
|
// change the velocity control limits if necessary
|
|
if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
|
|
break;
|
|
case TorqueControlType::dc_current:
|
|
println(F("dc curr"));
|
|
// change the velocity control limits if necessary
|
|
motor->PID_velocity.limit = motor->current_limit;
|
|
break;
|
|
case TorqueControlType::foc_current:
|
|
println(F("foc curr"));
|
|
// change the velocity control limits if necessary
|
|
motor->PID_velocity.limit = motor->current_limit;
|
|
break;
|
|
}
|
|
break;
|
|
case CMD_STATUS:
|
|
// enable/disable
|
|
printVerbose(F("Status: "));
|
|
if(!GET) (bool)value ? motor->enable() : motor->disable();
|
|
println(motor->enabled);
|
|
break;
|
|
default:
|
|
target(motor, user_cmd, separator);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Commander::pid(PIDController* pid, char* user_cmd){
|
|
char cmd = user_cmd[0];
|
|
bool GET = isSentinel(user_cmd[1]);
|
|
float value = atof(&user_cmd[1]);
|
|
|
|
switch (cmd){
|
|
case SCMD_PID_P: // P gain change
|
|
printVerbose("P: ");
|
|
if(!GET) pid->P = value;
|
|
println(pid->P);
|
|
break;
|
|
case SCMD_PID_I: // I gain change
|
|
printVerbose("I: ");
|
|
if(!GET) pid->I = value;
|
|
println(pid->I);
|
|
break;
|
|
case SCMD_PID_D: // D gain change
|
|
printVerbose("D: ");
|
|
if(!GET) pid->D = value;
|
|
println(pid->D);
|
|
break;
|
|
case SCMD_PID_RAMP: // ramp change
|
|
printVerbose("ramp: ");
|
|
if(!GET) pid->output_ramp = value;
|
|
println(pid->output_ramp);
|
|
break;
|
|
case SCMD_PID_LIM: // limit change
|
|
printVerbose("limit: ");
|
|
if(!GET) pid->limit = value;
|
|
println(pid->limit);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
|
|
char cmd = user_cmd[0];
|
|
bool GET = isSentinel(user_cmd[1]);
|
|
float value = atof(&user_cmd[1]);
|
|
|
|
switch (cmd){
|
|
case SCMD_LPF_TF: // Tf value change
|
|
printVerbose(F("Tf: "));
|
|
if(!GET) lpf->Tf = value;
|
|
println(lpf->Tf);
|
|
break;
|
|
default:
|
|
printError();
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Commander::scalar(float* value, char* user_cmd){
|
|
bool GET = isSentinel(user_cmd[0]);
|
|
if(!GET) *value = atof(user_cmd);
|
|
println(*value);
|
|
}
|
|
|
|
|
|
void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){
|
|
// if no values sent
|
|
if(isSentinel(user_cmd[0])) return;
|
|
|
|
float pos, vel, torque;
|
|
char* next_value;
|
|
switch(motor->controller){
|
|
case MotionControlType::torque: // setting torque target
|
|
torque = atof(strtok (user_cmd, separator));
|
|
motor->target = torque;
|
|
break;
|
|
case MotionControlType::velocity: // setting velocity target + torque limit
|
|
// set the target
|
|
vel= atof(strtok (user_cmd, separator));
|
|
motor->target = vel;
|
|
|
|
// allow for setting only the target velocity without chaning the torque limit
|
|
next_value = strtok (NULL, separator);
|
|
if (next_value){
|
|
torque = atof(next_value);
|
|
motor->PID_velocity.limit = torque;
|
|
// torque command can be voltage or current
|
|
if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
|
|
else motor->current_limit = torque;
|
|
}
|
|
break;
|
|
case MotionControlType::angle: // setting angle target + torque, velocity limit
|
|
// setting the target position
|
|
pos= atof(strtok (user_cmd, separator));
|
|
motor->target = pos;
|
|
|
|
// allow for setting only the target position without chaning the velocity/torque limits
|
|
next_value = strtok (NULL, separator);
|
|
if( next_value ){
|
|
vel = atof(next_value);
|
|
motor->velocity_limit = vel;
|
|
motor->P_angle.limit = vel;
|
|
|
|
// allow for setting only the target position and velocity limit without the torque limit
|
|
next_value = strtok (NULL, separator);
|
|
if( next_value ){
|
|
torque= atof(next_value);
|
|
motor->PID_velocity.limit = torque;
|
|
// torque command can be voltage or current
|
|
if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
|
|
else motor->current_limit = torque;
|
|
}
|
|
}
|
|
break;
|
|
case MotionControlType::velocity_openloop: // setting velocity target + torque limit
|
|
// set the target
|
|
vel= atof(strtok (user_cmd, separator));
|
|
motor->target = vel;
|
|
// allow for setting only the target velocity without chaning the torque limit
|
|
next_value = strtok (NULL, separator);
|
|
if (next_value ){
|
|
torque = atof(next_value);
|
|
// torque command can be voltage or current
|
|
if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
|
|
else motor->current_limit = torque;
|
|
}
|
|
break;
|
|
case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
|
|
// set the target
|
|
pos= atof(strtok (user_cmd, separator));
|
|
motor->target = pos;
|
|
|
|
// allow for setting only the target position without chaning the velocity/torque limits
|
|
next_value = strtok (NULL, separator);
|
|
if( next_value ){
|
|
vel = atof(next_value);
|
|
motor->velocity_limit = vel;
|
|
// allow for setting only the target velocity without chaning the torque limit
|
|
next_value = strtok (NULL, separator);
|
|
if (next_value ){
|
|
torque = atof(next_value);
|
|
// torque command can be voltage or current
|
|
if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
|
|
else motor->current_limit = torque;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
printVerbose(F("Target: "));
|
|
println(motor->target);
|
|
}
|
|
|
|
|
|
bool Commander::isSentinel(char ch)
|
|
{
|
|
if(ch == eol)
|
|
return true;
|
|
else if (ch == '\r')
|
|
{
|
|
printVerbose(F("Warn: \\r detected! \n"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Commander::print(const int number){
|
|
if( !com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->print(number);
|
|
}
|
|
void Commander::print(const float number){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->print((float)number,(int)decimal_places);
|
|
}
|
|
void Commander::print(const char* message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->print(message);
|
|
}
|
|
void Commander::print(const __FlashStringHelper *message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->print(message);
|
|
}
|
|
void Commander::print(const char message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->print(message);
|
|
}
|
|
|
|
void Commander::println(const int number){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->println(number);
|
|
}
|
|
void Commander::println(const float number){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->println((float)number, (int)decimal_places);
|
|
}
|
|
void Commander::println(const char* message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->println(message);
|
|
}
|
|
void Commander::println(const __FlashStringHelper *message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->println(message);
|
|
}
|
|
void Commander::println(const char message){
|
|
if(!com_port || verbose == VerboseMode::nothing ) return;
|
|
com_port->println(message);
|
|
}
|
|
|
|
|
|
void Commander::printVerbose(const char* message){
|
|
if(verbose == VerboseMode::user_friendly) print(message);
|
|
}
|
|
void Commander::printVerbose(const __FlashStringHelper *message){
|
|
if(verbose == VerboseMode::user_friendly) print(message);
|
|
}
|
|
void Commander::printError(){
|
|
println(F("err"));
|
|
}
|