altb_pmic / Mbed 2 deprecated Test_Realbot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@5:4c03a1a0ad98, 2020-03-10 (annotated)

Committer:
pmic
Date:
Tue Mar 10 09:14:00 2020 +0000
Revision:
5:4c03a1a0ad98
Parent:
3:e6d345973797 
Latest commit.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 0:937360eb9f8c 1 #include "mbed.h"
pmic 0:937360eb9f8c 2 #include "PT1_Controller.h"
pmic 0:937360eb9f8c 3 #include "Target_Planer.h"
pmic 3:e6d345973797 4 #include "Motion.h"
pmic 0:937360eb9f8c 5
pmic 0:937360eb9f8c 6 using namespace std;
pmic 0:937360eb9f8c 7
pmic 0:937360eb9f8c 8 #define pi 3.141592653589793
pmic 0:937360eb9f8c 9
pmic 0:937360eb9f8c 10 Serial pc(SERIAL_TX, SERIAL_RX);
pmic 0:937360eb9f8c 11
pmic 0:937360eb9f8c 12 double kpv = 0.6412; // proportional controller for distance error
pmic 0:937360eb9f8c 13 double kpw = 0.6801; // proportional controller for angle error
pmic 0:937360eb9f8c 14 double Ts = 1/30.0; // samplingrate 30 Hz
pmic 0:937360eb9f8c 15 double Tf = 1/(2.0*pi*4.0); // fg = 4 Hz
pmic 0:937360eb9f8c 16 PT1_Controller distance_cntr(kpv, Ts, Tf);
pmic 0:937360eb9f8c 17 PT1_Controller angle_cntr(kpw, Ts, Tf);
pmic 0:937360eb9f8c 18
pmic 0:937360eb9f8c 19 // (vRx (m/s), wR (rad/s)) -> (vRxRB (unit unknown), wRRB (unit unknown))
pmic 0:937360eb9f8c 20 // Cu2uRB = 2.8070 0
pmic 0:937360eb9f8c 21 // 0 0.9263
pmic 0:937360eb9f8c 22 double kv2RB = 2.8070; // maps desired forward velocity from m/s 2 RB unit
pmic 0:937360eb9f8c 23 double kw2RB = 0.9263; // maps desired turn speed from rad/s 2 RB unit
pmic 0:937360eb9f8c 24
pmic 0:937360eb9f8c 25 Timer timer; // timer for time measurement
pmic 0:937360eb9f8c 26 float dt = 0.0f;
pmic 0:937360eb9f8c 27
pmic 0:937360eb9f8c 28 uint32_t i;
pmic 0:937360eb9f8c 29
pmic 0:937360eb9f8c 30 // user defined functions
pmic 0:937360eb9f8c 31 double quat2psi(double qw, double qx, double qy, double qz);
pmic 5:4c03a1a0ad98 32 void quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi);
pmic 0:937360eb9f8c 33 double getShortRotation(double ang);
pmic 0:937360eb9f8c 34
pmic 2:27f16e37a176 35 const int N = 40;
pmic 2:27f16e37a176 36 double Pathx[N] = {7.3000, 7.2500, 7.2000, 7.1500, 7.1000, 7.0500, 7.0000, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500, 6.2000, 6.1500, 6.1000, 6.0500, 6.0000, 5.9500, 5.9000, 5.8500, 5.8000, 5.7500, 5.7000, 5.6500, 5.6000, 5.5500, 5.5000, 5.4500, 5.4000, 5.3500};
pmic 2:27f16e37a176 37 double Pathy[N] = {6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500};
pmic 2:27f16e37a176 38 double R = 0.116959064327485;
pmic 0:937360eb9f8c 39 Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
pmic 2:27f16e37a176 40 double RobotPosx = 4.35;
pmic 2:27f16e37a176 41 double RobotPosy = 6.19;
pmic 1:e79de7ffd211 42 double TargetPosx = -999.0;
pmic 1:e79de7ffd211 43 double TargetPosy = -999.0;
pmic 1:e79de7ffd211 44 double TargetAng = -999.0;
pmic 0:937360eb9f8c 45
pmic 3:e6d345973797 46 double pos0 = 179.0*pi/180.0;
pmic 3:e6d345973797 47 float vel0 = 0.0f*(float)pi/180.0f;
pmic 3:e6d345973797 48 float velMax = 90.0f*(float)pi/180.0f;
pmic 3:e6d345973797 49 float accMax = 60.0f*(float)pi/180.0f;
pmic 3:e6d345973797 50 Motion motion_planer(pos0, vel0, velMax, accMax);
pmic 3:e6d345973797 51 double targetPos = 0*pi/180.0;
pmic 3:e6d345973797 52 float targetVel = 0*(float)pi/180.0f;
pmic 3:e6d345973797 53
pmic 0:937360eb9f8c 54 int main()
pmic 0:937360eb9f8c 55 {
pmic 0:937360eb9f8c 56 pc.baud(2000000);
pmic 0:937360eb9f8c 57
pmic 0:937360eb9f8c 58 timer.start();
pmic 0:937360eb9f8c 59
pmic 0:937360eb9f8c 60 i = 0;
pmic 0:937360eb9f8c 61
pmic 0:937360eb9f8c 62 // reset internal filter storage to zero
pmic 0:937360eb9f8c 63 distance_cntr.reset();
pmic 0:937360eb9f8c 64 angle_cntr.reset();
pmic 0:937360eb9f8c 65
pmic 0:937360eb9f8c 66 while(1) {
pmic 0:937360eb9f8c 67
pmic 0:937360eb9f8c 68 dt = timer.read();
pmic 0:937360eb9f8c 69 timer.reset();
pmic 2:27f16e37a176 70
pmic 2:27f16e37a176 71 target_planer.initialize(RobotPosx, RobotPosy);
pmic 0:937360eb9f8c 72
pmic 0:937360eb9f8c 73 /*
pmic 0:937360eb9f8c 74 double phiR = quat2psi(double qw, double qx, double qy, double qz); // transform actual robot orientation (quaternioni) into turning angle
pmic 0:937360eb9f8c 75 // it is assumed that target_ang and phiR both lie between -pi and pi
pmic 0:937360eb9f8c 76 double e_ang = getShortRotation(target_angle - phiR);
pmic 0:937360eb9f8c 77 // update angle controller
pmic 0:937360eb9f8c 78 */
pmic 5:4c03a1a0ad98 79 double qw = 0.984005578673161;
pmic 5:4c03a1a0ad98 80 double qx = 0.009063618716137;
pmic 5:4c03a1a0ad98 81 double qy = 0.177899336159591;
pmic 5:4c03a1a0ad98 82 double qz = 0.001629344754243;
pmic 5:4c03a1a0ad98 83 double phi, theta, psi;
pmic 5:4c03a1a0ad98 84 quat2rpy(qw, qx, qy, qz, &phi, &theta, &psi);
pmic 5:4c03a1a0ad98 85 if(i++ == 0) {
pmic 5:4c03a1a0ad98 86 pc.printf("%.15f; %.15f; %.15f; \r\n", phi, theta, psi);
pmic 5:4c03a1a0ad98 87 }
pmic 0:937360eb9f8c 88
pmic 5:4c03a1a0ad98 89
pmic 5:4c03a1a0ad98 90 /*
pmic 3:e6d345973797 91 if(i < 200) {
pmic 0:937360eb9f8c 92 // pc.printf("%i; %.12f; %.12f; %0.12f; %.6f;\r\n", i, quat2psi(qw, qx, qy, qz), qx*qy + qw*qz, 0.5 - (qy*qy + qz*qz), dt);
pmic 0:937360eb9f8c 93 // pc.printf("%i; %i; %.6f;\r\n", i, target_planer.returnPathLength(), dt);
pmic 1:e79de7ffd211 94 // pc.printf("%i; %.12f; %.12f; %i; %i; %.6f;\r\n", i, target_planer.returnPathxAtIndex(i), target_planer.returnPathyAtIndex(i), target_planer.returnPathLength(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
pmic 1:e79de7ffd211 95 // pc.printf("%i; %.12f; %.12f;;\r\n", i, TargetPosx, TargetPosy);
pmic 1:e79de7ffd211 96 // target_planer.update(RobotPosx, RobotPosx);
pmic 1:e79de7ffd211 97 // target_planer.readTargetPos(&TargetPosx, &TargetPosy);
pmic 3:e6d345973797 98 // pc.printf("%i; %.12f; %.12f; %.12f; %i; %i; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, target_planer.return_i_min_(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
pmic 3:e6d345973797 99 // target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
pmic 3:e6d345973797 100
pmic 3:e6d345973797 101 pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f;\r\n", i, motion_planer.getTimeToPosition(targetPos), motion_planer.getDistanceToStop(), motion_planer.getPosition(), motion_planer.getVelocity(), motion_planer.getAcceleration(), dt);
pmic 3:e6d345973797 102 motion_planer.incrementToPosition(targetPos, Ts);
pmic 0:937360eb9f8c 103 }
pmic 0:937360eb9f8c 104
pmic 0:937360eb9f8c 105 i++;
pmic 0:937360eb9f8c 106 wait_us(33333);
pmic 5:4c03a1a0ad98 107 */
pmic 0:937360eb9f8c 108 }
pmic 0:937360eb9f8c 109 }
pmic 0:937360eb9f8c 110
pmic 0:937360eb9f8c 111 double quat2psi(double qw, double qx, double qy, double qz)
pmic 0:937360eb9f8c 112 {
pmic 0:937360eb9f8c 113 double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);
pmic 0:937360eb9f8c 114 return atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
pmic 0:937360eb9f8c 115 }
pmic 0:937360eb9f8c 116
pmic 5:4c03a1a0ad98 117 void quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi)
pmic 5:4c03a1a0ad98 118 {
pmic 5:4c03a1a0ad98 119 double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);
pmic 5:4c03a1a0ad98 120
pmic 5:4c03a1a0ad98 121 *phi = atan2(s*(qy*qz + qw*qx), 0.5 - s*(qx*qx + qy*qy));
pmic 5:4c03a1a0ad98 122
pmic 5:4c03a1a0ad98 123 *theta = -2.0*s*(qx*qz - qw*qy);
pmic 5:4c03a1a0ad98 124 if(*theta > 1.0) *theta = 1.0;
pmic 5:4c03a1a0ad98 125 if(*theta < -1.0) *theta = -1.0;
pmic 5:4c03a1a0ad98 126 *theta = asin(*theta);
pmic 5:4c03a1a0ad98 127
pmic 5:4c03a1a0ad98 128 *psi = atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
pmic 5:4c03a1a0ad98 129
pmic 5:4c03a1a0ad98 130 return;
pmic 5:4c03a1a0ad98 131 }
pmic 5:4c03a1a0ad98 132
pmic 0:937360eb9f8c 133 double getShortRotation(double ang)
pmic 0:937360eb9f8c 134 {
pmic 0:937360eb9f8c 135 // we maybe need to introduce a threshold in case the angle is noisy and around +/-pi (180° rotation)
pmic 0:937360eb9f8c 136 return atan2(sin(ang), cos(ang));
pmic 0:937360eb9f8c 137 }
pmic 0:937360eb9f8c 138