POP-7 Test RQ-ROBOT

#include <pop7.h>
void setup() {}
void loop() {menu();}
void menu(){
  setTextSize(2);
  glcdMode(3);
  setTextColor(color[5]);glcd(2,0,"Check");
  setTextSize(3);
  while(!sw_OK()){
    switch (knob(4)){
      case 0 : setTextColor(color[1]);glcd(2,1,"Motor   "); break;
      case 1 : setTextColor(color[2]);glcd(2,1,"Color   ");break;
      case 2 : setTextColor(color[3]);glcd(2,1,"Encoder ");break;
      case 3 : setTextColor(color[6]);glcd(2,1,"GP2Y0A41");break;
      default : setTextColor(color[7]);glcd(2,1,"Servo   ");break;
    }
  }
  setTextColor(GLCD_GREEN);
  switch (knob(4)){
      case 0 : chkMotor(); break;
      case 1 : chkColor();break;
      case 2 : chkEnc();break;
      case 3 : chkGP2();break;
      default : chkServo();break;
  }
}
void left() {
  tlEnc(30, 36); fdEnc(30, 30); sKeep(); bkWall(50);
}
void right() {
  trEnc(30, 36); fdEnc(30, 30); sKeep(); bkWall(50);
}

void run1(){
  sHome();
  fd(50); while(readColor()==3); ao();beep();
  fdEnc(50,75);beep();
  if (readColor()==2){glcdFillScreen(GLCD_YELLOW);left();}
  else{glcdFillScreen(GLCD_GREEN);right();}
}
void ShowColor(){
  while(!sw1()){
    if(readColor()==1){ glcdFillScreen(GLCD_WHITE);}
    else if(readColor()==2){ glcdFillScreen(GLCD_YELLOW);}
    if(readColor()==3){ glcdFillScreen(GLCD_RED);}
    if(readColor()==4){ glcdFillScreen(GLCD_GRAY);}
    if(readColor()==5){ glcdFillScreen(GLCD_VIOLET);}
    if(readColor()==6){ glcdFillScreen(GLCD_GREEN);}
  }
}
void chkMotor(){
  glcdClear();setTextSize(2);
  setTextColor(GLCD_RED);   glcd(1,0,"Forward   ");fd(100);delay(1000);ao();delay(200);
  setTextColor(GLCD_YELLOW);glcd(1,0,"Backward  ");bk(100);delay(1000);ao();delay(200);
  setTextColor(GLCD_VIOLET);glcd(1,0,"Spin Left ");sl(100);delay(1000);ao();delay(200);
  setTextColor(GLCD_PINK);  glcd(1,0,"Spin Right");sr(100);delay(1000);ao();delay(200);
  setTextColor(GLCD_GREEN); glcd(1,0,"Turn Left ");tl(100);delay(1000);ao();delay(200);
  setTextColor(GLCD_BLUE);  glcd(1,0,"Turn Right");tr(100);delay(1000);ao();delay(200);
  glcdClear();
}
void chkColor() {
  glcdClear();
  unsigned int I,R,G,B,a;
  setTextSize(2);
  setTextColor(GLCD_WHITE);glcd(0,0,"SW1 Exit");
  while(!sw1()){
    I=0;for(a=0;a<40;a++){I=analog(0)+I;}I=I/40;
    R=0;for(a=0;a<40;a++){R=analog(1)+R;}R=R/40;
    G=0;for(a=0;a<40;a++){G=analog(2)+G;}G=G/40;
    B=0;for(a=0;a<40;a++){B=analog(3)+B;}B=B/40;
    setTextColor(GLCD_VIOLET);glcd(2,0,"I=%d ",I);
    setTextColor(GLCD_RED);glcd(3,0,"R=%d ",R);
    setTextColor(GLCD_GREEN);glcd(4,0,"G=%d ",G);
    setTextColor(GLCD_BLUE);glcd(5,0,"B=%d ",B);
  }
  glcdClear();
}
void chkEnc(){
  glcdClear();setTextSize(2);
  long t;
  setTextColor(GLCD_RED);   glcd(1,0,"Forward   ");fdEnc(60,80);
  setTextColor(GLCD_GREEN); glcd(1,0,"Turn Left ");tlEnc(60,80);
  setTextColor(GLCD_BLUE);  glcd(1,0,"Turn Right");trEnc(60,80);
  setTextColor(GLCD_YELLOW);glcd(1,0,"Backward  ");bkEnc(60,80);
  setTextColor(GLCD_VIOLET);glcd(1,0,"Spin Left ");slEnc(60,40);
  setTextColor(GLCD_PINK);  glcd(1,0,"Spin Right");srEnc(60,40);
  glcdClear();
}
void chkGP2(){
  glcdClear();
  setTextSize(2);
  setTextColor(GLCD_WHITE);glcd(0,0,"SW1 Exit");
  setTextColor(GLCD_RED);
    while(!sw1()){
      setTextColor(GLCD_RED);glcd(2,0,"Raw=%d  ",GP2AV());
      setTextColor(GLCD_GREEN);glcd(3,0,"CW=%d  ",getdist(4));
    }
  glcdClear();
}
void chkServo(){
  glcdClear();  setTextSize(2);
  setTextColor(GLCD_WHITE);glcd(0,0,"SW1 Exit");
  setTextSize(1);
  setTextColor(GLCD_GREEN);glcd(3,0,"ZX-Switch for Test");
  sHome();delay(300);
  sKeep();delay(300);
  sHome();delay(300);
  servo(1,70);delay(300);
  servo(2,95);delay(300);
  servo(1,160);delay(300);
  servo(2,5);delay(300);

  setTextSize(2);
  int x=0,y=0;;
  while(!sw1()){
    if(!in(8)){
       beep();
       if(x){setTextColor(GLCD_YELLOW);glcd(3,0,"ServoL Home");servo(1,160);x=0;}
       else{setTextColor(GLCD_GREEN);glcd(3,0,"ServoL Keep");servo(1,70);x=1;}}
    if(!in(9)){
      beep();
      if(y){setTextColor(GLCD_BLUE);glcd(4,0,"ServoR Home");servo(2,5);y=0;}
      else{setTextColor(GLCD_RED);glcd(4,0,"ServoR Keep");servo(2,95);y=1;}}
  }
  glcdClear();
}

void fdWall(int D){
  while(GP2AV()<D){
    fd(50);
  }
  ao();
}
int GP2AV(){
int R=0,i;
  for(i=0;i<20;i++){
      R=analog(4)+R;
  }
  return (R/20);
}
void fdEnc(int Speed,int ENC){
  encoder(3,0);encoder(2,0);fd(Speed);
  while((encoder(3)<ENC)||(encoder(2)<ENC));
  ao();
}
void bkEnc(int Speed,int ENC){
  encoder(3,0);encoder(2,0);bk(Speed);
  while((encoder(3)<ENC)||(encoder(2)<ENC));
  ao();
}
void slEnc(int Speed,int ENC){
  encoder(3,0);sl(Speed);
  while(encoder(3)<ENC);ao();
}
void tlEnc(int Speed,int ENC){
  encoder(3,0);tl(Speed);
  while(encoder(3)<ENC);ao();
}
void trEnc(int Speed,int ENC){
  encoder(2,0);tr(Speed);
  while(encoder(2)<ENC);ao();
}
void srEnc(int Speed,int ENC){
  encoder(2,0);sr(Speed);
  while(encoder(2)<ENC);ao();
}
void bkWall(int Speed){
  bk(Speed);
  while(1){
   if(!in(8)){
      while(in(9)){
        motor(1,0);motor(2,-100);
          }
      ao();break;
   }
   else if(!in(9)){
     while(in(8)){
        motor(2,0);motor(1,-100);}
      ao();break;
   }
}
}
void sHome(){
  servo(1,170);servo(2,5);
}
void sKeep(){
  servo(1,70);servo(2,95);
}
int readColor(){
  unsigned int R,G,i,C;
  R=0;for(i=0;i<40;i++){R=analog(1)+R;}R=R/40;
  G=0;for(i=0;i<40;i++){G=analog(2)+G;}G=G/40;
  if(R>900){
      if(G>900){C=1;}
      else if(G>400){C=2;}
      else {C=3;}
   }
  else if(R>500){
      if (G>500){C=4;}
      else{C=5;}
      }
  else { C=6;  }
  return C;
}