Laura Schwendeman
/
274TeamProject2
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: MatrixMath Matrix ExperimentServer QEI_pmw MotorShield
BezierCurve.cpp@1:25284247a74c, 2021-11-22 (annotated)
- Committer:
- sabazerefa
- Date:
- Mon Nov 22 07:41:36 2021 +0000
- Revision:
- 1:25284247a74c
Implemented two motors, only copying desired. not input interface w matlab
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| sabazerefa | 1:25284247a74c | 1 | #include "BezierCurve.h" |
| sabazerefa | 1:25284247a74c | 2 | #include "math.h" |
| sabazerefa | 1:25284247a74c | 3 | #include "mbed.h" |
| sabazerefa | 1:25284247a74c | 4 | |
| sabazerefa | 1:25284247a74c | 5 | extern Serial pc; |
| sabazerefa | 1:25284247a74c | 6 | int factorial(int k) { |
| sabazerefa | 1:25284247a74c | 7 | int f = 1; |
| sabazerefa | 1:25284247a74c | 8 | for (int j = 2 ; j<= k ; j++) |
| sabazerefa | 1:25284247a74c | 9 | f*=j; |
| sabazerefa | 1:25284247a74c | 10 | return f; |
| sabazerefa | 1:25284247a74c | 11 | } |
| sabazerefa | 1:25284247a74c | 12 | |
| sabazerefa | 1:25284247a74c | 13 | BezierCurve::BezierCurve(int dim, int order) :_dim(dim), _order(order) { |
| sabazerefa | 1:25284247a74c | 14 | _pts = new float*[_order+1]; |
| sabazerefa | 1:25284247a74c | 15 | _nck = new int[_order+1]; |
| sabazerefa | 1:25284247a74c | 16 | _nck_deriv = new int[_order]; |
| sabazerefa | 1:25284247a74c | 17 | int facn = factorial(_order); |
| sabazerefa | 1:25284247a74c | 18 | int facn2= factorial(_order-1); |
| sabazerefa | 1:25284247a74c | 19 | for(int i = 0 ; i <= _order ; i++) { |
| sabazerefa | 1:25284247a74c | 20 | _pts[i] = new float[_dim]; |
| sabazerefa | 1:25284247a74c | 21 | _nck[i] = facn / factorial(i) / factorial(_order-i); |
| sabazerefa | 1:25284247a74c | 22 | if (i< _order) { |
| sabazerefa | 1:25284247a74c | 23 | _nck_deriv[i] = facn2 / factorial(i) / factorial(_order-1-i); |
| sabazerefa | 1:25284247a74c | 24 | } |
| sabazerefa | 1:25284247a74c | 25 | } |
| sabazerefa | 1:25284247a74c | 26 | } |
| sabazerefa | 1:25284247a74c | 27 | |
| sabazerefa | 1:25284247a74c | 28 | BezierCurve::~BezierCurve() { |
| sabazerefa | 1:25284247a74c | 29 | for(int i = 0 ; i < _dim ; i++) { |
| sabazerefa | 1:25284247a74c | 30 | delete _pts[i]; |
| sabazerefa | 1:25284247a74c | 31 | } |
| sabazerefa | 1:25284247a74c | 32 | delete _pts; |
| sabazerefa | 1:25284247a74c | 33 | } |
| sabazerefa | 1:25284247a74c | 34 | |
| sabazerefa | 1:25284247a74c | 35 | void BezierCurve::setPoints(float pts[] ) { |
| sabazerefa | 1:25284247a74c | 36 | pc.printf("Setting Points\n"); |
| sabazerefa | 1:25284247a74c | 37 | float * p = pts; |
| sabazerefa | 1:25284247a74c | 38 | for(int i = 0 ; i<=_order ; i++) { |
| sabazerefa | 1:25284247a74c | 39 | pc.printf("\n\r\tPt. %d:",i); |
| sabazerefa | 1:25284247a74c | 40 | for( int j = 0 ; j < _dim ; j++) { |
| sabazerefa | 1:25284247a74c | 41 | _pts[i][j] = *p; |
| sabazerefa | 1:25284247a74c | 42 | p++; |
| sabazerefa | 1:25284247a74c | 43 | pc.printf("\t\t%f",_pts[i][j]); |
| sabazerefa | 1:25284247a74c | 44 | } |
| sabazerefa | 1:25284247a74c | 45 | } |
| sabazerefa | 1:25284247a74c | 46 | } |
| sabazerefa | 1:25284247a74c | 47 | |
| sabazerefa | 1:25284247a74c | 48 | void BezierCurve::evaluate(float time, float point[]) { |
| sabazerefa | 1:25284247a74c | 49 | //float *_point = new float[_dim]; |
| sabazerefa | 1:25284247a74c | 50 | |
| sabazerefa | 1:25284247a74c | 51 | for(int i=0; i< _dim ; i++) { |
| sabazerefa | 1:25284247a74c | 52 | point[i] = 0; |
| sabazerefa | 1:25284247a74c | 53 | } |
| sabazerefa | 1:25284247a74c | 54 | for(int i=0; i<=_order ; i++) { |
| sabazerefa | 1:25284247a74c | 55 | float mi = pow(time,i)*pow(1-time,_order-i) * _nck[i]; |
| sabazerefa | 1:25284247a74c | 56 | for(int j=0 ; j < _dim ; j++) { |
| sabazerefa | 1:25284247a74c | 57 | point[j] += _pts[i][j] * mi; |
| sabazerefa | 1:25284247a74c | 58 | } |
| sabazerefa | 1:25284247a74c | 59 | } |
| sabazerefa | 1:25284247a74c | 60 | //for(int i=0; i< _dim ; i++) { |
| sabazerefa | 1:25284247a74c | 61 | // point[i] = _point[i]; |
| sabazerefa | 1:25284247a74c | 62 | //} |
| sabazerefa | 1:25284247a74c | 63 | //delete _point; |
| sabazerefa | 1:25284247a74c | 64 | } |
| sabazerefa | 1:25284247a74c | 65 | |
| sabazerefa | 1:25284247a74c | 66 | void BezierCurve::evaluateDerivative(float time, float point[]) { |
| sabazerefa | 1:25284247a74c | 67 | //double *_point = new double[_dim]; |
| sabazerefa | 1:25284247a74c | 68 | for(int i=0; i< _dim ; i++) { |
| sabazerefa | 1:25284247a74c | 69 | point[i] = 0; |
| sabazerefa | 1:25284247a74c | 70 | } |
| sabazerefa | 1:25284247a74c | 71 | //double dtime = time; |
| sabazerefa | 1:25284247a74c | 72 | for(int i=0; i<=_order-1 ; i++) { |
| sabazerefa | 1:25284247a74c | 73 | float mi = pow(time,i)*pow(1-time,_order-1-i) * _nck_deriv[i] * _order; |
| sabazerefa | 1:25284247a74c | 74 | for(int j=0 ; j < _dim ; j++) { |
| sabazerefa | 1:25284247a74c | 75 | point[j] += (_pts[i+1][j] - _pts[i][j] ) * mi; |
| sabazerefa | 1:25284247a74c | 76 | } |
| sabazerefa | 1:25284247a74c | 77 | } |
| sabazerefa | 1:25284247a74c | 78 | //for(int i=0; i< _dim ; i++) { |
| sabazerefa | 1:25284247a74c | 79 | // point[i] = _point[i]; |
| sabazerefa | 1:25284247a74c | 80 | //} |
| sabazerefa | 1:25284247a74c | 81 | //delete _point; |
| sabazerefa | 1:25284247a74c | 82 | } |
