2018_Project-R / Mbed 2 deprecated PS3_BlueUSB_downstate

PS3下の階

Dependencies:   FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed

Fork of PS3_BlueUSB by Bart Janssens

omni.cpp@1:b2063ffa4927, 2018-01-11 (annotated)

Committer:
e5115026
Date:
Thu Jan 11 07:47:26 2018 +0000
Revision:
1:b2063ffa4927
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5115026 1:b2063ffa4927 1
e5115026 1:b2063ffa4927 2 #include "mbed.h"
e5115026 1:b2063ffa4927 3 #include "RoboClaw.h"
e5115026 1:b2063ffa4927 4 #include "QEI.h"
e5115026 1:b2063ffa4927 5
e5115026 1:b2063ffa4927 6 //位置PID
e5115026 1:b2063ffa4927 7 #define Kp 100
e5115026 1:b2063ffa4927 8 #define Ki 7
e5115026 1:b2063ffa4927 9 #define Kd 0
e5115026 1:b2063ffa4927 10
e5115026 1:b2063ffa4927 11 //回転角度PID
e5115026 1:b2063ffa4927 12 #define a_Kp 100
e5115026 1:b2063ffa4927 13 #define a_Ki 7
e5115026 1:b2063ffa4927 14 #define a_Kd 0
e5115026 1:b2063ffa4927 15
e5115026 1:b2063ffa4927 16 //オドメトリ用輪(mm
e5115026 1:b2063ffa4927 17 #define omni1 40.9401
e5115026 1:b2063ffa4927 18 #define omni2 41.0987
e5115026 1:b2063ffa4927 19 #define omni3 40.6526
e5115026 1:b2063ffa4927 20
e5115026 1:b2063ffa4927 21 #define radius 101.6 //駆動輪直径(mm
e5115026 1:b2063ffa4927 22 #define gaisetuen 121.24355652982 //オドメトリ用車輪の外接円
e5115026 1:b2063ffa4927 23 #define Pi 3.141592 //円周率
e5115026 1:b2063ffa4927 24 #define dt 0.001 //動作周期時間(s)
e5115026 1:b2063ffa4927 25
e5115026 1:b2063ffa4927 26 #define resolution 200 //オドメトリエンコーダーの分解能
e5115026 1:b2063ffa4927 27 #define en_bai 2 //エンコーダーの倍する変数
e5115026 1:b2063ffa4927 28
e5115026 1:b2063ffa4927 29 //縦：X 横:Y
e5115026 1:b2063ffa4927 30
e5115026 1:b2063ffa4927 31 Ticker tick_odo; //オドメトリ関数を時間割り込みさせる宣言
e5115026 1:b2063ffa4927 32
e5115026 1:b2063ffa4927 33 //オドメトリ用エンコーダープルアップ抵抗有効化
e5115026 1:b2063ffa4927 34 DigitalIn Enc1_A(p26);
e5115026 1:b2063ffa4927 35 DigitalIn Enc1_B(p25);
e5115026 1:b2063ffa4927 36 DigitalIn Enc2_A(p24);
e5115026 1:b2063ffa4927 37 DigitalIn Enc2_B(p23);
e5115026 1:b2063ffa4927 38 DigitalIn Enc3_A(p22);
e5115026 1:b2063ffa4927 39 DigitalIn Enc3_B(p21);
e5115026 1:b2063ffa4927 40 QEI odo1(p26, p25,NC,resolution);
e5115026 1:b2063ffa4927 41 QEI odo2(p24, p23,NC,resolution);
e5115026 1:b2063ffa4927 42 QEI odo3(p22, p21,NC,resolution);
e5115026 1:b2063ffa4927 43
e5115026 1:b2063ffa4927 44 RoboClaw roboclaw1(0x80,230400, p13, p14);
e5115026 1:b2063ffa4927 45 RoboClaw roboclaw2(0x81,115200, p9, p10);
e5115026 1:b2063ffa4927 46
e5115026 1:b2063ffa4927 47 //グローバル変数宣言
e5115026 1:b2063ffa4927 48 double x_global=0.0,y_global=0.0,angle=0.0,t=0.0,vertical_degree=0.0;
e5115026 1:b2063ffa4927 49
e5115026 1:b2063ffa4927 50 //サブ関数宣言
e5115026 1:b2063ffa4927 51 double d_r(double degree); //度数法→ラジアン
e5115026 1:b2063ffa4927 52 double r_d(double radian); //ラジアン→度数法
e5115026 1:b2063ffa4927 53 double max_min(double val,double max,double min); //最大最小値設定関数.math.hでいうmax関数とmin関数を統合した関数
e5115026 1:b2063ffa4927 54 double odo_way(int mode); //オドメトリ用エンコーダーからとったpulseをmmに変換
e5115026 1:b2063ffa4927 55 double mms_qpps(double millimeter_sec); //mmsからqpps(秒あたりのパルス数)に変換する関数
e5115026 1:b2063ffa4927 56 void omni_move(double x_speed,double y_speed,double angle);
e5115026 1:b2063ffa4927 57 void odometry(); //オドメトリ関数
e5115026 1:b2063ffa4927 58
e5115026 1:b2063ffa4927 59 //動作コード
e5115026 1:b2063ffa4927 60 void setup_omni(){
e5115026 1:b2063ffa4927 61 //オドメトリ用エンコーダープルアップ抵抗有効化
e5115026 1:b2063ffa4927 62 Enc1_A.mode(PullUp);
e5115026 1:b2063ffa4927 63 Enc1_B.mode(PullUp);
e5115026 1:b2063ffa4927 64 Enc2_A.mode(PullUp);
e5115026 1:b2063ffa4927 65 Enc2_B.mode(PullUp);
e5115026 1:b2063ffa4927 66 Enc3_A.mode(PullUp);
e5115026 1:b2063ffa4927 67 Enc3_B.mode(PullUp);
e5115026 1:b2063ffa4927 68
e5115026 1:b2063ffa4927 69 //オドメトリ関数の時間割り込み有効化
e5115026 1:b2063ffa4927 70 tick_odo.attach(&odometry,dt);
e5115026 1:b2063ffa4927 71
e5115026 1:b2063ffa4927 72 //モーター初期化
e5115026 1:b2063ffa4927 73 omni_move(0.0,0.0,0.0);
e5115026 1:b2063ffa4927 74 }
e5115026 1:b2063ffa4927 75
e5115026 1:b2063ffa4927 76
e5115026 1:b2063ffa4927 77 /*
e5115026 1:b2063ffa4927 78 int main() {
e5115026 1:b2063ffa4927 79 setup();
e5115026 1:b2063ffa4927 80 wait(4.0);
e5115026 1:b2063ffa4927 81 while(1){
e5115026 1:b2063ffa4927 82 omni_move(100.0,100.0,0.0);
e5115026 1:b2063ffa4927 83 pc.printf("\nx_global:%lf\ty_global:%lf\tangle:%lf\t\n",x_global,y_global,angle);
e5115026 1:b2063ffa4927 84 wait(dt);
e5115026 1:b2063ffa4927 85 }
e5115026 1:b2063ffa4927 86 }
e5115026 1:b2063ffa4927 87 */
e5115026 1:b2063ffa4927 88
e5115026 1:b2063ffa4927 89 //－－－－－－－以下サブ関数－－－－－－－//
e5115026 1:b2063ffa4927 90 double d_r(double degree){//度数→ラジアン
e5115026 1:b2063ffa4927 91 return degree*Pi/180.0;
e5115026 1:b2063ffa4927 92 }
e5115026 1:b2063ffa4927 93
e5115026 1:b2063ffa4927 94 double r_d(double radian){//ラジアン→度数
e5115026 1:b2063ffa4927 95 return radian*180.0/Pi;
e5115026 1:b2063ffa4927 96 }
e5115026 1:b2063ffa4927 97
e5115026 1:b2063ffa4927 98 double max_min(double val,double max,double min){//限度設定関数
e5115026 1:b2063ffa4927 99 if(val>max)return max;
e5115026 1:b2063ffa4927 100 else if(val<min)return min;
e5115026 1:b2063ffa4927 101 else return val;
e5115026 1:b2063ffa4927 102 }
e5115026 1:b2063ffa4927 103
e5115026 1:b2063ffa4927 104 double mms_qpps(double millimeter_sec){//mm/sにqppsを変換
e5115026 1:b2063ffa4927 105 return millimeter_sec*((2024.0*4.0)/(radius*Pi));
e5115026 1:b2063ffa4927 106 }
e5115026 1:b2063ffa4927 107
e5115026 1:b2063ffa4927 108 double odo_way(int mode){//pulseからmmに変換
e5115026 1:b2063ffa4927 109 if(mode==1) return ((double)odo1.getPulses()*omni1*Pi)/(resolution*en_bai);
e5115026 1:b2063ffa4927 110 else if(mode==2) return ((double)odo2.getPulses()*omni2*Pi)/(resolution*en_bai);
e5115026 1:b2063ffa4927 111 else if(mode==3) return ((double)odo3.getPulses()*omni3*Pi)/(resolution*en_bai);
e5115026 1:b2063ffa4927 112 else return 0;
e5115026 1:b2063ffa4927 113 }
e5115026 1:b2063ffa4927 114
e5115026 1:b2063ffa4927 115 void odometry(){ //単位時間あたりの移動量を積分して自己位置を推定する,神が書いた領域である関数
e5115026 1:b2063ffa4927 116 static double radian,pre_odo_way1=0.0,pre_odo_way2=0.0,pre_odo_way3=0.0;
e5115026 1:b2063ffa4927 117 static double xg_dot, yg_dot, prev_xg_dot = 0, prev_yg_dot = 0, prev_theta_dot = 0;
e5115026 1:b2063ffa4927 118 double x_dot,y_dot,v1,v2,v3,theta_dot;
e5115026 1:b2063ffa4927 119 double dw[4];
e5115026 1:b2063ffa4927 120
e5115026 1:b2063ffa4927 121 dw[1] = odo_way(1);
e5115026 1:b2063ffa4927 122 dw[2] = odo_way(2);
e5115026 1:b2063ffa4927 123 dw[3] = odo_way(3);
e5115026 1:b2063ffa4927 124
e5115026 1:b2063ffa4927 125 //三角関数式
e5115026 1:b2063ffa4927 126 radian=(dw[1]+dw[2]+dw[3])/(3*gaisetuen);
e5115026 1:b2063ffa4927 127 //way_y=(dw[1]*cos(radian) + dw[2]*cos(radian+d_r(120.0)) + dw[3]*cos(radian+d_r(240.0)))/2.0;
e5115026 1:b2063ffa4927 128 //way_x=(dw[1]*sin(radian) + dw[2]*sin(radian+d_r(120.0)) + dw[3]*sin(radian+d_r(240.0)))/2.0;
e5115026 1:b2063ffa4927 129
e5115026 1:b2063ffa4927 130 //積分式
e5115026 1:b2063ffa4927 131 v1 = (dw[1] - pre_odo_way1)/dt;
e5115026 1:b2063ffa4927 132 v2 = (dw[2] - pre_odo_way2)/dt;
e5115026 1:b2063ffa4927 133 v3 = (dw[3] - pre_odo_way3)/dt;
e5115026 1:b2063ffa4927 134
e5115026 1:b2063ffa4927 135 x_dot=(v2-v3)/1.7320508;
e5115026 1:b2063ffa4927 136 y_dot=(-v1 + 2.0*(v2 + v3))/3.0;
e5115026 1:b2063ffa4927 137 theta_dot=(v1+v2+v3)/(3.0*gaisetuen/2.0);
e5115026 1:b2063ffa4927 138
e5115026 1:b2063ffa4927 139 xg_dot=x_dot*cos(radian) - y_dot*sin(radian);
e5115026 1:b2063ffa4927 140 yg_dot=x_dot*sin(radian) + y_dot*cos(radian) - theta_dot/(gaisetuen/2.0);
e5115026 1:b2063ffa4927 141
e5115026 1:b2063ffa4927 142 y_global += ((xg_dot + prev_xg_dot)/2.0)*dt;
e5115026 1:b2063ffa4927 143 x_global -= ((yg_dot + prev_yg_dot)/2.0)*dt;
e5115026 1:b2063ffa4927 144 radian += ((theta_dot + prev_theta_dot)/2.0)*dt;
e5115026 1:b2063ffa4927 145
e5115026 1:b2063ffa4927 146 //y_global=(-1.0) * y_global;//値が常に0になるエラー発生
e5115026 1:b2063ffa4927 147
e5115026 1:b2063ffa4927 148 prev_xg_dot = xg_dot;
e5115026 1:b2063ffa4927 149 prev_yg_dot = yg_dot;
e5115026 1:b2063ffa4927 150 prev_theta_dot = theta_dot;
e5115026 1:b2063ffa4927 151 pre_odo_way1=dw[1];
e5115026 1:b2063ffa4927 152 pre_odo_way2=dw[2];
e5115026 1:b2063ffa4927 153 pre_odo_way3=dw[3];
e5115026 1:b2063ffa4927 154 angle=(-1.0)*r_d(radian);
e5115026 1:b2063ffa4927 155 }
e5115026 1:b2063ffa4927 156
e5115026 1:b2063ffa4927 157 void omni_move(double x_speed,double y_speed,double angle){
e5115026 1:b2063ffa4927 158 x_speed=mms_qpps(x_speed);
e5115026 1:b2063ffa4927 159 y_speed=mms_qpps(y_speed);
e5115026 1:b2063ffa4927 160 angle=mms_qpps(angle);
e5115026 1:b2063ffa4927 161 roboclaw1.SpeedM2((int32_t)(x_speed*sin(d_r(90.0)) + y_speed*cos(d_r(90.0)) + angle));
e5115026 1:b2063ffa4927 162 roboclaw2.SpeedM1((int32_t)(x_speed*sin(d_r(330.0)) + y_speed*cos(d_r(330.0)) + angle));
e5115026 1:b2063ffa4927 163 roboclaw1.SpeedM1((int32_t)(-1.0*(x_speed*sin(d_r(210.0)) + y_speed*cos(d_r(210.0)) + angle)));
e5115026 1:b2063ffa4927 164 }
e5115026 1:b2063ffa4927 165
e5115026 1:b2063ffa4927 166