Diff: movement/movement.cpp

Revision:

1:d66ba60fa9a9

Parent:

0:0727df1dfa3e

--- a/movement/movement.cpp	Wed Jun 12 09:41:08 2019 +0000
+++ b/movement/movement.cpp	Sat Jun 15 04:46:41 2019 +0000
@@ -420,7 +420,7 @@
         if(t == (90/theta))break;
     }
 }
-/*
+
 double spline_base(int i,int k,double t,int nv[])  //スプライン基底関数を求める関数
 {
 // i:0~(制御点の個数-1)
@@ -434,10 +434,12 @@
         else return 0.0;
     } else {
         if((nv[i+k]-nv[i+1])!=0) {
-            w1=((nv[i+k]-t)/(nv[i+k]-nv[i+1])) * baseN(i+1,k-1,t,nv);
+            w1=((nv[i+k]-t)/(nv[i+k]-nv[i+1])) * spline_base(i+1,k-1,t,nv);
+            //printf("%f\n\r",w1);
         }
         if((nv[i+k-1]-nv[i]) !=0 ) {
-            w2=((t-nv[i])/(nv[i+k-1]-nv[i])) * baseN(i,k-1,t,nv);
+            w2=((t-nv[i])/(nv[i+k-1]-nv[i])) * spline_base(i,k-1,t,nv);
+            //printf("%f\n\r",w2);
         }
         return (w1+w2);
     }
@@ -446,9 +448,10 @@
 void spline_curve(double st_x,double st_y,//スプライン曲線を描いて曲線移動
                   double end_x,double end_y,
                   double cont1_x,double cont1_y,
-                  double cont2_x,double cont2_x,
-                  int num){ 
-                   
+                  double cont2_x,double cont2_y,
+                  int num)
+{
+
 // st_x,st_y:出発座標
 // end_x,end_y:目標座標
 // cont1_x,cont1_y:制御点１座標(出発座標に近い方)
@@ -456,7 +459,7 @@
 // num:経路を何等分するか
 
 
-/*-------------------------------------------------*/
+    /*-------------------------------------------------*/
 // ※以下のような経路をスプライン曲線で移動すことを想定
 //
 //    （start）→ → → →
@@ -464,20 +467,73 @@
 //                      ↘
 //                        → → → →(goal)
 //
-/*-------------------------------------------------*/
-/*
-    double nt[] = {0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0}; //ノットベクトル
+    /*-------------------------------------------------*/
+
+    int nt[] = {0, 0, 0, 1, 2, 2, 2}; //ノットベクトル
+    //double nt[] = {0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0};
     double plotx[num+1];  //楕円にとるplotのx座標
     double ploty[num+1];
-    int i;
+    double value_t;
+    int i, j;
+    
+    /*for(i = 0; i < 7; i++){
+     printf("not_V = %f\n\r",nt[i]);   
+    }*/
     
     for(i = 0; i < num+1; i++){
-        plotx[i] = 
-        
+        plotx[i] = 0.0;
+        ploty[i] = 0.0;   
+    }
+
+    for(i = 0; i < num+1; i++) {
+        value_t = (double)i / num;
+        for(j = 0; j < 4; j++) {
+            if(j == 0) {
+                plotx[i] += st_x * spline_base(j,2,value_t,nt);
+                ploty[i] += st_y * spline_base(j,2,value_t,nt);
+            } else if(j == 1) {
+                plotx[i] += cont1_x * spline_base(j,2,value_t,nt);
+                ploty[i] += cont1_y * spline_base(j,2,value_t,nt);
+            } else if(j == 2) {
+                plotx[i] += cont2_x * spline_base(j,2,value_t,nt);
+                ploty[i] += cont2_y * spline_base(j,2,value_t,nt);
+            } else if(j == 3) {
+                plotx[i] += end_x * spline_base(j,2,value_t,nt);
+                ploty[i] += end_y * spline_base(j,2,value_t,nt);
+            }
+        }
+        printf("plot_x = %f, plot_y = %f\n\r",plotx[i],ploty[i]);
     }
 
+    while(1);
+  /*  while(1) {
+
+        calc_xy(target_angle,u,v);
+
+        XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed,speed);
+        CalMotorOut(x_out,y_out,r_out);
+        //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out);
+
+        base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);  //m1～m4に代入
+        //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
+
+        if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
+
+#ifdef MAXON_MOTOR_MODE
+        MaxonControl(m1,m2,m3,m4);
+#endif
+
+#ifdef DC_MOTOR_MODE
+        DCControl(m1,m2,m3,m4);
+#endif
+
+        //debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle);
+
+        if(t == num)break;
+    }*/
+
 }
-*/
+
 void straight(double u,double v,                //直線運動プログラム
               double x1_point,double y1_point,
               double x2_point,double y2_point,