File content as of revision 0:c2262bac9aa6:

            //case  7: QZWPWrite(); break;
/*
 
// file out QZSS way point, 4 point only
void writeQZWp4(){
    FILE *fp = fopen("/local/QZpoint4.txt","w");  
        fprintf(fp, "%f\n",wyQ[1][1]);
        fprintf(fp, "%f\n",wxQ[1][1]);
        fprintf(fp, "%f\n",wyQ[7][1]);
        fprintf(fp, "%f\n",wxQ[7][1]);
        fprintf(fp, "%f\n",wyQ[1][7]);
        fprintf(fp, "%f\n",wxQ[1][7]);
        fprintf(fp, "%f\n",wyQ[7][7]);
        fprintf(fp, "%f\n",wxQ[7][7]);        
    fclose(fp);
    lcd.locate(0,1); lcd.printf("Completed!      "); 
    Thread::wait(1000);     
}    
    
// file read QZSS way point, 4 point only
void readQZWp4(){
     char st[20];
    FILE *fp = fopen("/local/QZpoint4.txt","r");                      
        fgets(st,20,fp); wyQ[1][1]=atof(st); 
        fgets(st,20,fp); wxQ[1][1]=atof(st); 
        fgets(st,20,fp); wyQ[7][1]=atof(st); 
        fgets(st,20,fp); wxQ[7][1]=atof(st); 
        fgets(st,20,fp); wyQ[1][7]=atof(st); 
        fgets(st,20,fp); wxQ[1][7]=atof(st); 
        fgets(st,20,fp); wyQ[7][7]=atof(st); 
        fgets(st,20,fp); wxQ[7][7]=atof(st); 
    fclose(fp);
    lcd.locate(0,1); lcd.printf("Completed!      "); 
    Thread::wait(1000); 
    calcQZWp();   
}

*/


/*
// calculation of all QZSS waypoint from 4 point, old version
void calcQZWp(){

//  xQ11=-0.7057; yQ11=3.5358; 
//  xQ71=7.78; yQ71=12.01; 
//  xQ17=-13.42; yQ17=16.26; 
   
  xQ11=wxQ[1][1]; yQ11=wyQ[1][1]; // xQ11=wxQ11(A1,B1), yQ11=wyQ11(A1,B1) 
  xQ71=wxQ[7][1]; yQ71=wyQ[7][1]; // xQ71=wxQ71(A7,B1), yQ71=wyQ71(A7,B1)
  xQ17=wxQ[1][7]; yQ17=wyQ[1][7]; // xQ17=wxQ17(A1,B7), yQ17=wyQ17(A1,B7)  
  xQ77=wxQ[7][7]; yQ77=wyQ[7][7];  
    
  dxQ71=xQ71-xQ11; dyQ71=yQ71-yQ11;
  dxQ17=xQ17-xQ11; dyQ17=yQ17-yQ11;
  
  theta=atan(dxQ71/dyQ71);
  magx=sqrt(dxQ71*dxQ71+dyQ71*dyQ71)/6;
  magy=sqrt(dxQ17*dxQ17+dyQ17*dyQ17)/6;
  
  for (int i=1;i<8;i++){
    for(int j=1;j<8;j++){
      wxQ[i][j]=magx*i*cos(theta)-magy*j*sin(theta);
      wyQ[i][j]=magx*i*sin(theta)+magy*j*cos(theta);
    }
  }  
}
*/

/*
// QZSS wp  wirte
void QZWPWrite(){
    int i;
    for(i=1;i<8;i++){
       wxQ[i][1]= 47.6213;
    }
    for(i=1;i<8;i++){
       wxQ[i][2]= 47.621483;
    }
    for(i=1;i<8;i++){
       wxQ[i][3]= 47.621666;
    }
    for(i=1;i<8;i++){
       wxQ[i][4]= 47.62185;
    }
    for(i=1;i<8;i++){
       wxQ[i][5]= 47.622033;
    }
    for(i=1;i<8;i++){
       wxQ[i][6]= 47.622166;
    }
    for(i=1;i<8;i++){
       wxQ[i][7]= 47.6224;
    }
    
    for(i=1;i<8;i++){
       wyQ[1][i]= 39.9302;
    }
    for(i=1;i<8;i++){
       wyQ[2][i]= 39.39183;
    }
    for(i=1;i<8;i++){
       wyQ[3][i]= 39.93347;
    }
    for(i=1;i<8;i++){
       wyQ[4][i]= 39.9351;
    }
    for(i=1;i<8;i++){
       wyQ[5][i]= 39.93673;
    }
    for(i=1;i<8;i++){
       wyQ[6][i]= 39.93837;
    }
    for(i=1;i<8;i++){
       wxQ[7][i]= 39.94;
    }    
    lcd.locate(0,1); lcd.printf("Completed!      "); 
    Thread::wait(1000); 
}

*/


/*
void countR(){
    float tmp; 
    cntR++; 
    tmp=timer.read_ms();
    spdR=30/(tmp-timR);  // m/s ?
    timR=tmp;
}
void countL(){
    float tmp; 
    cntL++; 
    tmp=timer.read_ms();
    spdL=30/(tmp-timL);  // m/s ?
    timL=tmp;
}

*/

/*
#define DEF_WR      2                   // ウェイポイントの半径
#define DEF_KS      0.1                 // ステアリング切れ角比例係数
#define DEF_STRMIN  0.2               // ステアリング左最大切れ角
#define DEF_STRMAX  0.8               // ステアリング右最大切れ角
#define DEF_STRN    0.5             // ステアリング停止時の中立値
#define DEF_SPD     0.2              // 走行時のモータの値

    // resetを押しながらリセットをかけた場合は，パラメータをデフォルト値に戻す
    if (resetSW==1){    
        readParam();
    } else {                        // パラメータをデフォルト値に戻す
        wr=DEF_WR;          
        ks=DEF_KS;
        strmin=DEF_STRMIN; 
        strmax=DEF_STRMAX;
        strn=DEF_STRN;
        spd=DEF_SPD;
        tNum=DEF_TNUM;
        writeParam();
    }

*/


//long wp=4;  // wp:0-15まで,ウェイポイントの数 -1
//double wy[]={0,111.111, 222.222, 333.333};
//double wx[]={0,123.456, 234.567, 345.678};

/*
//-------------------------------------------------------------------------- 
// ウェイポイントの読み出し、表示。GPSのジャンパーを外して、シリアルポートに接続
//--------------------------------------------------------------------------
void wpOut(){
  while(resetSW==1){
    pc.printf("wp= %d\n",wp);
    for(int i=0;i<16;i++){
      pc.printf("%d,  %f, %f\n",i, wy[i],wx[i]);
    }
    Thread::wait(1000); 
  }
}

//-------------------------------------------------------------------------- 
// パラメータのシリアル出力
//-------------------------------------------------------------------------- 
void paramOut(){
  while(resetSW==1){
    pc.printf("wr    =%f\n", wr); 
    pc.printf("ks    =%f\n", ks); 
    pc.printf("strmin=%f\n", strmin); 
    pc.printf("strmax=%f\n", strmax); 
    pc.printf("strn  =%d\n", strn); 
    pc.printf("spd   =%d\n", spd); 
    Thread::wait(1000); 
  }
}
*/   


    //writePoint();
    
/*
    printf("%d\n",wp);
    for(int i=0;i<wp;i++){
        printf("%f\n",wy[i]);
        printf("%f\n",wx[i]);
    }
*/

/*
//--------------------------------------------------------------------------
//  シリアルポートで文字列受信
//--------------------------------------------------------------------------
void recvStr(char *buf)
{
  int i = 0;
  char c;
  while (1) {
     if (Serial.available()) {
      c = Serial.read();
      buf[i] = c;
      if (c == '\n') break; 
      i++;
    }
  }
  buf[i] = '\0';  // \0: end of string
}


//--------------------------------------------------------------------------
//  GPS受信                                                         
//--------------------------------------------------------------------------
int recvGPS(){
  if (Serial.available()) {  
    recvStr(str);   
    if(strcmp(strtok(str,","),"$GPRMC")==0){   //if RMC line
      strtok(NULL,",");
      strtok(NULL,",");
      latitude=strtok(NULL,","); //get latitude
      strtok(NULL,",");
      longitude=strtok(NULL,","); //get longtude
      strtok(NULL,",");          // E読み飛ばし
      knot=strtok(NULL,",");     // 速度 get
      direct=strtok(NULL,",");   // 進行方向 get 
      
      gps_val();        //  GPS信号の数値返還             
      gps_cal();       // 現在地とウェイポイントとの関係を計算  
      return(1);
    }
  }
  return(0);
}
*/


            //lcd.cls();
            //lcd.printf("%f \n", latit);
            //lcd.printf("%f \n", longit);
            //lcd.printf("%f %f", latit, longit);
            //printf("%s\n", str);

/*  
    while(1){
       if(SW1==0){
           lcd.cls();
           lcd.printf("#S1#  ----  ----");
           servoC1 = POSC;
           servoC2 = POSC;
       }
       else if(SW2==0){
           lcd.cls();
           lcd.printf("----  #S2#  ----");
           servoC1 = POSC+0.2;
           servoC2 = POSC+0.2;
       }
       if(SW3==0){
           lcd.cls();
           lcd.printf("----  ----  #S3#");
           servoC1 = POSC-0.2;
           servoC2 = POSC-0.2;
       }
        // for usb
        // led=!led;
        // Thread::wait(500);
   }
   */
   
   
/*
void serial_task(void const*) {
    USBHostSerial serial;
    
    while(1) {
    
        // try to connect a serial device
        while(!serial.connect())
            Thread::wait(500);
        
        // in a loop, print all characters received
        // if the device is disconnected, we try to connect it again
        while (1) {
        
            // if device disconnected, try to connect it again
            if (!serial.connected())
                break;

            // print characters received
            while (serial.available()) {
                printf("%c", serial.getc());
            }
            
            Thread::wait(50);
        }
    }
}
*/