#include "Function.

h" //
#include "ColSensor.h" //
#include "Track.h" //
#include "Car.h" //
#include "ColQueue.h" //
#include "Servo.h"
int color[4] = { 0, 0, 0, 0 };
//
void Catch(int n){
int a;
int s11, s12, s13, s21, s22, s23, s31, s32;
//
s11 = 70; s12 = 60; s13 = 88;
s21 = 90; s22 = 110; s23 = 60;
s31 = 140; s32 = 0;
//1-33
switch (n){
case 1:a = 160; break;
case 2:a = 140; break;
case 3:a = 120; break;
default:return; break;
}
myservo1.attach(7); myservo2.attach(12); myservo3.attach(A0);
myservo2.write(s21); myservo1.write(s11); myservo3.write(s31);
delay(300);
ServoMove(s21, s22, 2); ServoMove(s31, s32, 3); ServoMove(s11, s12, 1);
ServoMove(s22, s23, 2); ServoMove(s12, s13, 1); ServoMove(s23, s22, 2);
ServoMove(s32, a, 3); ServoMove(s22, s21, 2); ServoMove(s13, s11, 1);
ServoMove(s21, s22, 2); ServoMove(a, s31, 3); ServoMove(s22, s21, 2);
myservo1.detach(); myservo2.detach(); myservo3.detach();
Plus();
}
//
void Down(int n){
int a = 0;
int s11, s12, s13, s21, s22, s23, s31, s32;
//
s11 = 70; s12 = s11; s13 = 88;
s21 = 90; s22 = 110; s23 = s22;
s31 = 140; s32 = s31 - 90;
//1-33
switch (n){
case 1:a = 160; break;
case 2:a = 140; break;
case 3:a = 120; break;
default:return; break;
}
myservo1.attach(7); myservo2.attach(12); myservo3.attach(A0);
myservo2.write(s21); myservo1.write(s11); myservo3.write(s31);
delay(300);
ServoMove(s21, s22, 2); ServoMove(s31, a, 3); ServoMove(s22, s21, 2);
ServoMove(s11, s13, 1); ServoMove(s21, s22, 2); ServoMove(a, s32, 3);
ServoMove(s22, s23, 2); ServoMove(s13, s11, 1); ServoMove(s23, s22, 2);
ServoMove(s32, s31, 3); ServoMove(s22, s21, 2);
myservo1.detach(); myservo2.detach(); myservo3.detach();
}
/********************setup********************/
void setup() {
SPEED1 = 130; SPEED2 = 140;
TURN = 180; BACK = 200; DELAY = 800;

Memory[0] = 0;
Serial.begin(9600);
mycar.Mode();
//IN1IN3
mycar.Infer(1, 1);
mytrack.Mode();
//
Modify();
//100%
mysensor.Mode();
Ready();
//
while (num != N){
Tracking(SPEED1, SPEED2); num++; In(); Check(num);
if (Memory[num] != 0 && color[Memory[num]] == 0){
Catch(Memory[num]); color[Memory[num]] = num;
Memory[num] = 0;
}
else{
mycar.Move(SPEED1, SPEED2, 2, BACK);
mycar.Move(0, 0, 5, 1);
}
Out();
}
num = 0;
Tracking(SPEED1, SPEED2);
TurnR();
while (num != 3){
Tracking(SPEED1, SPEED2);
num++;
if (color[num] != 0)Down(num);
}
num = 0;
Tracking(SPEED1, SPEED2);
TurnR();
for (int i = 1; i <= N; i++){
Push(i);
}

}
/********************loop********************/
void loop() {
int flag = 0;
color[1] = Red.Get(); color[2] = Blue.Get(); color[3] = Green.Get();
if (color[1] == 0 && color[2] == 0 && color[3] == 0){
while (1){
mycar.Move(0, 0, 5, 1);
}
}
while (num != N){
Tracking(SPEED1, SPEED2);
num++; flag++;
if (num == N / 2 + 1)flag = 1;
for (int i = 1; i <= 3; i++){
if (num == color[i]){
Stoping(flag);
In(); Catch(i);Out();
break;
}
}
}

num = 0;
Tracking(SPEED1, SPEED2); flag++;
Stoping(flag);
TurnR();
while (num != 3){
Tracking(SPEED1, SPEED2);
num++; flag++;
if (color[num] != 0){
Stoping(flag);
Down(num);
}
}
num = 0;
Tracking(SPEED1, SPEED2); flag++;
Stoping(flag);
TurnR();