Ted Parrott / Mbed 2 deprecated Frontal_rocker

NA

Dependencies:   AX12 BMA180_3 L3G4200D_2 mbed

Fork of AX12-HelloWorld by Chris Styles

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

Committer:
tedparrott6
Date:
Thu Nov 16 15:01:44 2017 +0000
Revision:
2:ce44bdd590ed
Parent:
1:b12b06e2fc2d 
NA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:f6f8cf11779f 1 #include "mbed.h"
tedparrott6 2:ce44bdd590ed 2 #include "BMA180.h"
tedparrott6 2:ce44bdd590ed 3 #include "math.h"
tedparrott6 2:ce44bdd590ed 4 #include "L3G4200D.h"
chris 0:f6f8cf11779f 5 #include "AX12.h"
chris 0:f6f8cf11779f 6
tedparrott6 2:ce44bdd590ed 7 Serial pc(USBTX, USBRX);
tedparrott6 2:ce44bdd590ed 8 L3G4200D gyro(p9,p10);
tedparrott6 2:ce44bdd590ed 9 BMA180 my_BMA180(p5,p6,p7,p15,p16);
tedparrott6 2:ce44bdd590ed 10
tedparrott6 2:ce44bdd590ed 11 AX12 m1 (p28, p27, 1);
tedparrott6 2:ce44bdd590ed 12 AX12 m2 (p28, p27, 2);
tedparrott6 2:ce44bdd590ed 13 AX12 m3 (p28, p27, 3);
tedparrott6 2:ce44bdd590ed 14 AX12 m4 (p28, p27, 4);
tedparrott6 2:ce44bdd590ed 15 AX12 m5 (p28, p27, 5);
tedparrott6 2:ce44bdd590ed 16 AX12 m6 (p28, p27, 6);
tedparrott6 2:ce44bdd590ed 17 AX12 m7 (p28, p27, 7);
tedparrott6 2:ce44bdd590ed 18 AX12 m8 (p28, p27, 8);
tedparrott6 2:ce44bdd590ed 19 AX12 m9 (p28, p27, 9);
tedparrott6 2:ce44bdd590ed 20 AX12 m10 (p28, p27, 10);
chris 0:f6f8cf11779f 21
tedparrott6 2:ce44bdd590ed 22 float theta_1;
tedparrott6 2:ce44bdd590ed 23 float theta_1_r;
tedparrott6 2:ce44bdd590ed 24 float theta_2;
tedparrott6 2:ce44bdd590ed 25 float theta_8;
tedparrott6 2:ce44bdd590ed 26 float theta_8_r;
tedparrott6 2:ce44bdd590ed 27 float theta_7;
tedparrott6 2:ce44bdd590ed 28 float angle_x;
tedparrott6 2:ce44bdd590ed 29 float angle_x_a;
tedparrott6 2:ce44bdd590ed 30 float angle_y;
tedparrott6 2:ce44bdd590ed 31 float angle_y_a;
tedparrott6 2:ce44bdd590ed 32 float r1;
tedparrott6 2:ce44bdd590ed 33 float r2;
tedparrott6 2:ce44bdd590ed 34 float pi = 3.14159;
tedparrott6 2:ce44bdd590ed 35 float speed_x;
tedparrott6 2:ce44bdd590ed 36 float speed_y;
tedparrott6 2:ce44bdd590ed 37 float g[3];
tedparrott6 2:ce44bdd590ed 38 float theta_roll;
tedparrott6 2:ce44bdd590ed 39 float theta_a_right;
tedparrott6 2:ce44bdd590ed 40 float theta_a_left;
tedparrott6 2:ce44bdd590ed 41 float theta_a_right_rad;
tedparrott6 2:ce44bdd590ed 42 float theta_a_left_rad;
tedparrott6 2:ce44bdd590ed 43 int x_msb, y_msb, z_msb;
tedparrott6 2:ce44bdd590ed 44 char x_lsb, y_lsb, z_lsb;
tedparrott6 2:ce44bdd590ed 45 short ax, ay, az;
tedparrott6 2:ce44bdd590ed 46 float afx, afy, afz;
chris 1:b12b06e2fc2d 47
tedparrott6 2:ce44bdd590ed 48
tedparrott6 2:ce44bdd590ed 49 int main()
tedparrott6 2:ce44bdd590ed 50 {
tedparrott6 2:ce44bdd590ed 51 m1.SetGoal(150);
tedparrott6 2:ce44bdd590ed 52 wait(0.5);
tedparrott6 2:ce44bdd590ed 53 m2.SetGoal(150);
tedparrott6 2:ce44bdd590ed 54 wait(0.5);
tedparrott6 2:ce44bdd590ed 55 m3.SetGoal(150);
tedparrott6 2:ce44bdd590ed 56 wait(0.5);
tedparrott6 2:ce44bdd590ed 57 m4.SetGoal(150);
tedparrott6 2:ce44bdd590ed 58 wait(0.5);
tedparrott6 2:ce44bdd590ed 59 m5.SetGoal(150);
tedparrott6 2:ce44bdd590ed 60 wait(0.5);
tedparrott6 2:ce44bdd590ed 61 m6.SetGoal(150);
tedparrott6 2:ce44bdd590ed 62 wait(0.5);
tedparrott6 2:ce44bdd590ed 63 m7.SetGoal(150);
tedparrott6 2:ce44bdd590ed 64 wait(0.5);
tedparrott6 2:ce44bdd590ed 65 m8.SetGoal(150);
tedparrott6 2:ce44bdd590ed 66 wait(0.5);
tedparrott6 2:ce44bdd590ed 67 m9.SetGoal(150);
tedparrott6 2:ce44bdd590ed 68 wait(0.5);
tedparrott6 2:ce44bdd590ed 69 m10.SetGoal(150);
tedparrott6 2:ce44bdd590ed 70 wait(0.5);
tedparrott6 2:ce44bdd590ed 71
tedparrott6 2:ce44bdd590ed 72 angle_y = 0;
tedparrott6 2:ce44bdd590ed 73 angle_x = 0;
chris 0:f6f8cf11779f 74 while (1) {
tedparrott6 2:ce44bdd590ed 75 gyro.read(g); // Read and Interpret Gyro Data
tedparrott6 2:ce44bdd590ed 76 speed_x = ((g[1])*0.07) - 1.12;
tedparrott6 2:ce44bdd590ed 77 speed_y = ((g[0])*0.07) - 0.3;
tedparrott6 2:ce44bdd590ed 78
tedparrott6 2:ce44bdd590ed 79 x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register
tedparrott6 2:ce44bdd590ed 80 x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register
tedparrott6 2:ce44bdd590ed 81 ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB
tedparrott6 2:ce44bdd590ed 82 ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits)
tedparrott6 2:ce44bdd590ed 83 afx = (float)ax*3/16384;
tedparrott6 2:ce44bdd590ed 84
tedparrott6 2:ce44bdd590ed 85 y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register
tedparrott6 2:ce44bdd590ed 86 y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register
tedparrott6 2:ce44bdd590ed 87 ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB
tedparrott6 2:ce44bdd590ed 88 ay = ay >> 2; // Remove unused first 2 LSB
tedparrott6 2:ce44bdd590ed 89 afy = (float)ay*3/16384;
tedparrott6 2:ce44bdd590ed 90
tedparrott6 2:ce44bdd590ed 91 z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register
tedparrott6 2:ce44bdd590ed 92 z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register
tedparrott6 2:ce44bdd590ed 93 az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB
tedparrott6 2:ce44bdd590ed 94 az = az >> 2; // Remove unused first 2 LSB
tedparrott6 2:ce44bdd590ed 95 afz = (float)az*3/16384;
tedparrott6 2:ce44bdd590ed 96
tedparrott6 2:ce44bdd590ed 97 r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force
tedparrott6 2:ce44bdd590ed 98 r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force
tedparrott6 2:ce44bdd590ed 99 angle_x_a = atan(r1)*180/pi; // Determine X, Y angles
tedparrott6 2:ce44bdd590ed 100 angle_y_a = atan(r2)*180/pi;
tedparrott6 2:ce44bdd590ed 101
tedparrott6 2:ce44bdd590ed 102 angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;
tedparrott6 2:ce44bdd590ed 103
tedparrott6 2:ce44bdd590ed 104 angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
tedparrott6 2:ce44bdd590ed 105 theta_roll = angle_y + 4.5;
tedparrott6 2:ce44bdd590ed 106
tedparrott6 2:ce44bdd590ed 107 if(theta_roll < -3) {
tedparrott6 2:ce44bdd590ed 108 theta_a_right = -7;
tedparrott6 2:ce44bdd590ed 109 } else {
tedparrott6 2:ce44bdd590ed 110 theta_a_right = 0;
tedparrott6 2:ce44bdd590ed 111 }
tedparrott6 2:ce44bdd590ed 112 if(theta_roll > 3) {
tedparrott6 2:ce44bdd590ed 113 theta_a_left = 7;
tedparrott6 2:ce44bdd590ed 114 } else {
tedparrott6 2:ce44bdd590ed 115 theta_a_left = 0;
tedparrott6 2:ce44bdd590ed 116 }
tedparrott6 2:ce44bdd590ed 117
tedparrott6 2:ce44bdd590ed 118 theta_a_right_rad = theta_a_right*3.14159/180;
tedparrott6 2:ce44bdd590ed 119 theta_a_left_rad = theta_a_left*3.14159/180;
tedparrott6 2:ce44bdd590ed 120 theta_2 = -1.43*theta_a_right + 150;
tedparrott6 2:ce44bdd590ed 121 theta_7 = -1.43*theta_a_left + 150;
tedparrott6 2:ce44bdd590ed 122 theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad));
tedparrott6 2:ce44bdd590ed 123 theta_1 = (theta_1_r*180/3.14159)+150;
tedparrott6 2:ce44bdd590ed 124 theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad));
tedparrott6 2:ce44bdd590ed 125 theta_8 = (theta_8_r*180/3.14159)+150;
tedparrott6 2:ce44bdd590ed 126 m1.SetGoal(theta_1);
tedparrott6 2:ce44bdd590ed 127 m2.SetGoal(theta_2);
tedparrott6 2:ce44bdd590ed 128 m7.SetGoal(theta_7);
tedparrott6 2:ce44bdd590ed 129 m8.SetGoal(theta_8);
chris 0:f6f8cf11779f 130 }
chris 0:f6f8cf11779f 131 }