2015a/Member/nttk/Mission3 の履歴ソース(No.2)

• 履歴一覧
• 差分 を表示
• 現在との差分 を表示
• 2015a/Member/nttk/Mission3 へ行く。
  • 1 (2015-07-24 (金) 08:43:01)
  • 2 (2015-07-24 (金) 08:48:49)

[[2015a/MemberOnly]]
**プログラムの説明 [#m117015b]
#define SPEED 50
#define SPEED_SLOW 30
#define shoot OnRev(OUT_A,10);Wait(500);OnFwd(OUT_BC,SPEED);Wait(1000);

const float diameter=5.45;
const float track=10.35;
const float pi=3.1415;

void fwdDist(float d)
{
    long angle;
    angle=d/(diameter*pi)*360.0;
    RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);
  }
 void turnAng(long ang)
 {
  long angle;
  angle=track/diameter*ang;
  RotateMotorEx(OUT_BC,SPEED_SLOW,angle,100,true,true);
}
  int searchDirection(long ang)
{
  long angle,tacho_min=0, tacho_corr;
  int d_min;
  d_min=300;

  angle = (track/diameter)*ang;
  turnAng(ang/2);
  ResetTachoCount(OUT_BC);
  OnFwdSync(OUT_BC,SPEED_SLOW,-100);
  while(MotorTachoCount(OUT_B)<=angle){
     if(SensorUS(S1)<d_min){
         d_min=SensorUS(S1);
         tacho_min=MotorTachoCount(OUT_B);
     }
  }
  OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
  until(MotorTachoCount(OUT_B)<=tacho_min||SensorUS(S1)<=d_min);
 
 Wait(14);
 Off(OUT_BC);Wait(500);
 return d_min;
}

task main()
{
  SetSensorLowspeed(S1);

  int d = searchDirection(120);
  if (d > 25){
      fwdDist(d-25.0);
   shoot;
   }
}