#include #include #include #include #include #include AF_DCMotor motor1(1); AF_DCMotor motor3(4); AF_DCMotor motor2(3); #include #include Us u; Compass compass; int azimut; void setup() { Serial.begin(9600); InfraredSeeker::Initialize(); //m.ports(3, 4, 2, 7, 6, 5, 10, 9, 8); compass.ci(); compass.read(); azimut = compass.cps; } int strk; void loop() { int g; float r = u.us(A5) - 30; float b = u.us(A1) - 30; float l = u.us(A4) - 30; float an; int o = 0; int up; int i=analogRead(A2); int i1=analogRead(A3); bool but=analogRead(A0); strk=EEPROM.read(3); compass.read(); InfraredResult InfraredBall = InfraredSeeker::ReadAC(); int Dir = InfraredBall.Direction; int str = InfraredBall.Strength; if(but!=1) EEPROM.write(3,str); up=ups(compass.cps,azimut); if((l>0&&l<31)&&(o==0)&&(Dir==5||Dir==6||Dir==4)&&(str>150)) { an=(atan((183-b)/(61-l)))*57,2; o=1; } if((r>0&&r<31)&&(o==0)&&(Dir==5||Dir==6||Dir==4)&&(str>150)) { an=(atan((183-b)/(61-r)))*57,2; o=-1; } if((r>31||l>31)||(r<0||l<0)) { str=InfraredBall.Strength; up=ups(compass.cps,azimut); if(str<=strk ) { //Serial.println("a"); if(Dir == 4) { motor(-180-up*2,1); motor(255-up*2,2); motor(180-up*-2,3); } if(Dir == 3) { motor(-255-up*2,1); motor(255-up*2,2); motor(220-up*-2,3); } if(Dir == 6) { motor(180-up*2,1); motor(-255-up*2,2); motor(-180-up*-2,3); } if(Dir==7) { motor(180-up*2,1); motor(-180-up*2,2); motor(-255-up*-2,3); } if(Dir == 2) { if(str>=130) { motor(-255-up*2,1); motor(-up*2,2); motor(-255+up*2,3); } if(str<=130) { motor(-255-up*4,1); motor(170-up*4,2); } } if(Dir == 8) { if(str>=130) { motor(-255-up*2,1); motor(-up*2,2); motor(-255+up*2,3); } if(str<=130) { motor(-255+up*4,3); motor(-170-up*4,2); } } if(Dir == 9) { if(str>=120) { motor(-255-up*2,1); motor(-up*2,2); motor(-255+up*2,3); } if(str<=120) { motor(-255+up*4,3); motor(-180-up*4,2); } } if(Dir==1) { if(str>=120) { motor(-255-up*2,1); motor(-up*2,2); motor(-255+up*2,3); } if(str<=120) { motor(-255-up*4,1); motor(180-up*4,2); } } if(Dir == 5){ motor(255-up*2,1); motor(-up*2,2); motor(255+up*2,3); } if(Dir==0) { motor(-255-up*2,1); motor(-up*2,2); motor(-255+up*2,3); } } else { motor(255-up*2,1); motor(-up*2,2); motor(255+up*2,3); } } if(o==1) { g=ups(compass.cps,azimut+(90-an)); motor(255-g*2,1); motor(-g*2,2); motor(255+g*2,3); if(Dir!=5||str<150) o=0; } if(o==-1) { g=ups(compass.cps,azimut-(90-an)); motor(255-g*2,1); motor(-g*2,2); motor(255+g*2,3); if(Dir!=5||str<150) o=0; } if(i1>=800) { motor(180-up*2,1); motor(-255-up*2,2); motor(-180-up*-2,3); } if(i<=580) { motor(-180-up*2,1); motor(255-up*2,2); motor(180-up*-2,3); } } int ups(int c, int a) { int up = (c + 540 - a) % 360 - 180; return up; }