Roman Krejci / Mbed 2 deprecated dipl_prace_v2

Servo control

Dependencies:   mbed-rtos mbed

angle.cpp@0:79ed3a6c9ccc, 2015-02-10 (annotated)

Committer:
romankrej
Date:
Tue Feb 10 14:31:40 2015 +0000
Revision:
0:79ed3a6c9ccc
version 2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
romankrej 0:79ed3a6c9ccc 1 #include "mbed.h"
romankrej 0:79ed3a6c9ccc 2 #include "rtos.h"
romankrej 0:79ed3a6c9ccc 3 #include "angle.h"
romankrej 0:79ed3a6c9ccc 4
romankrej 0:79ed3a6c9ccc 5 char i;
romankrej 0:79ed3a6c9ccc 6
romankrej 0:79ed3a6c9ccc 7 //konstruktor
romankrej 0:79ed3a6c9ccc 8 IRsensor::IRsensor(PinName IRpin) : IRsens(IRpin) {}
romankrej 0:79ed3a6c9ccc 9
romankrej 0:79ed3a6c9ccc 10
romankrej 0:79ed3a6c9ccc 11 //vraci uhel micku na kole
romankrej 0:79ed3a6c9ccc 12 float IRsensor::get_angle(void)
romankrej 0:79ed3a6c9ccc 13 {
romankrej 0:79ed3a6c9ccc 14 if(i != 20) {
romankrej 0:79ed3a6c9ccc 15 phi_new = IRsens.read();
romankrej 0:79ed3a6c9ccc 16 //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
romankrej 0:79ed3a6c9ccc 17 //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
romankrej 0:79ed3a6c9ccc 18 phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
romankrej 0:79ed3a6c9ccc 19 s_phi[7] = s_phi[6]; s_phi[6] = s_phi[5];
romankrej 0:79ed3a6c9ccc 20 s_phi[5] = s_phi[4]; s_phi[4] = s_phi[3];
romankrej 0:79ed3a6c9ccc 21 s_phi[3] = s_phi[2]; s_phi[2] = s_phi[1];
romankrej 0:79ed3a6c9ccc 22 s_phi[1] = s_phi[0]; s_phi[0] = phi_new;
romankrej 0:79ed3a6c9ccc 23 }
romankrej 0:79ed3a6c9ccc 24
romankrej 0:79ed3a6c9ccc 25 if(i == 20) {
romankrej 0:79ed3a6c9ccc 26 phi_new = IRsens.read();
romankrej 0:79ed3a6c9ccc 27 //phi_new = (-768.82*phi_new*phi_new*phi_new + 1756*phi_new*phi_new - 1381*phi_new + 359.14)*0.01745;
romankrej 0:79ed3a6c9ccc 28 //phi_new = (318.79*phi_new*phi_new - 455.26*phi_new + 142.85)*0.01745;
romankrej 0:79ed3a6c9ccc 29 phi_new = (-445.55*phi_new*phi_new*phi_new + 891.29*phi_new*phi_new - 627.67*phi_new + 143.9)*0.01745;
romankrej 0:79ed3a6c9ccc 30 s_phi[0] = phi_new; s_phi[1] = s_phi[0];
romankrej 0:79ed3a6c9ccc 31 s_phi[2] = s_phi[1]; s_phi[3] = s_phi[2];
romankrej 0:79ed3a6c9ccc 32 s_phi[4] = s_phi[3]; s_phi[5] = s_phi[4];
romankrej 0:79ed3a6c9ccc 33 s_phi[6] = s_phi[5]; s_phi[7] = s_phi[6];
romankrej 0:79ed3a6c9ccc 34 i = 0;
romankrej 0:79ed3a6c9ccc 35 }
romankrej 0:79ed3a6c9ccc 36
romankrej 0:79ed3a6c9ccc 37 phi_new = (s_phi[0] + s_phi[1] + s_phi[2] + s_phi[3] + s_phi[4] + s_phi[5] + s_phi[6] + s_phi[7])/8.0;
romankrej 0:79ed3a6c9ccc 38
romankrej 0:79ed3a6c9ccc 39 return phi_new;
romankrej 0:79ed3a6c9ccc 40
romankrej 0:79ed3a6c9ccc 41 }