Compare commits
No commits in common. "submission" and "master" have entirely different histories.
submission
...
master
4 changed files with 64 additions and 96 deletions
Binary file not shown.
|
|
@ -1,9 +1,11 @@
|
|||
#pragma once
|
||||
|
||||
const float ACCEL_LIMIT = 6.0; // cm/s/s
|
||||
const float VEL_LIMIT = 12.0; // cm/s
|
||||
const float WHEEL_DIAMETER = 6.61; // cm
|
||||
const float WHEEL_TO_WHEEL = 8.6; // cm
|
||||
const float ACCEL_LIMIT = 5.0; // cm/s/s
|
||||
const float VEL_LIMIT = 16.0; // cm/s
|
||||
const float BACKLASH_RIGHT = 0.0; // degrees
|
||||
const float BACKLASH_LEFT = 0.0; // degrees
|
||||
const float WHEEL_DIAMETER = 10.25; // cm
|
||||
const float WHEEL_TO_WHEEL = 9.0; // cm
|
||||
|
||||
// centimeters per second to rpm
|
||||
const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
|
||||
|
|
@ -11,22 +13,16 @@ const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);
|
|||
// degrees to centimeters
|
||||
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;
|
||||
|
||||
const float FORWARD_DISTANCE = 100.0; // cm
|
||||
const float TURN_AMOUNT = 190.0; // degrees
|
||||
const float FORWARD_DISTANCE = 10.0; // cm
|
||||
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 KP = 1.50; // proportional
|
||||
const float KI = 0.12; // integral
|
||||
const float KD = 0.00; // derivative
|
||||
const float Kv = 1.85; // onboard velocity feedforward
|
||||
|
||||
const float TURN_KP = 1.50; // proportional
|
||||
const float TURN_KI = 0.10; // integral
|
||||
const float TURN_KD = 0.00; // derivative
|
||||
const float TURN_Kv = 2.6; // onboard velocity feedforward
|
||||
|
||||
const float KPP = 0.14; // position proportional
|
||||
const float KPI = 0.12; // position integral
|
||||
const float KRP = 0.0; // rotation proportional
|
||||
const float V_MIN = 5.5; // velocity minimum
|
||||
const float V_KMIN = 0.7; // velocity minimum coefficient
|
||||
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
|
||||
const float KI = 0.5; // integral
|
||||
const float KD = 0.05; // derivative
|
||||
const float KPP = 0.25; // position proportional
|
||||
const float KPI = 0.05; // position integral
|
||||
|
|
|
|||
|
|
@ -16,6 +16,8 @@ enum ROBOT_STATE {
|
|||
COMPLETE,
|
||||
};
|
||||
|
||||
float CURRENT_POSITION = 0.0;
|
||||
float CURRENT_ROTATION = 0.0;
|
||||
struct Setpoint setpoint;
|
||||
// start of current state
|
||||
float phase_start;
|
||||
|
|
@ -31,6 +33,15 @@ float time_p = 0.0;
|
|||
float left_home = 0.0;
|
||||
float right_home = 0.0;
|
||||
|
||||
void write_rpm_ff(SmartMotor* motor, int32_t rpm, float ff) {
|
||||
if (rpm == 0) {rpm = 1;};
|
||||
float kV = ff / (float) rpm; // calculate velocity feedforward that causes desired absolute feedforward
|
||||
motor->tune_vel_pid(kV, KP,KI,KD);
|
||||
delay(1);
|
||||
motor->write_rpm(rpm);
|
||||
delay(1);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
// INIT SERIAL
|
||||
Serial.begin(115200);
|
||||
|
|
@ -38,25 +49,10 @@ void setup() {
|
|||
|
||||
Wire.begin(); // INIT ARDUINO UNO AS I2C CONTROLLER
|
||||
|
||||
left_motor.tune_pos_pid(0.4, 0.024, 0.008);
|
||||
left_motor.write_rpm(0.0);
|
||||
delay(1);
|
||||
right_motor.tune_pos_pid(0.4, 0.024, 0.008);
|
||||
delay(1);
|
||||
left_motor.tune_vel_pid(Kv, KP,KI,KD);
|
||||
delay(1);
|
||||
right_motor.tune_vel_pid(Kv, KP,KI,KD);
|
||||
delay(1);
|
||||
|
||||
delay(10);
|
||||
// runs to mend bad state
|
||||
left_motor.home();
|
||||
delay(1);
|
||||
left_motor.write_angle(0.0);
|
||||
delay(1);
|
||||
right_motor.home();
|
||||
delay(1);
|
||||
right_motor.write_angle(0.0);
|
||||
delay(3000);
|
||||
right_motor.write_rpm(0.0);
|
||||
delay(999);
|
||||
|
||||
phase_start = (float)millis() / 1000.0;
|
||||
robot_state = FORWARD;
|
||||
|
|
@ -85,9 +81,6 @@ void loop() {
|
|||
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();
|
||||
|
||||
// total wheel offness in degrees
|
||||
float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
|
||||
|
||||
// delta should be ~4ms
|
||||
if (time > time_p) {
|
||||
float delta = time - time_p;
|
||||
|
|
@ -102,52 +95,46 @@ void loop() {
|
|||
float right_velocity = setpoint.velocity + right_effort;
|
||||
float left_velocity = (turnsig * setpoint.velocity) + left_effort;
|
||||
|
||||
if (setpoint.complete && robot_state == TURN) {
|
||||
right_motor.write_angle((setpoint.position / DEG_CM) + right_home);
|
||||
left_motor.write_angle((turnsig * setpoint.position / DEG_CM) + left_home);
|
||||
} else {
|
||||
right_motor.write_rpm(right_velocity * CMS_RPM);
|
||||
delay(1);
|
||||
left_motor.write_rpm(left_velocity * CMS_RPM);
|
||||
delay(1);
|
||||
}
|
||||
// calculate feedforward from motion profile and position PI data
|
||||
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(&right_motor, right_velocity * CMS_RPM , feedforward_right);
|
||||
write_rpm_ff(&left_motor, left_velocity * CMS_RPM , feedforward_left);
|
||||
|
||||
// send telemetry
|
||||
int32_t rpm = right_motor.read_rpm();
|
||||
int32_t pos = right_motor.read_angle();
|
||||
int32_t rpm = left_motor.read_rpm();
|
||||
int32_t pos = left_motor.read_angle();
|
||||
int32_t error = setpoint.velocity * CMS_RPM - rpm;
|
||||
Serial.print("el:");
|
||||
Serial.print(pos_err_left);
|
||||
Serial.print(",er:");
|
||||
Serial.print(pos_err_right);
|
||||
Serial.print(",il:");
|
||||
Serial.print(left_iaccum);
|
||||
Serial.print(",ir:");
|
||||
Serial.print(right_iaccum);
|
||||
Serial.print(",effr:");
|
||||
Serial.print(right_effort);
|
||||
//Serial.print("ff:");
|
||||
//Serial.print(left_effort);
|
||||
//Serial.print(",time:");
|
||||
//Serial.print(time);
|
||||
//Serial.print(",distgoal:");
|
||||
//Serial.print(setpoint.position);
|
||||
//Serial.print(",dist:");
|
||||
//Serial.print(pos * DEG_CM);
|
||||
Serial.print(",vel:");
|
||||
Serial.print("ff:");
|
||||
Serial.print(left_effort);
|
||||
Serial.print(",time:");
|
||||
Serial.print(time);
|
||||
Serial.print(",distgoal:");
|
||||
Serial.print(setpoint.position);
|
||||
Serial.print(",dist:");
|
||||
Serial.print(pos * DEG_CM);
|
||||
Serial.print(",cms/s:");
|
||||
Serial.print(rpm / CMS_RPM);
|
||||
Serial.print(",setvel:");
|
||||
Serial.print(setpoint.velocity);
|
||||
Serial.print(",rvel:");
|
||||
Serial.print(right_velocity);
|
||||
//Serial.print(",setacc:");
|
||||
//Serial.print(setpoint.acceleration);
|
||||
//Serial.print(",Err:");
|
||||
//Serial.print(error / CMS_RPM);
|
||||
Serial.print(",setacc:");
|
||||
Serial.print(setpoint.acceleration);
|
||||
Serial.print(",Err:");
|
||||
Serial.print(error / CMS_RPM);
|
||||
Serial.println("");
|
||||
|
||||
// move on if at setpoint or timeout
|
||||
if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 5.0)) {
|
||||
// total wheel offness in degrees
|
||||
float total_pos_error = abs(pos_err_left) + abs(pos_err_right);
|
||||
// move on if at setpoint TODO: give up after timeout
|
||||
if (setpoint.complete && (total_pos_error < 3.0 || time - end_time > 3.0)) {
|
||||
// rehome
|
||||
right_home += (setpoint.position / DEG_CM);
|
||||
left_home += (turnsig * setpoint.position / DEG_CM);
|
||||
|
|
@ -156,21 +143,10 @@ void loop() {
|
|||
|
||||
switch (robot_state) {
|
||||
case FORWARD:
|
||||
// turn tuning (more aggressive)
|
||||
left_motor.tune_vel_pid(TURN_Kv, TURN_KP,TURN_KI,TURN_KD);
|
||||
delay(1);
|
||||
right_motor.tune_vel_pid(TURN_Kv,TURN_KP,TURN_KI,TURN_KD);
|
||||
|
||||
robot_state = TURN;
|
||||
right_iaccum = left_iaccum = 60.0;
|
||||
phase_start = (float)millis() / 1000.0;
|
||||
break;
|
||||
case TURN:
|
||||
// straight line tuning (less aggressive)
|
||||
left_motor.tune_vel_pid(Kv, KP,KI,KD);
|
||||
delay(1);
|
||||
right_motor.tune_vel_pid(Kv, KP,KI,KD);
|
||||
|
||||
robot_state = RETURN;
|
||||
phase_start = (float)millis() / 1000.0;
|
||||
|
||||
|
|
|
|||
|
|
@ -11,10 +11,6 @@ struct Setpoint trapezoidal_planner(struct Trapezoidal* trapezoidal, float time)
|
|||
float time_accelerating = max_vel / max_acc;
|
||||
float distance_while_accelerating = 0.5 * max_acc * time_accelerating * time_accelerating;
|
||||
|
||||
if (time < 0.0) {
|
||||
return setpoint;
|
||||
}
|
||||
|
||||
if (2.0 * distance_while_accelerating > dist) {
|
||||
// triangular
|
||||
float peak_velocity_time = sqrt(dist/max_acc);
|
||||
|
|
|
|||
Loading…
Reference in a new issue