ロボステ6期
あ
Dependencies: uw_28015 mbed move4wheel2 EC CruizCore_R6093U CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 1:26fc1b2f1c42
- Parent:
- 0:b87fd8dd4322
- Child:
- 2:820dcd23c8e3
--- a/movement/movement.cpp Tue Mar 19 13:20:23 2019 +0000
+++ b/movement/movement.cpp Sat Nov 16 06:26:57 2019 +0000
@@ -5,21 +5,28 @@
#include "math.h"
#include "PathFollowing.h"
#include "movement.h"
-#include "maxonsetting.h"
#include "manual.h"
#include "can.h"
+#include "R6093U.h"
#define PI 3.141592
char can_ashileddata[2]= {0};
+char can_ashileddata2[8]= {0};
+//char can_ashileddata3[2]= {0};
+//char can_ashileddata4[2]= {0};
+//char can_ashileddata5[2]= {0};
+
int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
-Ec EC1(p22,p21,NC,500,0.05);
-Ec EC2(p26,p8,NC,500,0.05);
+Ec EC2(p16,p15,NC,2048,0.05);
+Ec EC1(p18,p17,NC,500,0.05);
Ticker ec_ticker; //ec角速度計算用ticker
-R1370P gyro(p28,p27);
+//R1370P gyro(p9,p10);
+
+R6093U gyro(p9,p10);
double new_dist1=0,new_dist2=0;
double old_dist1=0,old_dist2=0;
@@ -35,6 +42,8 @@
int flag;
+int RL_mode;
+
///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言/////////////////
/*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する
@@ -66,10 +75,24 @@
gyro.initialize();
ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
- EC1.setDiameter_mm(25.5);
- EC2.setDiameter_mm(25.5); //測定輪半径//後で測定
- info.nowX.enc = -2962; //初期位置の設定
+ EC1.setDiameter_mm(70);
+ EC2.setDiameter_mm(70); //測定輪半径//後で測定
+ info.nowX.enc = 0; //初期位置の設定
+ info.nowY.enc = 0;
+}
+
+void UserLoopSetting_enc_right()
+{
+ info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド)
info.nowY.enc = 3500;
+ RL_mode = 0;
+}
+
+void UserLoopSetting_enc_left()
+{
+ info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド)
+ info.nowY.enc = 3500;
+ RL_mode = 1;
}
void calOmega() //角速度計算関数
@@ -103,7 +126,6 @@
void ashi_led()
{
-
if(now_angle > -1 && now_angle < 1) {
can_ashileddata0_0 = 1;
} else {
@@ -116,16 +138,30 @@
can_ashileddata0_1 = 0;
}
- if(now_x > -1 && now_x < 1) {
- can_ashileddata0_2 = 1;
- } else {
- can_ashileddata0_2 = 0;
- }
+ if(RL_mode == 0) {
+ if(now_x > 3110 && now_x < 3114) {
+ can_ashileddata0_2 = 1;
+ } else {
+ can_ashileddata0_2 = 0;
+ }
- if(now_y > -1 && now_y < 1) {
- can_ashileddata0_3 = 1;
- } else {
- can_ashileddata0_3 = 0;
+ if(now_y > 3498 && now_y < 3502) {
+ can_ashileddata0_3 = 1;
+ } else {
+ can_ashileddata0_3 = 0;
+ }
+ } else if(RL_mode == 1) {
+ if(now_x > -3114 && now_x < -3110) {
+ can_ashileddata0_2 = 1;
+ } else {
+ can_ashileddata0_2 = 0;
+ }
+
+ if(now_y > 3498 && now_y < 3502) {
+ can_ashileddata0_3 = 1;
+ } else {
+ can_ashileddata0_3 = 0;
+ }
}
can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4);
@@ -133,12 +169,22 @@
void calc_gyro()
{
- now_angle=gyro.getAngle(); //ジャイロの値読み込み
+ //now_angle=gyro.getAngle(); //ジャイロの値読み込み
+ now_angle = -gyro.getZ_Angle();
+}
+
+void print_gyro()
+{
+ while(1) {
+ //printf("now_gyro = %f\n\r",-gyro.getAngle());
+ }
+
}
void calc_xy_enc() //エンコーダ&ジャイロによる座標計算
{
- now_angle=gyro.getAngle(); //ジャイロの値読み込み
+ //now_angle=gyro.getAngle(); //ジャイロの値読み込み
+ now_angle = -gyro.getZ_Angle();
new_dist1=EC1.getDistance_mm();
new_dist2=EC2.getDistance_mm();
@@ -176,7 +222,8 @@
double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
double D1=30,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離（時計回りを正とする）
- now_angle=gyro.getAngle(); //ジャイロの値読み込み
+// now_angle=gyro.getAngle(); //ジャイロの値読み込み
+ now_angle = -gyro.getZ_Angle();
if(tgt_angle==0) {
if((xy_type==0 || xy_type==2) && pm_typeX==0) {
@@ -440,8 +487,8 @@
if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++;
- MaxonControl(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);
+// MaxonControl(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);
if(t == (90/theta))break;
}
@@ -471,7 +518,7 @@
calc_xy(target_angle,u,v);
XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2);
- //printf("x = %f, y = %f,angle = %f,x_out=%lf, y_out=%lf, r_out=%lf\n\r",now_x,now_y,now_angle,x_out, y_out,r_out);
+ //printf("n_x = %f, n_y = %f,n_angle = %f, t_x = %f, t_y = %f, t_angle = %f, x_out=%lf, y_out=%lf, r_out=%lf\n\r",now_x,now_y,now_angle,x2_point,y2_point,target_angle,x_out, y_out,r_out);
CalMotorOut(x_out,y_out,r_out);
//printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3));
@@ -479,8 +526,9 @@
base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095);
//printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4);
- MaxonControl(m1,m2,m3,m4);
- debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
+// MaxonControl(m1,m2,m3,m4);
+// debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
+ printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
}
@@ -530,7 +578,7 @@
{
//距離に比例させて補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする）
- double first_speed, first_speed50 = 150, last_speed = 25, Max_speed = 500;
+ double first_speed, first_speed50 = 10, last_speed = 10, Max_speed = 500;
double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
double out;
@@ -539,7 +587,7 @@
//r = hypot(now_x - tgt_x, now_y - tgt_y);
while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
-
+ //printf("col\n\n\n");
if(id1_value[0] != 1)break;
if(id1_value[6] != flag)break;
@@ -551,9 +599,16 @@
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed,last_speed,5,0.1,10,0.1,500,tgt_angle);
}*/
- gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
+ //gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
+ gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,10,10,5,0.1,10,0.1,50,tgt_angle);
+ //gogo_straight(1,1,0,0,200,0,50,500,5,0.1,10,0.1,50,0);
+ //gogo_straight(u,v,now_x,now_y,0,100,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
- MaxonControl(0,0,0,0);
+// MaxonControl(0,0,0,0);
+ m1 = 0;
+ m2 = 0;
+ m3 = 0;
+ m4 = 0;
calc_xy(tgt_angle, u, v);
@@ -568,20 +623,47 @@
if(id1_value[6] != flag)break;
//calc_gyro();
- now_angle=gyro.getAngle();
+// now_angle=gyro.getAngle();
+ now_angle = -gyro.getZ_Angle();
printf("angle = %f\n\r",now_angle);
out = 10 * (tgt_angle - now_angle);
if(out > 300) { //0～179°のときは時計回りに回転
- MaxonControl(-300,-300,-300,-300);
+// MaxonControl(-300,-300,-300,-300);
+ m1 = -300;
+ m2 = -300;
+ m3 = -300;
+ m4 = -300;
+
} else if(out < -300) {
- MaxonControl(300,300,300,300);
+// MaxonControl(300,300,300,300);
+ m1 = 300;
+ m2 = 300;
+ m3 = 300;
+ m4 = 300;
} else if(out <= 300 && out > -300) {
- MaxonControl(-out,-out,-out,-out);
+// MaxonControl(-out,-out,-out,-out);
+ m1 = -out;
+ m2 = -out;
+ m3 = -out;
+ m4 = -out;
}
if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
}
- MaxonControl(0,0,0,0);
+// MaxonControl(0,0,0,0);
+ m1 = 0;
+ m2 = 0;
+ m3 = 0;
+ m4 = 0;
+}
+
+void mt_stop()
+{
+ m1 = 0;
+ m2 = 0;
+ m3 = 0;
+ m4 = 0;
+ printf("motor stop\n\r");
}
\ No newline at end of file