ロボステ6期
/
control_UC
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 1:d66ba60fa9a9, committed 2019-06-15
- Comitter:
- yuki0701
- Date:
- Sat Jun 15 04:46:41 2019 +0000
- Parent:
- 0:0727df1dfa3e
- Commit message:
- a
Changed in this revision
--- a/main.cpp Wed Jun 12 09:41:08 2019 +0000 +++ b/main.cpp Sat Jun 15 04:46:41 2019 +0000 @@ -95,8 +95,10 @@ /*-------------------------------------------試走用プログラム-----------------------------------------------*/ #ifdef NHK2020_TEST_MODE - straight(1,1,0,0,1000,0,5000,5000,5,0.1,10,0.1,600,0); - MaxonControl(0,0,0,0); + //spline_curve(0,0,100,50,30,0,70,50,10); + spline_curve(0,0,100,50,30,0,70,50,10); + //straight(1,1,0,0,1000,0,5000,5000,5,0.1,10,0.1,600,0); + //MaxonControl(0,0,0,0); #endif /*-------------------------------------------------------------------------------------------------------*/
--- 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,
--- a/movement/movement.h Wed Jun 12 09:41:08 2019 +0000
+++ b/movement/movement.h Sat Jun 15 04:46:41 2019 +0000
@@ -32,23 +32,32 @@
void enc_correction(int x_select,int y_select);
void ellipse_curve(int type,double u, double v, //正面を変えずに円弧or楕円を描いて曲がる
- double point_x1,double point_y1,
- double point_x2,double point_y2,
- int theta,
- double speed,
- double q_p,double q_d,
- double r_p,double r_d,
- double r_out_max,
- double target_angle);
-
-void straight(double u, double v, double x1_point,double y1_point, //直線運動プログラム
- double x2_point,double y2_point,
- double speed1,double speed2,
+ double point_x1,double point_y1,
+ double point_x2,double point_y2,
+ int theta,
+ double speed,
double q_p,double q_d,
double r_p,double r_d,
double r_out_max,
double target_angle);
+
+double spline_base(int i,int k,double t,double nv[]);
+
+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_y,
+ int num);
+
+void straight(double u, double v, double x1_point,double y1_point, //直線運動プログラム
+ double x2_point,double y2_point,
+ double speed1,double speed2,
+ double q_p,double q_d,
+ double r_p,double r_d,
+ double r_out_max,
+ double target_angle);
+
void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v);
#endif
\ No newline at end of file