Ted Parrott / Mbed 2 deprecated Frontal_tilt

NA

Dependencies:   AX12 mbed

Fork of AX12-HelloWorld by Chris Styles

main.cpp@2:8a752bc2f2f8, 2017-11-16 (annotated)

Committer:
tedparrott6
Date:
Thu Nov 16 15:02:21 2017 +0000
Revision:
2:8a752bc2f2f8
Parent:
1:b12b06e2fc2d 
NA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:f6f8cf11779f 1 #include "mbed.h"
chris 0:f6f8cf11779f 2 #include "AX12.h"
chris 0:f6f8cf11779f 3
chris 0:f6f8cf11779f 4 int main() {
chris 0:f6f8cf11779f 5
tedparrott6 2:8a752bc2f2f8 6 AX12 m1 (p28, p27, 1);
tedparrott6 2:8a752bc2f2f8 7 AX12 m2 (p28, p27, 2);
tedparrott6 2:8a752bc2f2f8 8 AX12 m3 (p28, p27, 3);
tedparrott6 2:8a752bc2f2f8 9 AX12 m4 (p28, p27, 4);
tedparrott6 2:8a752bc2f2f8 10 AX12 m6 (p28, p27, 6);
tedparrott6 2:8a752bc2f2f8 11 AX12 m5 (p28, p27, 5);
tedparrott6 2:8a752bc2f2f8 12 AX12 m7 (p28, p27, 7);
tedparrott6 2:8a752bc2f2f8 13 AX12 m8 (p28, p27, 8);
tedparrott6 2:8a752bc2f2f8 14 AX12 m9 (p28, p27, 9);
tedparrott6 2:8a752bc2f2f8 15 AX12 m10 (p28, p27, 10);
tedparrott6 2:8a752bc2f2f8 16 Serial pc(USBTX,USBRX);
tedparrott6 2:8a752bc2f2f8 17 Timer t;
tedparrott6 2:8a752bc2f2f8 18 float l_theta_k;
tedparrott6 2:8a752bc2f2f8 19 float l_theta_h;
tedparrott6 2:8a752bc2f2f8 20 float l_theta_a_x;
tedparrott6 2:8a752bc2f2f8 21 float l_theta_a_x_rad;
tedparrott6 2:8a752bc2f2f8 22 float l_theta_7_x;
tedparrott6 2:8a752bc2f2f8 23 float l_theta_8_x;
tedparrott6 2:8a752bc2f2f8 24 float l_theta_a_y;
tedparrott6 2:8a752bc2f2f8 25 float l_theta_a_y_rad;
tedparrott6 2:8a752bc2f2f8 26 float l_theta_7_y;
tedparrott6 2:8a752bc2f2f8 27 float l_theta_8_y;
tedparrott6 2:8a752bc2f2f8 28 float l_theta_8_r_y;
tedparrott6 2:8a752bc2f2f8 29 float l_theta_5;
tedparrott6 2:8a752bc2f2f8 30 float l_theta_7_m;
tedparrott6 2:8a752bc2f2f8 31 float l_theta_8_m;
tedparrott6 2:8a752bc2f2f8 32 float l_theta_6;
tedparrott6 2:8a752bc2f2f8 33 float h_l;
tedparrott6 2:8a752bc2f2f8 34 float r_theta_k;
tedparrott6 2:8a752bc2f2f8 35 float r_theta_h;
tedparrott6 2:8a752bc2f2f8 36 float r_theta_a_x;
tedparrott6 2:8a752bc2f2f8 37 float r_theta_a_x_rad;
tedparrott6 2:8a752bc2f2f8 38 float r_theta_2_x;
tedparrott6 2:8a752bc2f2f8 39 float r_theta_1_x;
tedparrott6 2:8a752bc2f2f8 40 float r_theta_a_y;
tedparrott6 2:8a752bc2f2f8 41 float r_theta_a_y_rad;
tedparrott6 2:8a752bc2f2f8 42 float r_theta_2_y;
tedparrott6 2:8a752bc2f2f8 43 float r_theta_1_y;
tedparrott6 2:8a752bc2f2f8 44 float r_theta_1_r_y;
tedparrott6 2:8a752bc2f2f8 45 float r_theta_4;
tedparrott6 2:8a752bc2f2f8 46 float r_theta_2_m;
tedparrott6 2:8a752bc2f2f8 47 float r_theta_1_m;
tedparrott6 2:8a752bc2f2f8 48 float r_theta_3;
tedparrott6 2:8a752bc2f2f8 49 float h_r;
tedparrott6 2:8a752bc2f2f8 50 float time;
tedparrott6 2:8a752bc2f2f8 51 float angle_read;
tedparrott6 2:8a752bc2f2f8 52 float angle_read_rad;
tedparrott6 2:8a752bc2f2f8 53 m5.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 54 wait(1);
tedparrott6 2:8a752bc2f2f8 55 m6.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 56 wait(1);
tedparrott6 2:8a752bc2f2f8 57 m7.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 58 wait(1);
tedparrott6 2:8a752bc2f2f8 59 m8.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 60 wait(1);
tedparrott6 2:8a752bc2f2f8 61 m1.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 62 wait(1);
tedparrott6 2:8a752bc2f2f8 63 m2.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 64 wait(1);
tedparrott6 2:8a752bc2f2f8 65 m3.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 66 wait(1);
tedparrott6 2:8a752bc2f2f8 67 m4.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 68 wait(1);
tedparrott6 2:8a752bc2f2f8 69 m9.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 70 wait(1);
tedparrott6 2:8a752bc2f2f8 71 m10.SetGoal(150);
tedparrott6 2:8a752bc2f2f8 72 wait(1);
tedparrott6 2:8a752bc2f2f8 73 t.start();
tedparrott6 2:8a752bc2f2f8 74 h_l = 0;
tedparrott6 2:8a752bc2f2f8 75 h_r = 0;
tedparrott6 2:8a752bc2f2f8 76
chris 0:f6f8cf11779f 77 while (1) {
tedparrott6 2:8a752bc2f2f8 78 time = t.read();
tedparrott6 2:8a752bc2f2f8 79 angle_read = 5*sin(time);
tedparrott6 2:8a752bc2f2f8 80 angle_read_rad = angle_read*(3.14159/180);
tedparrott6 2:8a752bc2f2f8 81
tedparrott6 2:8a752bc2f2f8 82 if(angle_read == 0){
tedparrott6 2:8a752bc2f2f8 83 h_l = 0;
tedparrott6 2:8a752bc2f2f8 84 h_r = 0;
tedparrott6 2:8a752bc2f2f8 85 }else if(angle_read > 0){
tedparrott6 2:8a752bc2f2f8 86 h_l = 79*tan(angle_read_rad);
tedparrott6 2:8a752bc2f2f8 87 h_r = 0;
tedparrott6 2:8a752bc2f2f8 88 }else if(angle_read < 0){
tedparrott6 2:8a752bc2f2f8 89 angle_read_rad = abs(angle_read_rad);
tedparrott6 2:8a752bc2f2f8 90 h_l = 0;
tedparrott6 2:8a752bc2f2f8 91 h_r = 79*tan(angle_read_rad);
tedparrott6 2:8a752bc2f2f8 92 }
tedparrott6 2:8a752bc2f2f8 93 l_theta_h = ((180/3.14159)*acos(1-(h_l/204)));
tedparrott6 2:8a752bc2f2f8 94 l_theta_k = 2*l_theta_h;
tedparrott6 2:8a752bc2f2f8 95 l_theta_5 = 150 + l_theta_h;
tedparrott6 2:8a752bc2f2f8 96 l_theta_6 = 150 + l_theta_k;
tedparrott6 2:8a752bc2f2f8 97
tedparrott6 2:8a752bc2f2f8 98 r_theta_h = ((180/3.14159)*acos(1-(h_r/204)));
tedparrott6 2:8a752bc2f2f8 99 r_theta_k = 2*r_theta_h;
tedparrott6 2:8a752bc2f2f8 100 r_theta_4 = 150 - r_theta_h;
tedparrott6 2:8a752bc2f2f8 101 r_theta_3 = 150 - r_theta_k;
tedparrott6 2:8a752bc2f2f8 102
tedparrott6 2:8a752bc2f2f8 103 l_theta_a_x = l_theta_h;
tedparrott6 2:8a752bc2f2f8 104 l_theta_a_x_rad = l_theta_a_x*(3.14159/180);
tedparrott6 2:8a752bc2f2f8 105 l_theta_8_x = (l_theta_a_x/0.34);
tedparrott6 2:8a752bc2f2f8 106 l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad)));
tedparrott6 2:8a752bc2f2f8 107
tedparrott6 2:8a752bc2f2f8 108 l_theta_a_y = angle_read;
tedparrott6 2:8a752bc2f2f8 109 l_theta_a_y_rad = l_theta_a_y*3.14159/180;
tedparrott6 2:8a752bc2f2f8 110 l_theta_7_y = -1.43*l_theta_a_y;
tedparrott6 2:8a752bc2f2f8 111 l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad));
tedparrott6 2:8a752bc2f2f8 112 l_theta_8_y= (l_theta_8_r_y*180/3.14159);
tedparrott6 2:8a752bc2f2f8 113
tedparrott6 2:8a752bc2f2f8 114 r_theta_a_x = r_theta_h;
tedparrott6 2:8a752bc2f2f8 115 r_theta_a_x_rad = r_theta_a_x*(3.14159/180);
tedparrott6 2:8a752bc2f2f8 116 r_theta_1_x = (r_theta_a_x/0.34);
tedparrott6 2:8a752bc2f2f8 117 r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad)));
tedparrott6 2:8a752bc2f2f8 118
tedparrott6 2:8a752bc2f2f8 119 r_theta_a_y = angle_read;
tedparrott6 2:8a752bc2f2f8 120 r_theta_a_y_rad = r_theta_a_y*3.14159/180;
tedparrott6 2:8a752bc2f2f8 121 r_theta_2_y = -1.43*r_theta_a_y;
tedparrott6 2:8a752bc2f2f8 122 r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad));
tedparrott6 2:8a752bc2f2f8 123 r_theta_1_y= (r_theta_1_r_y*180/3.14159);
tedparrott6 2:8a752bc2f2f8 124
tedparrott6 2:8a752bc2f2f8 125 l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y;
tedparrott6 2:8a752bc2f2f8 126 l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y;
tedparrott6 2:8a752bc2f2f8 127
tedparrott6 2:8a752bc2f2f8 128 r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y;
tedparrott6 2:8a752bc2f2f8 129 r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y;
tedparrott6 2:8a752bc2f2f8 130
tedparrott6 2:8a752bc2f2f8 131 m1.SetGoal(r_theta_1_m);
tedparrott6 2:8a752bc2f2f8 132 m2.SetGoal(r_theta_2_m);
tedparrott6 2:8a752bc2f2f8 133 m3.SetGoal(r_theta_3);
tedparrott6 2:8a752bc2f2f8 134 m4.SetGoal(r_theta_4);
tedparrott6 2:8a752bc2f2f8 135 m5.SetGoal(l_theta_5);
tedparrott6 2:8a752bc2f2f8 136 m6.SetGoal(l_theta_6);
tedparrott6 2:8a752bc2f2f8 137 m7.SetGoal(l_theta_7_m);
tedparrott6 2:8a752bc2f2f8 138 m8.SetGoal(l_theta_8_m);
tedparrott6 2:8a752bc2f2f8 139 pc.printf("\n\rAngle: %05f",angle_read);
chris 0:f6f8cf11779f 140 }
chris 0:f6f8cf11779f 141 }