BioRobotics / angleandposition

Library to calculate angles from positions and vice versa (also used for shooting angles)

Dependents:   includeair includeair calcul_Ueff2 Mesure_energie

angleandposition.cpp@0:b8295c4b5793, 2015-10-15 (annotated)

Committer:
Gerth
Date:
Thu Oct 15 11:19:39 2015 +0000
Revision:
0:b8295c4b5793
Child:
1:6cace9fdb088
library to calculate angles from positions and vice versa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:b8295c4b5793 1 #include "angleandposition.h"
Gerth 0:b8295c4b5793 2 #include "mbed.h"
Gerth 0:b8295c4b5793 3 #include "math.h"
Gerth 0:b8295c4b5793 4 #include <vector>
Gerth 0:b8295c4b5793 5 using std::vector;
Gerth 0:b8295c4b5793 6
Gerth 0:b8295c4b5793 7 Serial comp(USBTX,USBRX);
Gerth 0:b8295c4b5793 8
Gerth 0:b8295c4b5793 9 const float bar1=260; // length bar 1 in mm
Gerth 0:b8295c4b5793 10 const float bar2=342; // length bar 2 in mm
Gerth 0:b8295c4b5793 11 const float y_position=225;// y positon of pod in mm
Gerth 0:b8295c4b5793 12 const float y_punch=425;// y position when punched out in mm
Gerth 0:b8295c4b5793 13 const float width_playfield=473; //width in mm
Gerth 0:b8295c4b5793 14 const float base_spacing=105.29; //spacing between two dc motors at base
Gerth 0:b8295c4b5793 15 const float number_steps=10;
Gerth 0:b8295c4b5793 16 // make linspace of y positions
Gerth 0:b8295c4b5793 17
Gerth 0:b8295c4b5793 18
Gerth 0:b8295c4b5793 19 angleandposition::angleandposition(void)
Gerth 0:b8295c4b5793 20 {
Gerth 0:b8295c4b5793 21 float y_step=y_position;
Gerth 0:b8295c4b5793 22 vector<double> array;
Gerth 0:b8295c4b5793 23 double step = (y_punch-y_position) / (number_steps-1);
Gerth 0:b8295c4b5793 24
Gerth 0:b8295c4b5793 25 while(y_step <= y_punch) {
Gerth 0:b8295c4b5793 26 array.push_back(y_step);
Gerth 0:b8295c4b5793 27 y_step += step; // could recode to better handle rounding errors
Gerth 0:b8295c4b5793 28 comp.printf("y= %f\n",y_step);
Gerth 0:b8295c4b5793 29 }
Gerth 0:b8295c4b5793 30 }
Gerth 0:b8295c4b5793 31
Gerth 0:b8295c4b5793 32 float angleandposition::positiontoangle1(float x_position)
Gerth 0:b8295c4b5793 33 {
Gerth 0:b8295c4b5793 34 /* float virt_bar_right = sqrt((x_const-0.5.*base_spacing).^2 + y.^2);
Gerth 0:b8295c4b5793 35 return angle1;*/
Gerth 0:b8295c4b5793 36 }
Gerth 0:b8295c4b5793 37
Gerth 0:b8295c4b5793 38 float angleandposition::positiontoangle2(float x_position)
Gerth 0:b8295c4b5793 39 {
Gerth 0:b8295c4b5793 40
Gerth 0:b8295c4b5793 41
Gerth 0:b8295c4b5793 42 //return angle2;
Gerth 0:b8295c4b5793 43 }