春ロボ1班(元F3RC4班+)
3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 6:26724c287387
- Parent:
- 4:317c53a674fa
- Child:
- 7:44ce34007499
--- a/movement/movement.cpp Sat Mar 02 07:18:38 2019 +0000
+++ b/movement/movement.cpp Sat Mar 02 07:48:18 2019 +0000
@@ -11,10 +11,12 @@
#define PI 3.141592
-char ashi_data[4]={0};
+char can_ashileddata[1]= {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 EC1(p22,p21,NC,500,0.05);
+
Ticker ec_ticker; //ec角速度計算用ticker
R1370P gyro(p28,p27);
@@ -40,25 +42,26 @@
*info.nowY.usw → 超音波センサーにより求めた機体位置のy座標
*/
-typedef struct{ //使用センサーの種類
+typedef struct { //使用センサーの種類
double usw; //超音波センサー
double enc; //エンコーダ
double gyro; //ジャイロ
//double line;//ラインセンサー
-}robo_sensor;
+} robo_sensor;
-typedef struct{ //機体情報の種類
+typedef struct { //機体情報の種類
robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも
robo_sensor nowX;
robo_sensor nowY;
-}robo_data;
+} robo_data;
-robo_data info={{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
+robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void UserLoopSetting_sensor(){
-
+void UserLoopSetting_sensor()
+{
+
gyro.initialize();
ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
EC1.setDiameter_mm(25.5);
@@ -91,40 +94,43 @@
else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL));
else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR));
else output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR));
- } else {
+ } else {
output(FL,BL,BR,FR);
}
}
-void ashi_led(){
-
- if(now_angle > -1 && now_angle < 1){
- ashi_data[0] = 1;
- }else{
- ashi_data[0] = 0;
+void ashi_led()
+{
+
+ if(now_angle > -1 && now_angle < 1) {
+ can_ashileddata0_0 = 1;
+ } else {
+ can_ashileddata0_0 = 0;
}
-
- if(now_angle > 350){
- ashi_data[1] = 1;
- }else{
- ashi_data[1] = 0;
+
+ if(now_angle > 350) {
+ can_ashileddata0_1 = 1;
+ } else {
+ can_ashileddata0_1 = 0;
}
-
- if(now_x > -1 && now_x < 1){
-
- ashi_data[2] = 1;
- }else{
- ashi_data[2] = 0;
+
+ if(now_x > -1 && now_x < 1) {
+ can_ashileddata0_2 = 1;
+ } else {
+ can_ashileddata0_2 = 0;
}
-
- if(now_y > -1 && now_y < 1){
- ashi_data[3] = 1;
- }else{
- ashi_data[3] = 0;
+
+ if(now_y > -1 && now_y < 1) {
+ 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);
}
-void calc_gyro(){
+void calc_gyro()
+{
now_angle=gyro.getAngle(); //ジャイロの値読み込み
}
@@ -145,109 +151,112 @@
info.nowY.enc = info.nowY.enc - d_y; //微小時間毎に座標に加算
}
-void set_cond(int t, int px, double bx, int py, double by){ //超音波センサーを使用するときの条件設定関数
+void set_cond(int t, int px, double bx, int py, double by) //超音波センサーを使用するときの条件設定関数
+{
//引数の詳細は関数"calc_xy_usw"参照
-
- xy_type = t;
-
- pm_typeX = px;
- x_base = bx;
-
- pm_typeY = py;
- y_base = by;
+
+ xy_type = t;
+
+ pm_typeX = px;
+ x_base = bx;
+
+ pm_typeY = py;
+ y_base = by;
}
-void calc_xy_usw(double tgt_angle){ //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+void calc_xy_usw(double tgt_angle) //超音波センサーによる座標計算（機体が旋回する場合はこの方法による座標計算は出来ない）
+{
//tgt_angle:機体の目標角度（運動初期角度と同じ/今大会では0,90,180のみ）
//xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む）
//pm_typeX,pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む）
//x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
-
+
double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
-
+
now_angle=gyro.getAngle(); //ジャイロの値読み込み
-
- if(tgt_angle==0){
- if((xy_type==0 || xy_type==2) && pm_typeX==0){
-
- info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-
- }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-
- info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-
- }
- if((xy_type==1 || xy_type==2) && pm_typeY==0){
-
- info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-
- }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-
- info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-
- }
-
- }else if(tgt_angle==90){
- if((xy_type==0 || xy_type==2) && pm_typeX==0){
-
- info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-
- }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-
- info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-
- }
- if((xy_type==1 || xy_type==2) && pm_typeY==0){
-
- info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-
- }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-
- info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-
- }
-
- }else if(tgt_angle==180){
- if((xy_type==0 || xy_type==2) && pm_typeX==0){
-
- info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
-
- }else if((xy_type==0 || xy_type==2) && pm_typeX==1){
-
- info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
-
- }
- if((xy_type==1 || xy_type==2) && pm_typeY==0){
-
- info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
-
- }else if((xy_type==1 || xy_type==2) && pm_typeY==1){
-
- info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
-
- }
+
+ if(tgt_angle==0) {
+ if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+ info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+ } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+ info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+ }
+ if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+ info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+ } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+ info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+ }
+
+ } else if(tgt_angle==90) {
+ if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+ info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+ } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+ info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+ }
+ if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+ info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+ } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+ info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+ }
+
+ } else if(tgt_angle==180) {
+ if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+ info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+
+ } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+ info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+
+ }
+ if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+ info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+
+ } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+ info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+
+ }
}
}
-void calc_xy(double target_angle, double u,double v){
+void calc_xy(double target_angle, double u,double v)
+{
//エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する
-
- calc_xy_enc();
-
- if(u != 1 || v != 1){
- calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
- }
-
+
+ calc_xy_enc();
+
+ if(u != 1 || v != 1) {
+ calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。
+ }
+
now_x = u * info.nowX.enc + (1-u) * info.nowX.usw;
now_y = v * info.nowY.enc + (1-v) * info.nowY.usw;
-
+
/*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){ //スタート時の0合わせ用
ec_led = 1;
}else{
ec_led = 0;
}
-
+
if(now_angle >-0.5 && now_angle <0.5){
gyro_led = 1;
}else{
@@ -391,12 +400,12 @@
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++;
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;
if(id1_value[0] != 1)break;
}
@@ -417,7 +426,7 @@
r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数
set_r_out(r_out_max); //旋回時の最大出力値設定関数
set_target_angle(target_angle); //機体目標角度設定関数
-
+
while (1) {
calc_xy(target_angle,u,v);
@@ -430,50 +439,51 @@
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);
if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
- if(id1_value[0] != 1)break;
+ if(id1_value[0] != 1)break;
}
}
-void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v){ //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
-
+void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
+{
+
double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
double out;
-
+
calc_xy(tgt_angle, u, v);
-
- while(1){ //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+ while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,5,0.1,10,0.1,500,tgt_angle);
MaxonControl(0,0,0,0);
-
+
calc_xy(tgt_angle, u, v);
-
+
r=hypot(now_x - tgt_x, now_y - tgt_y);
-
+
if(r < R) break;
-
+
}
-
- while(1){
-
+
+ while(1) {
+
calc_gyro();
-
+
out = 10 * (tgt_angle - now_angle);
-
+
if(out > 300) { //0～179°のときは時計回りに回転
MaxonControl(300,300,300,300);
- }else if(out < -300){
- MaxonControl(-300,-300,-300,-300);
- }else if(out <= 300 && out > -300) {
+ } else if(out < -300) {
+ MaxonControl(-300,-300,-300,-300);
+ } else if(out <= 300 && out > -300) {
MaxonControl(out,out,out,out);
}
-
+
if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
-
+
}
MaxonControl(0,0,0,0);
}
\ No newline at end of file