use this
Dependencies: mbed
Fork of greenimugetithothot by
main.cpp@2:e8807de906c9, 2017-12-13 (annotated)
- Committer:
- iampalop
- Date:
- Wed Dec 13 03:35:22 2017 +0000
- Revision:
- 2:e8807de906c9
- Parent:
- 1:44b26709b117
PopDetect
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
csggreen | 0:92bbf3093a25 | 1 | #include "mbed.h" |
csggreen | 1:44b26709b117 | 2 | #include "GETMPU9250.h" |
csggreen | 0:92bbf3093a25 | 3 | #include "math.h" |
csggreen | 0:92bbf3093a25 | 4 | |
csggreen | 0:92bbf3093a25 | 5 | ZMU9250 axis_Gen; |
csggreen | 0:92bbf3093a25 | 6 | |
csggreen | 0:92bbf3093a25 | 7 | Serial pc(D1, D0); |
csggreen | 0:92bbf3093a25 | 8 | |
iampalop | 2:e8807de906c9 | 9 | DigitalIn get(PC_7); |
iampalop | 2:e8807de906c9 | 10 | DigitalIn mon1(PA_7); |
iampalop | 2:e8807de906c9 | 11 | DigitalIn mon2(PB_6); |
iampalop | 2:e8807de906c9 | 12 | DigitalIn mon3(PA_5); |
iampalop | 2:e8807de906c9 | 13 | |
csggreen | 0:92bbf3093a25 | 14 | float axis_gz, axis_gy, axis_gx; |
csggreen | 0:92bbf3093a25 | 15 | float axis_az, axis_ay, axis_ax; |
iampalop | 2:e8807de906c9 | 16 | float CurAy, CurAx, CurAz, CurGx, CurGy, CurGz; |
iampalop | 2:e8807de906c9 | 17 | float diffAy, diffAx, diffAz, diffGx, diffGy, diffGz; |
iampalop | 2:e8807de906c9 | 18 | float getAy, getAx, getAz, getGx, getGy, getGz; |
iampalop | 2:e8807de906c9 | 19 | int getitem = 1, monster1 = 1, monster2 = 1, monster3 = 1, boss1 = 1, boss2 = 1, boss3 = 1; |
csggreen | 0:92bbf3093a25 | 20 | |
csggreen | 0:92bbf3093a25 | 21 | int main(){ |
csggreen | 0:92bbf3093a25 | 22 | |
csggreen | 0:92bbf3093a25 | 23 | while(1){ |
iampalop | 2:e8807de906c9 | 24 | //wait(0.1); |
csggreen | 0:92bbf3093a25 | 25 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 26 | axis_ax = (axis_Gen.g_ax()*1000)+0; |
iampalop | 2:e8807de906c9 | 27 | axis_ay = (axis_Gen.g_ay()*1000)+0; |
iampalop | 2:e8807de906c9 | 28 | axis_az = (axis_Gen.g_az()*1000)-1000; |
iampalop | 2:e8807de906c9 | 29 | axis_gx = axis_Gen.g_gx()*4; |
iampalop | 2:e8807de906c9 | 30 | axis_gy = axis_Gen.g_gy()*4; |
iampalop | 2:e8807de906c9 | 31 | axis_gz = axis_Gen.g_gz()*4; |
iampalop | 2:e8807de906c9 | 32 | pc.printf(" Gaxis_x %f\t Gaxis_y %f\t Gaxis_z %f ", axis_gx, axis_gy, axis_gz); |
iampalop | 2:e8807de906c9 | 33 | pc.printf(" Aaxis_x %f\t Aaxis_y %f\t Aaxis_z %f\n\n\n\r", axis_ax, axis_ay, axis_az); |
iampalop | 2:e8807de906c9 | 34 | // wait(0.2); |
iampalop | 2:e8807de906c9 | 35 | |
iampalop | 2:e8807de906c9 | 36 | if(get == 0) |
iampalop | 2:e8807de906c9 | 37 | { |
iampalop | 2:e8807de906c9 | 38 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 39 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 40 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 41 | while(getitem) |
iampalop | 2:e8807de906c9 | 42 | { |
iampalop | 2:e8807de906c9 | 43 | pc.printf("\n\nAZ = %f ", getAz); |
iampalop | 2:e8807de906c9 | 44 | wait(0.1); |
iampalop | 2:e8807de906c9 | 45 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 46 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 47 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 48 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 49 | pc.printf("\n\nCurrentZ = %f ", CurAz); |
iampalop | 2:e8807de906c9 | 50 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 51 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 52 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 53 | pc.printf("\n\nDiffAy = %f ", diffAy); |
iampalop | 2:e8807de906c9 | 54 | pc.printf("\n\nDiffAx = %f ", diffAx); |
iampalop | 2:e8807de906c9 | 55 | pc.printf("\n\nDiffAz = %f \n\n", diffAz); |
iampalop | 2:e8807de906c9 | 56 | if(diffAz > 1300 and diffAx < 200 and diffAy < 200 ) |
iampalop | 2:e8807de906c9 | 57 | { |
iampalop | 2:e8807de906c9 | 58 | pc.printf("\n\n ITEM \n\n"); |
iampalop | 2:e8807de906c9 | 59 | getitem = 0; |
iampalop | 2:e8807de906c9 | 60 | } |
iampalop | 2:e8807de906c9 | 61 | } |
iampalop | 2:e8807de906c9 | 62 | getitem = 1; |
iampalop | 2:e8807de906c9 | 63 | } |
iampalop | 2:e8807de906c9 | 64 | if(mon1 == 0) |
iampalop | 2:e8807de906c9 | 65 | { |
iampalop | 2:e8807de906c9 | 66 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 67 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 68 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 69 | while(monster1) |
iampalop | 2:e8807de906c9 | 70 | { |
iampalop | 2:e8807de906c9 | 71 | pc.printf("\n\nAx = %f ", getAx); |
iampalop | 2:e8807de906c9 | 72 | wait(0.1); |
iampalop | 2:e8807de906c9 | 73 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 74 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 75 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 76 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 77 | pc.printf("\n\nCurrentX = %f ", CurAx); |
iampalop | 2:e8807de906c9 | 78 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 79 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 80 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 81 | pc.printf("\n\nDiffAy = %f ", diffAy); |
iampalop | 2:e8807de906c9 | 82 | pc.printf("\n\nDiffAx = %f ", diffAx); |
iampalop | 2:e8807de906c9 | 83 | pc.printf("\n\nDiffAz = %f \n\n", diffAz); |
iampalop | 2:e8807de906c9 | 84 | if(diffAx > 600 and diffAz < 1000 and diffAy < 200 ) |
iampalop | 2:e8807de906c9 | 85 | { |
iampalop | 2:e8807de906c9 | 86 | pc.printf("\n\n Monster 1 \n\n"); |
iampalop | 2:e8807de906c9 | 87 | monster1 = 0; |
iampalop | 2:e8807de906c9 | 88 | } |
iampalop | 2:e8807de906c9 | 89 | } |
iampalop | 2:e8807de906c9 | 90 | monster1 = 1; |
iampalop | 2:e8807de906c9 | 91 | } |
iampalop | 2:e8807de906c9 | 92 | if(mon2 == 0) |
iampalop | 2:e8807de906c9 | 93 | { |
iampalop | 2:e8807de906c9 | 94 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 95 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 96 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 97 | while(monster2) |
iampalop | 2:e8807de906c9 | 98 | { |
iampalop | 2:e8807de906c9 | 99 | pc.printf("\n\nAy = %f ", getAy); |
iampalop | 2:e8807de906c9 | 100 | wait(0.1); |
iampalop | 2:e8807de906c9 | 101 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 102 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 103 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 104 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 105 | pc.printf("\n\nCurrentY = %f ", CurAy); |
iampalop | 2:e8807de906c9 | 106 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 107 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 108 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 109 | pc.printf("\n\nDiffAy = %f ", diffAy); |
iampalop | 2:e8807de906c9 | 110 | pc.printf("\n\nDiffAx = %f ", diffAx); |
iampalop | 2:e8807de906c9 | 111 | pc.printf("\n\nDiffAz = %f \n\n", diffAz); |
iampalop | 2:e8807de906c9 | 112 | if(diffAy > 800 and diffAz < 1000 and diffAx < 200 ) |
iampalop | 2:e8807de906c9 | 113 | { |
iampalop | 2:e8807de906c9 | 114 | pc.printf("\n\n Monster 2 \n\n"); |
iampalop | 2:e8807de906c9 | 115 | monster2 = 0; |
iampalop | 2:e8807de906c9 | 116 | } |
iampalop | 2:e8807de906c9 | 117 | } |
iampalop | 2:e8807de906c9 | 118 | monster2 = 1; |
iampalop | 2:e8807de906c9 | 119 | } |
iampalop | 2:e8807de906c9 | 120 | if(mon3 == 0) |
iampalop | 2:e8807de906c9 | 121 | { |
iampalop | 2:e8807de906c9 | 122 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 123 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 124 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 125 | while(monster3) |
iampalop | 2:e8807de906c9 | 126 | { |
iampalop | 2:e8807de906c9 | 127 | pc.printf("\n\nAx = %f ", getAx); |
iampalop | 2:e8807de906c9 | 128 | wait(0.1); |
iampalop | 2:e8807de906c9 | 129 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 130 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 131 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 132 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 133 | pc.printf("\n\nCurrentX = %f ", CurAx); |
iampalop | 2:e8807de906c9 | 134 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 135 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 136 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 137 | pc.printf("\n\nDiffAy = %f ", diffAy); |
iampalop | 2:e8807de906c9 | 138 | pc.printf("\n\nDiffAx = %f ", diffAx); |
iampalop | 2:e8807de906c9 | 139 | pc.printf("\n\nDiffAz = %f \n\n", diffAz); |
iampalop | 2:e8807de906c9 | 140 | if(diffAy > 200 and diffAz < 900 and diffAy < 300 ) |
iampalop | 2:e8807de906c9 | 141 | { |
iampalop | 2:e8807de906c9 | 142 | pc.printf("\n\n Monster 3 \n\n"); |
iampalop | 2:e8807de906c9 | 143 | monster3 = 0; |
iampalop | 2:e8807de906c9 | 144 | } |
iampalop | 2:e8807de906c9 | 145 | } |
iampalop | 2:e8807de906c9 | 146 | monster3 = 1; |
iampalop | 2:e8807de906c9 | 147 | } |
iampalop | 2:e8807de906c9 | 148 | if(get == 0 and mon1 == 0) |
iampalop | 2:e8807de906c9 | 149 | { |
iampalop | 2:e8807de906c9 | 150 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 151 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 152 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 153 | getGx = abs(axis_gx); |
iampalop | 2:e8807de906c9 | 154 | getGy = abs(axis_gy); |
iampalop | 2:e8807de906c9 | 155 | getGz = abs(axis_gz); |
iampalop | 2:e8807de906c9 | 156 | while(boss1) |
iampalop | 2:e8807de906c9 | 157 | { |
iampalop | 2:e8807de906c9 | 158 | pc.printf("\n\nAx = %f ", getAx); |
iampalop | 2:e8807de906c9 | 159 | wait(0.1); |
iampalop | 2:e8807de906c9 | 160 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 161 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 162 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 163 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 164 | CurGx = axis_Gen.g_gx()*4; |
iampalop | 2:e8807de906c9 | 165 | CurGy = axis_Gen.g_gy()*4; |
iampalop | 2:e8807de906c9 | 166 | CurGz = axis_Gen.g_gz()*4; |
iampalop | 2:e8807de906c9 | 167 | pc.printf("\n\nCurrentX = %f ", CurAx); |
iampalop | 2:e8807de906c9 | 168 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 169 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 170 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 171 | diffGx = abs(abs(CurGx)-getGx); |
iampalop | 2:e8807de906c9 | 172 | diffGy = abs(abs(CurGy)-getGy); |
iampalop | 2:e8807de906c9 | 173 | diffGz = abs(abs(CurGz)-getGz); |
iampalop | 2:e8807de906c9 | 174 | if(diffAx > 600 and diffAz < 1000 and diffAy < 200 and diffGx > 700) |
iampalop | 2:e8807de906c9 | 175 | { |
iampalop | 2:e8807de906c9 | 176 | pc.printf("\n\n Boss 1 \n\n"); |
iampalop | 2:e8807de906c9 | 177 | boss1 = 0; |
iampalop | 2:e8807de906c9 | 178 | } |
iampalop | 2:e8807de906c9 | 179 | } |
iampalop | 2:e8807de906c9 | 180 | boss1 = 1; |
iampalop | 2:e8807de906c9 | 181 | } |
iampalop | 2:e8807de906c9 | 182 | if(get == 0 and mon2 == 0) |
iampalop | 2:e8807de906c9 | 183 | { |
iampalop | 2:e8807de906c9 | 184 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 185 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 186 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 187 | getGx = abs(axis_gx); |
iampalop | 2:e8807de906c9 | 188 | getGy = abs(axis_gy); |
iampalop | 2:e8807de906c9 | 189 | getGz = abs(axis_gz); |
iampalop | 2:e8807de906c9 | 190 | while(boss2) |
iampalop | 2:e8807de906c9 | 191 | { |
iampalop | 2:e8807de906c9 | 192 | pc.printf("\n\nAx = %f ", getAx); |
iampalop | 2:e8807de906c9 | 193 | wait(0.1); |
iampalop | 2:e8807de906c9 | 194 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 195 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 196 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 197 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 198 | CurGx = axis_Gen.g_gx()*4; |
iampalop | 2:e8807de906c9 | 199 | CurGy = axis_Gen.g_gy()*4; |
iampalop | 2:e8807de906c9 | 200 | CurGz = axis_Gen.g_gz()*4; |
iampalop | 2:e8807de906c9 | 201 | pc.printf("\n\nCurrentX = %f ", CurAx); |
iampalop | 2:e8807de906c9 | 202 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 203 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 204 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 205 | diffGx = abs(abs(CurGx)-getGx); |
iampalop | 2:e8807de906c9 | 206 | diffGy = abs(abs(CurGy)-getGy); |
iampalop | 2:e8807de906c9 | 207 | diffGz = abs(abs(CurGz)-getGz); |
iampalop | 2:e8807de906c9 | 208 | if(diffAx > 200 and diffAz < 1000 and diffAy < 200 and diffGz > 500) |
iampalop | 2:e8807de906c9 | 209 | { |
iampalop | 2:e8807de906c9 | 210 | pc.printf("\n\n Boss 2 \n\n"); |
iampalop | 2:e8807de906c9 | 211 | boss2 = 0; |
iampalop | 2:e8807de906c9 | 212 | } |
iampalop | 2:e8807de906c9 | 213 | } |
iampalop | 2:e8807de906c9 | 214 | boss2 = 1; |
iampalop | 2:e8807de906c9 | 215 | } |
iampalop | 2:e8807de906c9 | 216 | if(get == 0 and mon3 == 0) |
iampalop | 2:e8807de906c9 | 217 | { |
iampalop | 2:e8807de906c9 | 218 | getAy = abs(axis_ay); |
iampalop | 2:e8807de906c9 | 219 | getAx = abs(axis_ax); |
iampalop | 2:e8807de906c9 | 220 | getAz = abs(axis_az); |
iampalop | 2:e8807de906c9 | 221 | getGx = abs(axis_gx); |
iampalop | 2:e8807de906c9 | 222 | getGy = abs(axis_gy); |
iampalop | 2:e8807de906c9 | 223 | getGz = abs(axis_gz); |
iampalop | 2:e8807de906c9 | 224 | while(boss3) |
iampalop | 2:e8807de906c9 | 225 | { |
iampalop | 2:e8807de906c9 | 226 | pc.printf("\n\nAx = %f ", getAx); |
iampalop | 2:e8807de906c9 | 227 | wait(0.1); |
iampalop | 2:e8807de906c9 | 228 | axis_Gen.Update(); |
iampalop | 2:e8807de906c9 | 229 | CurAy = axis_Gen.g_ay()*1000; |
iampalop | 2:e8807de906c9 | 230 | CurAx = axis_Gen.g_ax()*1000; |
iampalop | 2:e8807de906c9 | 231 | CurAz = axis_Gen.g_az()*1000; |
iampalop | 2:e8807de906c9 | 232 | CurGx = axis_Gen.g_gx()*4; |
iampalop | 2:e8807de906c9 | 233 | CurGy = axis_Gen.g_gy()*4; |
iampalop | 2:e8807de906c9 | 234 | CurGz = axis_Gen.g_gz()*4; |
iampalop | 2:e8807de906c9 | 235 | pc.printf("\n\nCurrentX = %f ", CurAx); |
iampalop | 2:e8807de906c9 | 236 | diffAy = abs(abs(CurAy)-getAy); |
iampalop | 2:e8807de906c9 | 237 | diffAx = abs(abs(CurAx)-getAx); |
iampalop | 2:e8807de906c9 | 238 | diffAz = abs(abs(CurAz)-getAz); |
iampalop | 2:e8807de906c9 | 239 | diffGx = abs(abs(CurGx)-getGx); |
iampalop | 2:e8807de906c9 | 240 | diffGy = abs(abs(CurGy)-getGy); |
iampalop | 2:e8807de906c9 | 241 | diffGz = abs(abs(CurGz)-getGz); |
iampalop | 2:e8807de906c9 | 242 | if(diffAx > 500 and diffAy > 500 and diffAz > 800 and diffGx > 500 and diffGy > 500 and diffGz > 500) |
iampalop | 2:e8807de906c9 | 243 | { |
iampalop | 2:e8807de906c9 | 244 | pc.printf("\n\n Boss 3 \n\n"); |
iampalop | 2:e8807de906c9 | 245 | boss3 = 0; |
iampalop | 2:e8807de906c9 | 246 | } |
iampalop | 2:e8807de906c9 | 247 | } |
iampalop | 2:e8807de906c9 | 248 | boss3 = 1; |
iampalop | 2:e8807de906c9 | 249 | } |
csggreen | 0:92bbf3093a25 | 250 | } |
csggreen | 0:92bbf3093a25 | 251 | } |
csggreen | 0:92bbf3093a25 | 252 | |
csggreen | 0:92bbf3093a25 | 253 |