Add some basic bounds checking during calibration

This commit is contained in:
Scott Bezek 2023-06-13 22:29:28 -07:00
parent 3fdd9fa601
commit 08146e292d

View File

@ -338,6 +338,7 @@ void MotorTask::calibrate() {
// TODO: dig into SimpleFOC calibration and find/fix the issue
log("\n\n\nStarting calibration, please DO NOT TOUCH MOTOR until complete!");
delay(1000);
motor.controller = MotionControlType::angle_openloop;
motor.pole_pairs = 1;
@ -345,6 +346,9 @@ void MotorTask::calibrate() {
float a = 0;
motor.voltage_limit = FOC_VOLTAGE_LIMIT;
motor.move(a);
// #### Determine direction motor rotates relative to angle sensor
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
@ -371,7 +375,12 @@ void MotorTask::calibrate() {
log("");
// TODO: check for no motor movement!
float movement_angle = fabsf(end_sensor - start_sensor);
if (movement_angle < radians(30) || movement_angle > radians(180)) {
snprintf(buf_, sizeof(buf_), "ERROR! Unexpected sensor change: start=%.2f end=%.2f", start_sensor, end_sensor);
log(buf_);
return;
}
log("Sensor measures positive for positive motor rotation:");
if (end_sensor > start_sensor) {
@ -431,6 +440,12 @@ void MotorTask::calibrate() {
snprintf(buf_, sizeof(buf_), "Electrical angle / mechanical angle (i.e. pole pairs) = %.2f", electrical_per_mechanical);
log(buf_);
if (electrical_per_mechanical < 3 || electrical_per_mechanical > 12) {
snprintf(buf_, sizeof(buf_), "ERROR! Unexpected calculated pole pairs: %.2f", electrical_per_mechanical);
log(buf_);
return;
}
int measured_pole_pairs = (int)round(electrical_per_mechanical);
snprintf(buf_, sizeof(buf_), "Pole pairs set to %d", measured_pole_pairs);
log(buf_);