Tuk ZeroFour
borrowed codes
Dependencies: mbed Encoder_Nucleo_16_bits
Diff: main.cpp
- Revision:
- 0:242dc469ac2b
- Child:
- 1:e5dd0af74983
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jun 06 20:53:45 2018 +0000
@@ -0,0 +1,299 @@
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+
+#define PI 3.1415926535898
+
+Nucleo_Encoder_16_bits _Lencoder (TIM4);
+Nucleo_Encoder_16_bits _Rencoder (TIM3);
+PwmOut _Lpwm (PA_9);
+PwmOut _Rpwm (PA_8);
+DigitalOut _LdirA (PC_9);
+DigitalOut _LdirB (PC_8);
+DigitalOut _RdirA (PC_6);
+DigitalOut _RdirB (PC_5);
+
+Serial pc (PA_2, PA_3, 921600); // Create a serial link to PC for communication
+
+DigitalOut led1 (PA_5); // Added Led1 for test purpose
+DigitalOut led2 (PD_2); // Added Led2 for test purpose
+DigitalOut disquette (PA_12); // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)
+
+Ticker _tick;
+
+void controlLoop(void);
+
+/**
+ * Set the Kp value
+ *
+ * @param Kp (float) : value of Kp
+ * @return The value of Kp (float)
+ * @note Can also be accessed using the global variable _Kp
+ */
+float setProportionnalValue (float KpValue);
+
+/**
+ * Set the Ki value
+ *
+ * @param Ki (float) : value of Ki
+ * @return The value of Ki (float)
+ * @note Can also be accessed using the global variable _Ki
+ */
+float setintegralValue (float KiValue);
+
+/**
+ * Set the Kd value
+ *
+ * @param Kd (float) : value of Kd
+ * @return The value of Kd (float)
+ * @note Can also be accessed using the global variable _Kd
+ */
+float setDerivativeValue (float KdValue);
+
+/**
+ * Set the Set point value of the speed for integrated full bridge
+ *
+ * @param left (double) : Set point value for left wheel speed (in mm/s)
+ * @param right (double) : Set point value for right wheel speed (in mm/s)
+ */
+void setSpeed (double left, double right);
+
+/**
+ * Get position of the robot (in mm for X and Y and radian for Theta)
+ *
+ * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup)
+ * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup)
+ * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup
+ * @note the position is updated each time a motion computation take place (ie : each milliseconds)
+ */
+void getPosition (double *x, double *y, double *theta);
+
+/**
+ * Reset position of the robot (in mm for X and Y and radian for Theta)
+ *
+ * @note the positionis equal to 0 on all 3 axis (cannot be undone)
+ */
+void resetPosition (void);
+
+/**
+ * Get speed of the two wheels of the robot
+ *
+ * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
+ * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
+ */
+void getSpeed (double *vG, double *vD);
+
+/**
+ * Global Variable of corrective values
+ * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
+ */
+double _Kp, _Ki, _Kd;
+/**
+ * Global Variable of speed
+ */
+double _SpeedG, _SpeedD;
+/**
+ * Global Variable of measured speed
+ */
+double _measSpeedG, _measSpeedD;
+/**
+ * Global Variable of measured displacement
+ */
+double _measDistG, _measDistD;
+/**
+ * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable)
+ * @note Must be cleared by user
+ */
+int _WheelStuckG, _WheelStuckD;
+/**
+ * Global Variable of wheel PWM value
+ */
+double _PwmValueG, _PwmValueD;
+/**
+ * Global Variable of gravity center of robot position (odometric, error increase with time)
+ */
+double _X, _Y, _THETA;
+
+/**
+ * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
+ * @note Must be cleared by user
+ */
+int RobotIsStuck;
+
+int main ()
+{
+ _tick.attach(&controlLoop, 0.001);
+
+ _Lpwm.period_us(50);
+ _Lpwm = 0;
+
+ _Rpwm.period_us(50);
+ _Rpwm = 0;
+
+ _Kp = 2.0;
+ _Ki = 0.4;
+ _Kd = 1.0;
+
+ pc.printf ("\n\rHelloWorld\n");
+ led1 = 1;
+ led2 = 0;
+ disquette = 0;
+ resetPosition();
+ wait(3);
+
+ setSpeed (80,80);
+ while (1);
+}
+
+float setProportionnalValue (float KpValue)
+{
+ _Kp = KpValue;
+ return _Kp;
+}
+
+float setintegralValue (float KiValue)
+{
+ _Ki = KiValue;
+ return _Ki;
+}
+
+float setDerivativeValue (float KdValue)
+{
+ _Kd = KdValue;
+ return _Kd;
+}
+
+void setSpeed (double left, double right)
+{
+ _SpeedG = left;
+ _SpeedD = right;
+}
+
+void getSpeed (double *vG, double *vD)
+{
+ *vG = _measSpeedG;
+ *vD = _measSpeedD;
+}
+
+void getPosition (double *x, double *y, double *theta)
+{
+ *x = _X;
+ *y = _Y;
+ *theta = _THETA;
+}
+
+void resetPosition (void)
+{
+ _X = 0;
+ _Y = 0;
+ _THETA = 0;
+}
+
+void controlLoop()
+{
+ long PositionG, PositionD;
+ static float integralErrorG = 0, integralErrorD = 0;
+ static float oldErrorG, oldErrorD;
+ double errorG, commandeG;
+ double errorD, commandeD;
+ double meanDist, diffDist, deltaX, deltaY, deltaTheta;
+ static long oldPositionG=0, oldPositionD=0;
+
+ PositionG = _Lencoder.GetCounter();
+ PositionD = -_Rencoder.GetCounter();
+
+ // As we use mm/s for speed unit and we convert all mesure to this unit
+
+ // Converting step to millimeters
+ _measDistG = ((double)PositionG - (double)oldPositionG) / 63.662;
+ _measDistD = ((double)PositionD - (double)oldPositionD) / 63.662;
+
+ // Converting position to speed
+ _measSpeedG = 1000.0 * _measDistG;
+ _measSpeedD = 1000.0 * _measDistD;
+
+ // Computing errors
+ errorG = _SpeedG - _measSpeedG;
+ errorD = _SpeedD - _measSpeedD;
+ integralErrorG += errorG;
+ integralErrorD += errorD;
+
+ // Limiting integral error
+ if (integralErrorG > 10000) {
+ _WheelStuckG = 1;
+ integralErrorG = 10000;
+ } else {
+ if (integralErrorG < -10000) {
+ _WheelStuckG = 1;
+ integralErrorG = -10000;
+ } else {
+ _WheelStuckG = 0;
+ }
+ }
+
+ if (integralErrorD > 10000) {
+ _WheelStuckD = 1;
+ integralErrorD = 10000;
+ } else {
+ if (integralErrorD < -10000) {
+ _WheelStuckD = 1;
+ integralErrorD = -10000;
+ } else {
+ _WheelStuckD = 0;
+ }
+ }
+
+ // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting)
+ commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG);
+ commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD);
+
+ // Convert to PWM
+ _PwmValueG = commandeG / 1300.0;
+ if (_PwmValueG > 1) _PwmValueG = 1;
+ if (_PwmValueG < -1) _PwmValueG = -1;
+
+ if (_PwmValueG < 0) {
+ _Lpwm = -_PwmValueG;
+ _LdirA = 0;
+ _LdirB = 1;
+ } else {
+ _Lpwm = _PwmValueG;
+ _LdirA = 1;
+ _LdirB = 0;
+ }
+
+ _PwmValueD = commandeD/1300.0;
+ if (_PwmValueD > 1) _PwmValueD = 1;
+ if (_PwmValueD < -1) _PwmValueD = -1;
+
+ if (_PwmValueD < 0) {
+ _Rpwm = -_PwmValueD;
+ _RdirA = 1;
+ _RdirB = 0;
+ } else {
+ _Rpwm = _PwmValueD;
+ _RdirA = 0;
+ _RdirB = 1;
+ }
+
+ //odometry (segments approx.)
+ meanDist = (_measDistG + _measDistD) / 2.0;
+ diffDist = _measDistD - _measDistG;
+
+ deltaX = meanDist * cos (_THETA);
+ deltaY = meanDist * sin (_THETA);
+ deltaTheta = diffDist / 230;
+
+ _X += deltaX;
+ _Y += deltaY;
+ _THETA += deltaTheta;
+ if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
+ if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
+
+ oldPositionG = PositionG;
+ oldPositionD = PositionD;
+ oldErrorG = errorG;
+ oldErrorD = errorD;
+
+ pc.printf ("\rG = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedG, _measDistG, errorG, commandeG, _PwmValueG);
+ pc.printf ("\rD = %5.1lf - %5.1lf = %5.1lf -> %5.1lf -> %4.2lf\n", _SpeedD, _measDistD, errorD, commandeD, _PwmValueD);
+}