add torque limit, change servo parameters

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_controller.h

@@ -17,6 +17,7 @@ class DiffController
 private:
     void updateWheelLoop();
     void updateOdometryLoop();
+    void debugLoop();
     void inputWatchdog();
 
     Wheel *wheelFL;

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. 

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

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<float>(_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<int16_t>(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;

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();