SGMP Coperations / Mbed 2 deprecated greenimugetithothot

use this

Dependencies:   mbed

Fork of greenimugetithothot by SGMP Coperations

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?

UserRevisionLine numberNew 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