春ロボ1班(元F3RC4班+)
/
harurobo_main_ver5
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 EC PathFollowing-ver11 CruizCore_R1370P
Diff: main.cpp
- Revision:
- 5:7493649d098b
- Parent:
- 3:e696a6dd4254
- Child:
- 6:14cb400f99f7
--- a/main.cpp Sat Nov 24 14:53:03 2018 +0000
+++ b/main.cpp Wed Nov 28 05:41:58 2018 +0000
@@ -57,7 +57,7 @@
//Ec EC1(PC_6,PC_8,NC,500,0.05);
//Ec EC2(PB_1,PB_12,NC,500,0.05); //Nucleo
-Ec EC1(p21,p22,NC,500,0.05);
+Ec EC1(p21,p22,NC,500,0.05);
Ec EC2(p8,p26,NC,500,0.05); //←mbad
Ticker motor_tick; //角速度計算用ticker
Ticker ticker; //for enc
@@ -125,7 +125,7 @@
d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180); //微小時間毎の座標変化
now_x=now_x+d_x;
now_y=now_y-d_y; //微小時間毎に座標に加算
-
+
}
//ここからそれぞれのプログラム//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -156,7 +156,7 @@
ploty[s] = Y + r * sin(PI - s * (PI*theta/180));
//plotvx[s] = -v * cos(PI - s * (PI*theta/180));
//plotvy[s] = v * sin(PI - s * (PI*theta/180));
- //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
}
while(1) {
@@ -178,7 +178,7 @@
MotorControl(m1,m2,m3,m4); //出力
- 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);
+ //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);
}
@@ -192,7 +192,7 @@
while(1) {
now_angle=gyro.getAngle(); //ジャイロの値読み込み
-
+
calc_xy();
XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
@@ -206,7 +206,144 @@
}
}
-void gogo_straight(double x1_point,double y1_point,double x2_point,double y2_point)//直線運動プログラム（引数：出発地点の座標（x,y）、目標地点の座標(x,y)）
+void purecurve2(int type,double point_x1,double point_y1,double point_x2,double point_y2,int theta/*,double speed,double v*/)
+{
+//正面を変えずに円弧を描いて90°曲がる
+//point_x1,point_y1=出発地点の座標 point_x2,point_x2=目標地点の座標,theta=plotの間隔(0～90°)、v=目標速度
+//type:動きの種類（8パターン)
+
+ int s;
+ int t = 0;
+ double X,Y;//X=楕円の中心座標、Y=楕円の中心座標
+ double a,b; //a=楕円のx軸方向の幅の半分,b=楕円のy軸方向の幅の半分
+ double plotx[(90/theta)+1]; //楕円にとるplotのx座標
+ double ploty[(90/theta)+1];
+ //double plotvx[(90/theta)+1]; //各plotにおける速度
+ //double plotvy[(90/theta)+1];
+ double x_out,y_out,r_out;
+
+ a=fabs(point_x1-point_x2);
+ b=fabs(point_y1-point_y2);
+
+ switch(type) {
+
+ case 1://→↑移動
+ X=point_x1;
+ Y=point_y2;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(-PI/2 + s * (PI*theta/180));
+ ploty[s] = Y + b * sin(-PI/2 + s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 2://↑→移動
+ X=point_x2;
+ Y=point_y1;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(PI - s * (PI*theta/180));
+ ploty[s] = Y + b * sin(PI - s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 3://↑←移動
+ X=point_x2;
+ Y=point_y1;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(s * (PI*theta/180));
+ ploty[s] = Y + b * sin(s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 4://←↑移動
+ X=point_x1;
+ Y=point_y2;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(-PI/2 - s * (PI*theta/180));
+ ploty[s] = Y + b * sin(-PI/2 - s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 5://←↓移動
+ X=point_x1;
+ Y=point_y2;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(PI/2 + s * (PI*theta/180));
+ ploty[s] = Y + b * sin(PI/2 + s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 6://↓←移動
+ X=point_x2;
+ Y=point_y1;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(-s * (PI*theta/180));
+ ploty[s] = Y + b * sin(-s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 7://↓→移動
+ X=point_x2;
+ Y=point_y1;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(PI + s * (PI*theta/180));
+ ploty[s] = Y + b * sin(PI + s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+
+ case 8://→↓移動
+ X=point_x1;
+ Y=point_y2;
+
+ for(s=0; s<((90/theta)+1); s++) {
+ plotx[s] = X + a * cos(PI/2 - s * (PI*theta/180));
+ ploty[s] = Y + b * sin(PI/2 - s * (PI*theta/180));
+ //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]);
+ }
+ break;
+ }
+
+ while(1) {
+ now_angle=gyro.getAngle(); //ジャイロの値読み込み
+
+ calc_xy();
+
+ XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out);
+ CalMotorOut(x_out,y_out,r_out); //move4wheel内のモーター番号定義または成分分解が違うかも？
+ //CalMotorOut(plotvx[t], plotvy[t],0);
+
+ //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);
+ //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
+
+ base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000); //m1～m4に代入
+
+ if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
+ if(t == (90/theta))break;
+
+ MotorControl(m1,m2,m3,m4); //出力
+
+ 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);
+
+ }
+}
+
+
+
+
+void gogo_straight(double x1_point,double y1_point,double x2_point,double y2_point) //直線運動プログラム（引数：出発地点の座標（x,y）、目標地点の座標(x,y)）
{
while (1) {
@@ -226,8 +363,8 @@
base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),1000);
//printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m1,m2,m3,m4);
MotorControl(m1,m2,m3,m4);
-
-
+
+
if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0) break;
@@ -236,7 +373,7 @@
MotorControl(0,0,0,0);
}
-void go_straight(int type,double goal_x,double goal_y,double speed,double front)//移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
+void go_straight(int type,double goal_x,double goal_y,double speed,double front) //移動パターン(1,2,3,4)、目標X、目標Y、最高速度(0~1)、正面角度
{
double y_hosei=(now_y-goal_y)*0.001;//Y座標(mm単位)にP処理
double x_hosei=(now_x-goal_x)*0.001;//X座標(mm単位)にP処理
@@ -244,26 +381,26 @@
switch(type) {
case 1://Y座標一定の正方向横移動
- while(now_x<goal_x){
- base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
+ while(now_x<goal_x) {
+ base(-1-y_hosei-incl_hosei,-1+y_hosei-incl_hosei,1+y_hosei-incl_hosei,1-y_hosei-incl_hosei,speed);
}
break;
-
+
case 2://Y座標一定の負方向横移動
- while(now_x>goal_x){
- base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
+ while(now_x>goal_x) {
+ base(1-y_hosei-incl_hosei,1+y_hosei-incl_hosei,-1+y_hosei-incl_hosei,-1-y_hosei-incl_hosei,speed);
}
break;
-
+
case 3://Y座標一定の正方向横移動
- while(now_y<goal_y){
- base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
+ while(now_y<goal_y) {
+ base(1+x_hosei-incl_hosei,-1+x_hosei-incl_hosei,-1-x_hosei-incl_hosei,1-x_hosei-incl_hosei,speed);
}
break;
-
+
case 4://X座標一定の負方向横移動
- while(now_y>goal_y){
- base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
+ while(now_y>goal_y) {
+ base(-1+x_hosei-incl_hosei,1+x_hosei-incl_hosei,1-x_hosei-incl_hosei,-1-x_hosei-incl_hosei,speed);
}
break;
}
@@ -281,13 +418,14 @@
now_x=start_x;
now_y=start_y;
-
-
- // purecurve(1,1000,0,1000,9,1000);
- // MotorControl(0,0,0,0);
-
-
- //gogo_straight(0,0,1500,0);
+
+ purecurve2(8,0,0,1000,-1000,9);
+ //purecurve2(8,0,0,1000,-500,9);
+ //purecurve(1,1000,0,1000,9,1000);
+ // MotorControl(0,0,0,0);
+
+
+ //gogo_straight(0,0,1500,0);
}
///////////////////////////////////////////////////////////////////////以下マクソン関連///////////////////////////////////////////////////////////////////////////
@@ -315,7 +453,7 @@
q_setPDparam(0.1,0.1); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数
r_setPDparam(10,0.1); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
set_r_out(500); //旋回時の最大出力値設定関数
- // set_target_angle(0); //機体目標角度設定関数
+ // set_target_angle(0); //機体目標角度設定関数
#ifdef DEBUG_MODE
debug_led = 1;
@@ -334,7 +472,7 @@
#define MCP4922_SET_OUTB (0xB000) //( MCP4922_AB || MCP4922_GA || MCP4922_SHDN ) //45056
#define MCP4922_MASKSET (0x0FFF) //4095
-void DAC_Write(int16_t data, DigitalOut* DAC_cs) //(出力,出力場所)
+void DAC_Write(int16_t data, DigitalOut* DAC_cs) //(出力,出力場所)
{
static uint16_t dataA; //送るデータ
static uint16_t dataB;
@@ -371,7 +509,7 @@
}
-void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力
+void MotorControl(int16_t val_md1, int16_t val_md2, int16_t val_md3, int16_t val_md4) //出力
{
static int16_t zero_check;