Ted Parrott / Mbed 2 deprecated walker

NA

Dependencies:   AX12 BMA180_4 L3G4200D_3 mbed

Fork of AX12-HelloWorld by Chris Styles

main.cpp@2:056f36912bf0, 2017-11-16 (annotated)

Committer:
tedparrott6
Date:
Thu Nov 16 15:05:18 2017 +0000
Revision:
2:056f36912bf0
Parent:
1:b12b06e2fc2d 
NA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:f6f8cf11779f 1 #include "mbed.h"
tedparrott6 2:056f36912bf0 2 #include "BMA180.h"
tedparrott6 2:056f36912bf0 3 #include "math.h"
tedparrott6 2:056f36912bf0 4 #include "L3G4200D.h"
chris 0:f6f8cf11779f 5 #include "AX12.h"
chris 0:f6f8cf11779f 6
tedparrott6 2:056f36912bf0 7 L3G4200D gyro(p9,p10);
tedparrott6 2:056f36912bf0 8 BMA180 my_BMA180(p5,p6,p7,p15,p16);
tedparrott6 2:056f36912bf0 9
tedparrott6 2:056f36912bf0 10 AX12 m1 (p28, p27, 1);
tedparrott6 2:056f36912bf0 11 AX12 m2 (p28, p27, 2);
tedparrott6 2:056f36912bf0 12 AX12 m3 (p28, p27, 3);
tedparrott6 2:056f36912bf0 13 AX12 m4 (p28, p27, 4);
tedparrott6 2:056f36912bf0 14 AX12 m5 (p28, p27, 5);
tedparrott6 2:056f36912bf0 15 AX12 m6 (p28, p27, 6);
tedparrott6 2:056f36912bf0 16 AX12 m7 (p28, p27, 7);
tedparrott6 2:056f36912bf0 17 AX12 m8 (p28, p27, 8);
tedparrott6 2:056f36912bf0 18 AX12 m9 (p28, p27, 9);
tedparrott6 2:056f36912bf0 19 AX12 m10 (p28, p27, 10);
tedparrott6 2:056f36912bf0 20
tedparrott6 2:056f36912bf0 21 Timer t_1;
tedparrott6 2:056f36912bf0 22 Timer t_2;
chris 0:f6f8cf11779f 23
tedparrott6 2:056f36912bf0 24 float theta_1;
tedparrott6 2:056f36912bf0 25 float theta_1_r;
tedparrott6 2:056f36912bf0 26 float theta_2;
tedparrott6 2:056f36912bf0 27 float theta_8;
tedparrott6 2:056f36912bf0 28 float theta_8_r;
tedparrott6 2:056f36912bf0 29 float theta_7;
tedparrott6 2:056f36912bf0 30 float time_1;
tedparrott6 2:056f36912bf0 31 float time_2;
tedparrott6 2:056f36912bf0 32 float angle_x;
tedparrott6 2:056f36912bf0 33 float angle_x_a;
tedparrott6 2:056f36912bf0 34 float angle_y;
tedparrott6 2:056f36912bf0 35 float angle_y_a;
tedparrott6 2:056f36912bf0 36 float r1;
tedparrott6 2:056f36912bf0 37 float r2;
tedparrott6 2:056f36912bf0 38 float pi = 3.14159;
tedparrott6 2:056f36912bf0 39 float speed_x;
tedparrott6 2:056f36912bf0 40 float speed_y;
tedparrott6 2:056f36912bf0 41 float g[3];
tedparrott6 2:056f36912bf0 42 float theta_roll;
tedparrott6 2:056f36912bf0 43 float theta_a_right;
tedparrott6 2:056f36912bf0 44 float theta_a_left;
tedparrott6 2:056f36912bf0 45 float theta_a_right_rad;
tedparrott6 2:056f36912bf0 46 float theta_a_left_rad;
tedparrott6 2:056f36912bf0 47 float time_sum;
tedparrott6 2:056f36912bf0 48 float period;
tedparrott6 2:056f36912bf0 49 int n;
tedparrott6 2:056f36912bf0 50 float A;
tedparrott6 2:056f36912bf0 51 float B;
tedparrott6 2:056f36912bf0 52 float C;
tedparrott6 2:056f36912bf0 53 float leg_control_signal;
tedparrott6 2:056f36912bf0 54 float theta_4;
tedparrott6 2:056f36912bf0 55 float theta_5;
tedparrott6 2:056f36912bf0 56 int dir_0;
tedparrott6 2:056f36912bf0 57 int dir_1;
tedparrott6 2:056f36912bf0 58 int x_msb, y_msb, z_msb;
tedparrott6 2:056f36912bf0 59 char x_lsb, y_lsb, z_lsb;
tedparrott6 2:056f36912bf0 60 short ax, ay, az;
tedparrott6 2:056f36912bf0 61 float afx, afy, afz;
tedparrott6 2:056f36912bf0 62
tedparrott6 2:056f36912bf0 63 int main()
tedparrott6 2:056f36912bf0 64 {
tedparrott6 2:056f36912bf0 65 m1.SetGoal(150);
tedparrott6 2:056f36912bf0 66 wait(0.5);
tedparrott6 2:056f36912bf0 67 m2.SetGoal(150);
tedparrott6 2:056f36912bf0 68 wait(0.5);
tedparrott6 2:056f36912bf0 69 m3.SetGoal(150);
tedparrott6 2:056f36912bf0 70 wait(0.5);
tedparrott6 2:056f36912bf0 71 m4.SetGoal(150);
tedparrott6 2:056f36912bf0 72 wait(0.5);
tedparrott6 2:056f36912bf0 73 m5.SetGoal(150);
tedparrott6 2:056f36912bf0 74 wait(0.5);
tedparrott6 2:056f36912bf0 75 m6.SetGoal(150);
tedparrott6 2:056f36912bf0 76 wait(0.5);
tedparrott6 2:056f36912bf0 77 m7.SetGoal(150);
tedparrott6 2:056f36912bf0 78 wait(0.5);
tedparrott6 2:056f36912bf0 79 m8.SetGoal(150);
tedparrott6 2:056f36912bf0 80 wait(0.5);
tedparrott6 2:056f36912bf0 81 m9.SetGoal(150);
tedparrott6 2:056f36912bf0 82 wait(0.5);
tedparrott6 2:056f36912bf0 83 m10.SetGoal(150);
tedparrott6 2:056f36912bf0 84 wait(0.5);
tedparrott6 2:056f36912bf0 85
tedparrott6 2:056f36912bf0 86 t_1.start();
tedparrott6 2:056f36912bf0 87 t_2.start();
tedparrott6 2:056f36912bf0 88 time_sum = 0;
tedparrott6 2:056f36912bf0 89 n = 0;
tedparrott6 2:056f36912bf0 90 angle_y = 0;
tedparrott6 2:056f36912bf0 91 dir_1 = 0;
tedparrott6 2:056f36912bf0 92 dir_0 = 0;
chris 1:b12b06e2fc2d 93
chris 0:f6f8cf11779f 94 while (1) {
tedparrott6 2:056f36912bf0 95 gyro.read(g); // Read and Interpret Gyro Data
tedparrott6 2:056f36912bf0 96 speed_x = ((g[1])*0.07) - 1.12;
tedparrott6 2:056f36912bf0 97 speed_y = ((g[0])*0.07) - 0.3;
tedparrott6 2:056f36912bf0 98
tedparrott6 2:056f36912bf0 99 x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register
tedparrott6 2:056f36912bf0 100 x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register
tedparrott6 2:056f36912bf0 101 ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB
tedparrott6 2:056f36912bf0 102 ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits)
tedparrott6 2:056f36912bf0 103 afx = (float)ax*3/16384;
tedparrott6 2:056f36912bf0 104
tedparrott6 2:056f36912bf0 105 y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register
tedparrott6 2:056f36912bf0 106 y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register
tedparrott6 2:056f36912bf0 107 ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB
tedparrott6 2:056f36912bf0 108 ay = ay >> 2; // Remove unused first 2 LSB
tedparrott6 2:056f36912bf0 109 afy = (float)ay*3/16384;
tedparrott6 2:056f36912bf0 110
tedparrott6 2:056f36912bf0 111 z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register
tedparrott6 2:056f36912bf0 112 z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register
tedparrott6 2:056f36912bf0 113 az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB
tedparrott6 2:056f36912bf0 114 az = az >> 2; // Remove unused first 2 LSB
tedparrott6 2:056f36912bf0 115 afz = (float)az*3/16384;
tedparrott6 2:056f36912bf0 116
tedparrott6 2:056f36912bf0 117 r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force
tedparrott6 2:056f36912bf0 118 r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force
tedparrott6 2:056f36912bf0 119 angle_x_a = atan(r1)*180/pi; // Determine X, Y angles
tedparrott6 2:056f36912bf0 120 angle_y_a = atan(r2)*180/pi;
tedparrott6 2:056f36912bf0 121
tedparrott6 2:056f36912bf0 122 angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;
tedparrott6 2:056f36912bf0 123
tedparrott6 2:056f36912bf0 124 angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
tedparrott6 2:056f36912bf0 125 theta_roll = angle_y + 4.5;
tedparrott6 2:056f36912bf0 126
tedparrott6 2:056f36912bf0 127 dir_0 = dir_1;
tedparrott6 2:056f36912bf0 128
tedparrott6 2:056f36912bf0 129 if(theta_roll >= 0) {
tedparrott6 2:056f36912bf0 130 dir_1 = 1;
tedparrott6 2:056f36912bf0 131 } else {
tedparrott6 2:056f36912bf0 132 dir_1 = 0;
tedparrott6 2:056f36912bf0 133 }
tedparrott6 2:056f36912bf0 134
tedparrott6 2:056f36912bf0 135 if(dir_1 != dir_0) {
tedparrott6 2:056f36912bf0 136 time_2 = t_2.read();
tedparrott6 2:056f36912bf0 137 t_2.reset();
tedparrott6 2:056f36912bf0 138 time_sum = time_sum + time_2;
tedparrott6 2:056f36912bf0 139 n=n+1;
tedparrott6 2:056f36912bf0 140 }
tedparrott6 2:056f36912bf0 141
tedparrott6 2:056f36912bf0 142 period = 2*(time_sum/n);
tedparrott6 2:056f36912bf0 143 B = 2*3.14159/period;
tedparrott6 2:056f36912bf0 144 C = -(period/4)*B;
tedparrott6 2:056f36912bf0 145 A = 5;
tedparrott6 2:056f36912bf0 146
tedparrott6 2:056f36912bf0 147 if(theta_roll < -3) {
tedparrott6 2:056f36912bf0 148 theta_a_right = -7;
tedparrott6 2:056f36912bf0 149 } else {
tedparrott6 2:056f36912bf0 150 theta_a_right = 0;
tedparrott6 2:056f36912bf0 151 }
tedparrott6 2:056f36912bf0 152 if(theta_roll > 3) {
tedparrott6 2:056f36912bf0 153 theta_a_left = 7;
tedparrott6 2:056f36912bf0 154 } else {
tedparrott6 2:056f36912bf0 155 theta_a_left = 0;
tedparrott6 2:056f36912bf0 156 }
tedparrott6 2:056f36912bf0 157
tedparrott6 2:056f36912bf0 158 time_1 = t_1.read();
tedparrott6 2:056f36912bf0 159 leg_control_signal = A*sin(B*time_1 + C);
tedparrott6 2:056f36912bf0 160
tedparrott6 2:056f36912bf0 161 theta_4 = 150 + leg_control_signal;
tedparrott6 2:056f36912bf0 162 theta_5 = 150 + leg_control_signal;
tedparrott6 2:056f36912bf0 163
tedparrott6 2:056f36912bf0 164 theta_a_right_rad = theta_a_right*3.14159/180;
tedparrott6 2:056f36912bf0 165 theta_a_left_rad = theta_a_left*3.14159/180;
tedparrott6 2:056f36912bf0 166 theta_2 = -1.43*theta_a_right + 150;
tedparrott6 2:056f36912bf0 167 theta_7 = -1.43*theta_a_left + 150;
tedparrott6 2:056f36912bf0 168 theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad));
tedparrott6 2:056f36912bf0 169 theta_1 = (theta_1_r*180/3.14159)+150;
tedparrott6 2:056f36912bf0 170 theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad));
tedparrott6 2:056f36912bf0 171 theta_8 = (theta_8_r*180/3.14159)+150;
tedparrott6 2:056f36912bf0 172 m1.SetGoal(theta_1);
tedparrott6 2:056f36912bf0 173 m2.SetGoal(theta_2);
tedparrott6 2:056f36912bf0 174 m7.SetGoal(theta_7);
tedparrott6 2:056f36912bf0 175 m8.SetGoal(theta_8);
tedparrott6 2:056f36912bf0 176 m4.SetGoal(theta_4);
tedparrott6 2:056f36912bf0 177 m5.SetGoal(theta_5);
chris 0:f6f8cf11779f 178 }
chris 0:f6f8cf11779f 179 }