Andrew SaLoutos / Mbed OS Bezier_Traj_Follower_Example

Example Mbed code for MIT 2.74 Lab 4

Dependencies:   ExperimentServer QEI_pmw MotorShield

Dependents:   project

BezierCurve.cpp@17:1bb5aa45826e, 2020-09-25 (annotated)

Committer:
saloutos
Date:
Fri Sep 25 04:39:17 2020 +0000
Revision:
17:1bb5aa45826e
Parent:
16:f9ea2b2d410f 
Fixed bugs and added inverse kinematics for joint space

Who changed what in which revision?

UserRevisionLine numberNew contents of line
saloutos 16:f9ea2b2d410f 1 #include "BezierCurve.h"
saloutos 16:f9ea2b2d410f 2 #include "math.h"
saloutos 16:f9ea2b2d410f 3 #include "mbed.h"
saloutos 16:f9ea2b2d410f 4
saloutos 16:f9ea2b2d410f 5 extern Serial pc;
saloutos 16:f9ea2b2d410f 6 int factorial(int k) {
saloutos 16:f9ea2b2d410f 7 int f = 1;
saloutos 16:f9ea2b2d410f 8 for (int j = 2 ; j<= k ; j++)
saloutos 16:f9ea2b2d410f 9 f*=j;
saloutos 16:f9ea2b2d410f 10 return f;
saloutos 16:f9ea2b2d410f 11 }
saloutos 16:f9ea2b2d410f 12
saloutos 16:f9ea2b2d410f 13 BezierCurve::BezierCurve(int dim, int order) :_dim(dim), _order(order) {
saloutos 16:f9ea2b2d410f 14 _pts = new float*[_order+1];
saloutos 16:f9ea2b2d410f 15 _nck = new int[_order+1];
saloutos 16:f9ea2b2d410f 16 _nck_deriv = new int[_order];
saloutos 16:f9ea2b2d410f 17 int facn = factorial(_order);
saloutos 16:f9ea2b2d410f 18 int facn2= factorial(_order-1);
saloutos 16:f9ea2b2d410f 19 for(int i = 0 ; i <= _order ; i++) {
saloutos 16:f9ea2b2d410f 20 _pts[i] = new float[_dim];
saloutos 16:f9ea2b2d410f 21 _nck[i] = facn / factorial(i) / factorial(_order-i);
saloutos 16:f9ea2b2d410f 22 if (i< _order) {
saloutos 16:f9ea2b2d410f 23 _nck_deriv[i] = facn2 / factorial(i) / factorial(_order-1-i);
saloutos 16:f9ea2b2d410f 24 }
saloutos 16:f9ea2b2d410f 25 }
saloutos 16:f9ea2b2d410f 26 }
saloutos 16:f9ea2b2d410f 27
saloutos 16:f9ea2b2d410f 28 BezierCurve::~BezierCurve() {
saloutos 16:f9ea2b2d410f 29 for(int i = 0 ; i < _dim ; i++) {
saloutos 16:f9ea2b2d410f 30 delete _pts[i];
saloutos 16:f9ea2b2d410f 31 }
saloutos 16:f9ea2b2d410f 32 delete _pts;
saloutos 16:f9ea2b2d410f 33 }
saloutos 16:f9ea2b2d410f 34
saloutos 16:f9ea2b2d410f 35 void BezierCurve::setPoints(float pts[] ) {
saloutos 16:f9ea2b2d410f 36 pc.printf("Setting Points\n");
saloutos 16:f9ea2b2d410f 37 float * p = pts;
saloutos 16:f9ea2b2d410f 38 for(int i = 0 ; i<=_order ; i++) {
saloutos 17:1bb5aa45826e 39 pc.printf("\n\r\tPt. %d:",i);
saloutos 16:f9ea2b2d410f 40 for( int j = 0 ; j < _dim ; j++) {
saloutos 16:f9ea2b2d410f 41 _pts[i][j] = *p;
saloutos 16:f9ea2b2d410f 42 p++;
saloutos 17:1bb5aa45826e 43 pc.printf("\t\t%f",_pts[i][j]);
saloutos 16:f9ea2b2d410f 44 }
saloutos 16:f9ea2b2d410f 45 }
saloutos 16:f9ea2b2d410f 46 }
saloutos 16:f9ea2b2d410f 47
saloutos 16:f9ea2b2d410f 48 void BezierCurve::evaluate(float time, float point[]) {
saloutos 16:f9ea2b2d410f 49 //float *_point = new float[_dim];
saloutos 16:f9ea2b2d410f 50
saloutos 16:f9ea2b2d410f 51 for(int i=0; i< _dim ; i++) {
saloutos 16:f9ea2b2d410f 52 point[i] = 0;
saloutos 16:f9ea2b2d410f 53 }
saloutos 16:f9ea2b2d410f 54 for(int i=0; i<=_order ; i++) {
saloutos 16:f9ea2b2d410f 55 float mi = pow(time,i)*pow(1-time,_order-i) * _nck[i];
saloutos 16:f9ea2b2d410f 56 for(int j=0 ; j < _dim ; j++) {
saloutos 16:f9ea2b2d410f 57 point[j] += _pts[i][j] * mi;
saloutos 16:f9ea2b2d410f 58 }
saloutos 16:f9ea2b2d410f 59 }
saloutos 16:f9ea2b2d410f 60 //for(int i=0; i< _dim ; i++) {
saloutos 16:f9ea2b2d410f 61 // point[i] = _point[i];
saloutos 16:f9ea2b2d410f 62 //}
saloutos 16:f9ea2b2d410f 63 //delete _point;
saloutos 16:f9ea2b2d410f 64 }
saloutos 16:f9ea2b2d410f 65
saloutos 16:f9ea2b2d410f 66 void BezierCurve::evaluateDerivative(float time, float point[]) {
saloutos 16:f9ea2b2d410f 67 //double *_point = new double[_dim];
saloutos 16:f9ea2b2d410f 68 for(int i=0; i< _dim ; i++) {
saloutos 16:f9ea2b2d410f 69 point[i] = 0;
saloutos 16:f9ea2b2d410f 70 }
saloutos 16:f9ea2b2d410f 71 //double dtime = time;
saloutos 16:f9ea2b2d410f 72 for(int i=0; i<=_order-1 ; i++) {
saloutos 16:f9ea2b2d410f 73 float mi = pow(time,i)*pow(1-time,_order-1-i) * _nck_deriv[i] * _order;
saloutos 16:f9ea2b2d410f 74 for(int j=0 ; j < _dim ; j++) {
saloutos 16:f9ea2b2d410f 75 point[j] += (_pts[i+1][j] - _pts[i][j] ) * mi;
saloutos 16:f9ea2b2d410f 76 }
saloutos 16:f9ea2b2d410f 77 }
saloutos 16:f9ea2b2d410f 78 //for(int i=0; i< _dim ; i++) {
saloutos 16:f9ea2b2d410f 79 // point[i] = _point[i];
saloutos 16:f9ea2b2d410f 80 //}
saloutos 16:f9ea2b2d410f 81 //delete _point;
saloutos 16:f9ea2b2d410f 82 }