diff --git a/robot_controller/constants.h b/robot_controller/constants.h index 381a8e1..4498c1c 100644 --- a/robot_controller/constants.h +++ b/robot_controller/constants.h @@ -14,11 +14,11 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER); const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0; const float FORWARD_DISTANCE = 10.0; // cm -const float TURN_AMOUNT = 180.0; // degrees +const float TURN_AMOUNT = 175.0; // degrees const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI; const float RETURN_DISTANCE = 100.0; // cm -const float FF_ACCEL = 2.7; // motor acceleration feedforward +const float FF_ACCEL = 2.3; // motor acceleration feedforward const float FF_VEL = 0.7; // motor velocity feedforward const float FF_STAT = 16.4; // motor static friction const float KP = 2.0; // proportional diff --git a/robot_controller/robot_controller.ino b/robot_controller/robot_controller.ino index 28a2a52..1f99668 100644 --- a/robot_controller/robot_controller.ino +++ b/robot_controller/robot_controller.ino @@ -78,36 +78,36 @@ void loop() { float turnsig = (robot_state == TURN ? 1.0 : -1.0); // direction of travel for right motor // position PI controller - float pos_err_left = ((setpoint.position / DEG_CM) + left_home) - left_motor.read_angle(); - float pos_err_right = ((turnsig * setpoint.position / DEG_CM) + right_home) - right_motor.read_angle(); + float pos_err_right = ((setpoint.position / DEG_CM) + right_home) - right_motor.read_angle(); + float pos_err_left = ((turnsig * setpoint.position / DEG_CM) + left_home) - left_motor.read_angle(); // delta should be ~4ms if (time > time_p) { float delta = time - time_p; - right_iaccum += pos_err_right * delta; left_iaccum += pos_err_left * delta; + right_iaccum += pos_err_right * delta; } time_p = time; - float left_effort = pos_err_left * KPP + left_iaccum * KPI; float right_effort = pos_err_right * KPP + right_iaccum * KPI; + float left_effort = pos_err_left * KPP + left_iaccum * KPI; - float left_velocity = setpoint.velocity + left_effort; - float right_velocity = (turnsig * setpoint.velocity) + right_effort; + float right_velocity = setpoint.velocity + right_effort; + float left_velocity = (turnsig * setpoint.velocity) + left_effort; // calculate feedforward from motion profile and position PI data - float feedforward_left = - setpoint.acceleration * CMS_RPM * FF_ACCEL + - left_velocity * CMS_RPM * FF_VEL + - ((left_velocity > 0.0) ? FF_STAT : -FF_STAT); float feedforward_right = setpoint.acceleration * CMS_RPM * FF_ACCEL + right_velocity * CMS_RPM * FF_VEL + ((right_velocity > 0.0) ? FF_STAT : -FF_STAT); + float feedforward_left = + setpoint.acceleration * CMS_RPM * FF_ACCEL + + left_velocity * CMS_RPM * FF_VEL + + ((left_velocity > 0.0) ? FF_STAT : -FF_STAT); // arbitrary feedforward with on-board velocity PID - write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left); - write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right); + write_rpm_ff(&right_motor, right_velocity * CMS_RPM , feedforward_right); + write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left); // send telemetry int32_t rpm = left_motor.read_rpm(); @@ -136,8 +136,8 @@ void loop() { // move on if at setpoint TODO: give up after timeout if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) { // rehome - left_home += (setpoint.position / DEG_CM); - right_home += (turnsig * setpoint.position / DEG_CM); + right_home += (setpoint.position / DEG_CM); + left_home += (turnsig * setpoint.position / DEG_CM); // zero accumulators right_iaccum = 0.0; left_iaccum = 0.0;