Diff: temp.cpp

Revision:

0:c2262bac9aa6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/temp.cpp	Thu Nov 13 15:33:47 2014 +0000
@@ -0,0 +1,320 @@
+
+
+            //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);
+        }
+    }
+}
+*/
\ No newline at end of file