diff --git a/diff_controller.cpp b/diff_controller.cpp index 2742746..5cb86a5 100644 --- a/diff_controller.cpp +++ b/diff_controller.cpp @@ -3,16 +3,17 @@ #include "diff_controller.h" #include "params.h" +#include "utils.h" DiffController::DiffController(uint32_t input_timeout) : _last_wheel_L_ang_pos(0), _last_wheel_R_ang_pos(0), _input_timeout(input_timeout) { - wheelFL = new Wheel(hMotC, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT); - wheelRL = new Wheel(hMotD, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT); - wheelFR = new Wheel(hMotA, 0, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT); - wheelRR = new Wheel(hMotB, 0, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT); + wheelFL = new Wheel(hMotC, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); + wheelRL = new Wheel(hMotD, 1, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); + wheelFR = new Wheel(hMotA, 0, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); + wheelRR = new Wheel(hMotB, 0, WHEEL_MAX_SPEED, PID_P, PID_I, PID_D, POWER_LIMIT, TORQUE_LIMIT); } void DiffController::start() @@ -23,16 +24,9 @@ void DiffController::start() _last_update = sys.getRefTime(); sys.taskCreate(std::bind(&DiffController::inputWatchdog, this)); } -} - -float clamp(float value, float limit) -{ - if (value > limit) - return limit; - else if (value < -limit) - return -limit; - else - return value; +#ifdef DEBUG + sys.taskCreate(std::bind(&DiffController::debugLoop, this)); +#endif } void DiffController::setSpeed(float linear, float angular) @@ -114,6 +108,20 @@ void DiffController::updateOdometryLoop() } } +void DiffController::debugLoop() +{ + uint32_t t = sys.getRefTime(); + uint32_t dt = 100; + + while(true) + { + Serial.printf("Motor powers: %d %d %d %d\r\n", + wheelFL->getPower(), wheelRL->getPower(), + wheelFR->getPower(), wheelRR->getPower()); + sys.delaySync(t, dt); + } +} + void DiffController::inputWatchdog() { while (true) diff --git a/diff_controller.h b/diff_controller.h index 6979b57..2285518 100644 --- a/diff_controller.h +++ b/diff_controller.h @@ -17,6 +17,7 @@ class DiffController private: void updateWheelLoop(); void updateOdometryLoop(); + void debugLoop(); void inputWatchdog(); Wheel *wheelFL; diff --git a/params.h b/params.h index b13457e..cdfd1b1 100644 --- a/params.h +++ b/params.h @@ -10,39 +10,39 @@ const uint16_t SERVO_PERIOD = 20000; enum voltage {VOLTAGE_5V, VOLTAGE_6V, VOLTAGE_7V4, VOLTAGE_8V6}; const voltage SERVO_VOLTAGE = VOLTAGE_7V4; -const int16_t SERVO_1_ANGLE_MIN = 0; +const int16_t SERVO_1_ANGLE_MIN = -90; const int16_t SERVO_1_ANGLE_MAX = 90; const uint16_t SERVO_1_WIDTH_MIN = 1000; -const uint16_t SERVO_1_WIDTH_MAX = 1300; +const uint16_t SERVO_1_WIDTH_MAX = 2000; -const int16_t SERVO_2_ANGLE_MIN = 0; +const int16_t SERVO_2_ANGLE_MIN = -90; const int16_t SERVO_2_ANGLE_MAX = 90; const uint16_t SERVO_2_WIDTH_MIN = 1000; -const uint16_t SERVO_2_WIDTH_MAX = 1300; +const uint16_t SERVO_2_WIDTH_MAX = 2000; -const int16_t SERVO_3_ANGLE_MIN = 0; +const int16_t SERVO_3_ANGLE_MIN = -90; const int16_t SERVO_3_ANGLE_MAX = 90; const uint16_t SERVO_3_WIDTH_MIN = 1000; -const uint16_t SERVO_3_WIDTH_MAX = 1300; +const uint16_t SERVO_3_WIDTH_MAX = 2000; -const int16_t SERVO_4_ANGLE_MIN = 0; +const int16_t SERVO_4_ANGLE_MIN = -90; const int16_t SERVO_4_ANGLE_MAX = 90; const uint16_t SERVO_4_WIDTH_MIN = 1000; -const uint16_t SERVO_4_WIDTH_MAX = 1300; +const uint16_t SERVO_4_WIDTH_MAX = 2000; -const int16_t SERVO_5_ANGLE_MIN = 0; +const int16_t SERVO_5_ANGLE_MIN = -90; const int16_t SERVO_5_ANGLE_MAX = 90; const uint16_t SERVO_5_WIDTH_MIN = 1000; -const uint16_t SERVO_5_WIDTH_MAX = 1300; +const uint16_t SERVO_5_WIDTH_MAX = 2000; -const int16_t SERVO_6_ANGLE_MIN = 0; +const int16_t SERVO_6_ANGLE_MIN = -90; const int16_t SERVO_6_ANGLE_MAX = 90; const uint16_t SERVO_6_WIDTH_MIN = 1000; -const uint16_t SERVO_6_WIDTH_MAX = 1300; +const uint16_t SERVO_6_WIDTH_MAX = 2000; // Wheels const float ENCODER_RESOLUTION = 8256; //in ticks per rotation -const float WHEEL_RADIUS = 0.06; //in meters +const float WHEEL_RADIUS = 0.0625; //in meters const float WHEEL_MAX_SPEED = 6500; //in ticks per second const float ROBOT_WIDTH = 0.33; //in meters @@ -54,7 +54,12 @@ const float PID_D = 0.0; // Value between 0 and 1000 describing power limit // e.g. 1000 means no limit, 800 corresponds to 80% // Take into account that this value only limits the Voltage (PWM) and not the current. -const int32_t POWER_LIMIT = 1000; +const uint16_t POWER_LIMIT = 1000; + +// Value between 0 and 1000 describing torque limit +// This value limits power depending on actual speed +// e.g. 800 means that power is limited to 80% at 0% speed, 90% at 10% speed etc. +const uint16_t TORQUE_LIMIT = 800; // Input timeout in ms. // The controller will stop the motors if it doesn't receive a command for a specified time. diff --git a/utils.h b/utils.h new file mode 100644 index 0000000..c864167 --- /dev/null +++ b/utils.h @@ -0,0 +1,14 @@ +#ifndef _UTILS_H_ +#define _UTILS_H_ + +inline float clamp(float value, float limit) +{ + if (value > limit) + return limit; + else if (value < -limit) + return -limit; + else + return value; +} + +#endif \ No newline at end of file diff --git a/wheel.cpp b/wheel.cpp index 75f8893..969da65 100644 --- a/wheel.cpp +++ b/wheel.cpp @@ -1,10 +1,14 @@ #include "wheel.h" +#include "utils.h" Wheel::Wheel(hMotor& motor, bool polarity, float max_speed, - float Kp, float Ki, float Kd, int32_t power_limit = 1000) + float Kp, float Ki, float Kd, + uint16_t power_limit = 1000, uint16_t torque_limit = 1000) : mot(motor), pol(polarity), _max_speed(max_speed), + _power_limit(power_limit), + _torque_limit(torque_limit), turnedOn(true), dNow(0.0), vNow(0.0), @@ -34,13 +38,18 @@ void Wheel::update(uint32_t dt) float vErr = vNow - vTarget; float pidOut = vReg.update(vErr, dt); + float est_power = (std::abs(vNow) / _max_speed) * 1000.0; + float max_power = std::min(est_power + static_cast(_torque_limit), (float)1000.0); + if (turnedOn == true) { if (vNow == 0.0 && vTarget == 0.0){ vReg.reset(); - mot.setPower(0); + _power = 0; } else { - mot.setPower(pidOut); + float power_limited = clamp(pidOut, max_power); + _power = static_cast(power_limited); } + mot.setPower(_power); } } @@ -62,6 +71,11 @@ float Wheel::getSpeed() return vNow; } +int16_t Wheel::getPower() +{ + return _power; +} + int32_t Wheel::getDistance() { return dNow; diff --git a/wheel.h b/wheel.h index 20bb8f8..e8fb153 100644 --- a/wheel.h +++ b/wheel.h @@ -12,6 +12,9 @@ class Wheel { hPIDRegulator vReg; bool pol; + int16_t _power; + uint16_t _power_limit; + uint16_t _torque_limit; float _max_speed; bool turnedOn; @@ -22,12 +25,15 @@ class Wheel { float vRange = 1000.0; public: - Wheel(hMotor &motor, bool polarity, float max_speed, float Kp, float Ki, float Kd, int32_t power_limit); + Wheel(hMotor &motor, bool polarity, float max_speed, + float Kp, float Ki, float Kd, + uint16_t power_limit, uint16_t torque_limit); void update(uint32_t dt); void setSpeed(float speed); float getSpeed(); + int16_t getPower(); int32_t getDistance(); void resetDistance();