2020_TeamA
/
somaomuni2
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 YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C
Diff: main.cpp
- Revision:
- 2:ec149525ec2a
- Parent:
- 1:b9f698be841c
- Child:
- 3:b77f51108bf1
--- a/main.cpp Sat Mar 14 05:29:14 2020 +0000
+++ b/main.cpp Tue Mar 17 05:17:59 2020 +0000
@@ -17,6 +17,8 @@
ROCATION NowAcc;
/* ロボットの座標 */
ROCATION NowLoc;
+/* ロボットの目標座標 */
+ROCATION Target;
/* 定数定義 ------------------------------------------------------------------*/
/* マクロ定義 ----------------------------------------------------------------*/
@@ -28,6 +30,9 @@
/* 自己位置推定処理 */
void LocEstimate(void);
+/* P制御値代入 */
+void Pro_control(int tar_x, int tar_y, int tar_rev);
+
/* 変数定義 ------------------------------------------------------------------*/
@@ -41,7 +46,7 @@
double EncoderDeg[EncoderMAX] = {0};
/* 足回り値保存変数 */
-int MovMotor[4]={0};
+int MovMotor[4]= {0};
/* 自動yaw補整目標角度 */
double TarTheta=0;
@@ -57,6 +62,9 @@
/* gyro用タイマ */
Timer yawCnt;
+/* P制御終了タイマ */
+Timer P_fin;
+
/* UART (Tx,Rx) */
Serial telemetry(USBTX, USBRX, 115200);
@@ -87,105 +95,105 @@
/*----------------------------------- main ----------------------------------*/
int main()
{
- telemetry.printf("\n\rMainStart");
+ telemetry.printf("\n\rMainStart");
- /* 割り込みの設定
- * IT_CallBack関数を0.1msで割り込み */
- flipper.attach_us(&IT_CallBack, 100);
+ /* 割り込みの設定
+ * IT_CallBack関数を0.1msで割り込み */
+ flipper.attach_us(&IT_CallBack, 100);
- /* ジャイロの設定 */
- bno.setmode(OPERATION_MODE_IMUPLUS);
+ /* ジャイロの設定 */
+ bno.setmode(OPERATION_MODE_IMUPLUS);
- /* I2CMDの設定 */
- MD.Init(0,MD_SMB);
- MD.Init(1,MD_SMB);
- MD.Init(2,MD_SMB);
- MD.Init(3,MD_SMB);
+ /* I2CMDの設定 */
+ MD.Init(0,MD_SMB);
+ MD.Init(1,MD_SMB);
+ MD.Init(2,MD_SMB);
+ MD.Init(3,MD_SMB);
- telemetry.printf("\n\rMainLoopStart");
- /* メインループ --------------------------------------------------------------*/
- while(1) {
- /* オンボードLED点滅 */
- led=!led;
+ telemetry.printf("\n\rMainLoopStart");
+ /* メインループ --------------------------------------------------------------*/
+ while(1) {
+ /* オンボードLED点滅 */
+ led=!led;
- /* 表示改行 */
- telemetry.printf("\n\r");
+ /* 表示改行 */
+ telemetry.printf("\n\r");
- /* 自動処理関連テレメトリ */
- telemetry.printf("ope:%d ",operate);
- /* 座標テレメトリ */
- telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
+ /* 自動処理関連テレメトリ */
+ telemetry.printf("ope:%d ",operate);
+ /* 座標テレメトリ */
+ telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
- /* 自己位置推定更新 */
- LocEstimate();
+ /* 自己位置推定更新 */
+ LocEstimate();
- /* I2CMD実行 */
- MD.Exe();
-
- /* タイマ-スタート */
- yawCnt.start();
- /* タイマーリセット */
- yawCnt.reset();
+ /* I2CMD実行 */
+ MD.Exe();
+
+ /* タイマ-スタート */
+ yawCnt.start();
+ /* タイマーリセット */
+ yawCnt.reset();
- /* 操縦権変更 ×停止 △手動 〇自動 */
- if(DS3.CROSS) operate=0;
- if(DS3.TRIANGLE) operate=1;
- if(DS3.CIRCLE) operate=2;
+ /* 操縦権変更 ×停止 △手動 〇自動 */
+ if(DS3.CROSS) operate=0;
+ if(DS3.TRIANGLE) operate=1;
+ if(DS3.CIRCLE) operate=2;
- /* 操縦権：なし 停止動作 */
- if(operate==0){
- /* 足回り停止 */
- omuni.XmarkOmni_Move(0,0,0);
- for (int i = 0; i < 4; i++) {
+ /* 操縦権：なし 停止動作 */
+ if(operate==0) {
+ /* 足回り停止 */
+ omuni.XmarkOmni_Move(0,0,0);
+ for (int i = 0; i < 4; i++) {
MD.Set(i,MovMotor[i]);
}
- }
- /* 操縦権：手動 */
- else if(operate==1){
- /* 足回り手動動作 */
- int x_val = (double)(DS3.LX-64)*100/64;
- int y_val = (double)(64-DS3.LY)*100/64;
- int r_val = (double)(DS3.RX-64)*100/64;
+ }
+ /* 操縦権：手動 */
+ else if(operate==1) {
+ /* 足回り手動動作 */
+ int x_val = (double)(DS3.LX-64)*100/64;
+ int y_val = (double)(64-DS3.LY)*100/64;
+ int r_val = (double)(DS3.RX-64)*100/64;
- /* 目標角更新 */
- if(DS3.RX!=64) yawCnt.reset();
- if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
-
- /* gyro値による補正 */
- r_val += (TarTheta-NowLoc.theta)*cor;
-
- omuni.XmarkOmni_Move(x_val,y_val,r_val);
- for (int i = 0; i < 4; i++) {
+ /* 目標角更新 */
+ if(DS3.RX!=64) yawCnt.reset();
+ if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
+
+ /* gyro値による補正 */
+ r_val += (TarTheta-NowLoc.theta)*cor;
+
+ omuni.XmarkOmni_Move(x_val,y_val,r_val);
+ for (int i = 0; i < 4; i++) {
MD.Set(i,MovMotor[i]);
}
- }
- /* 操縦権：自動 */
- else if(operate==2){
- switch(auto_mode){
- /* スタート待機処理 */
- case 0:
- /* オンボードSWで次のステップに */
- if(UB) auto_mode++;
- break;
+ }
+ /* 操縦権：自動 */
+ else if(operate==2) {
+ switch(auto_mode) {
+ /* スタート待機処理 */
+ case 0:
+ /* オンボードSWで次のステップに */
+ if(UB) auto_mode++;
+ break;
- /* 〇〇の処理 */
- case 1:
- /* 〇〇の時次のステップに */
- if(1) auto_mode++;
- break;
+ /* 〇〇の処理 */
+ case 1:
+ /* 〇〇の時次のステップに */
+ if(1) auto_mode++;
+ break;
- /* 終了処理 */
- default:
- auto_mode=0;
- operate=0;
- break;
- }
+ /* 終了処理 */
+ default:
+ auto_mode=0;
+ operate=0;
+ break;
+ }
+ }
+
}
-
- }
}
@@ -194,37 +202,38 @@
* @引数 なし
* @返り値 なし
*******************************************************************************/
-void LocEstimate(void){
- static double GyroDeg[2]={0};
- static double EncDeg[2][2]={0};
- static double disp[3]={0};
+void LocEstimate(void)
+{
+ static double GyroDeg[2]= {0};
+ static double EncDeg[2][2]= {0};
+ static double disp[3]= {0};
- /* ジャイロの値取得 */
- bno.get_angles();
- GyroDeg[1]=GyroDeg[0];
- GyroDeg[0]=bno.euler.yaw;
- if(GyroDeg[0]!=0) {
- /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
- if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
- /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
- else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
- /* 差を求める*/
- disp[2]=GyroDeg[1]-GyroDeg[0];
- }
- /* Enc2つの差分求める */
- for(int i=0;i<2;i++){
- EncDeg[i][1]=EncDeg[i][0];
- EncDeg[i][0]=EncoderDeg[i];
- disp[i]=EncDeg[i][1]-EncDeg[i][0];
- }
- /* 差分を加速度として保存 */
- NowAcc.theta = disp[2];
- NowAcc.X = disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
- NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
- /* 差分を累積して現在位置を保存 */
- NowLoc.X += NowAcc.X;
- NowLoc.Y += NowAcc.Y;
- NowLoc.theta += NowAcc.theta;
+ /* ジャイロの値取得 */
+ bno.get_angles();
+ GyroDeg[1]=GyroDeg[0];
+ GyroDeg[0]=bno.euler.yaw;
+ if(GyroDeg[0]!=0) {
+ /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
+ if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
+ /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
+ else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
+ /* 差を求める*/
+ disp[2]=GyroDeg[1]-GyroDeg[0];
+ }
+ /* Enc2つの差分求める */
+ for(int i=0; i<2; i++) {
+ EncDeg[i][1]=EncDeg[i][0];
+ EncDeg[i][0]=EncoderDeg[i];
+ disp[i]=EncDeg[i][1]-EncDeg[i][0];
+ }
+ /* 差分を加速度として保存 */
+ NowAcc.theta = disp[2];
+ NowAcc.X = disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
+ NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
+ /* 差分を累積して現在位置を保存 */
+ NowLoc.X += NowAcc.X;
+ NowLoc.Y += NowAcc.Y;
+ NowLoc.theta += NowAcc.theta;
}
@@ -233,41 +242,117 @@
/* 割り込み(100us) *************************************************************/
void IT_CallBack(void)
{
- static int cnt = 0;
- static int data[EncoderMAX] = {0};
- static double EncDeg[EncoderMAX][2] = {0};
+ static int cnt = 0;
+ static int data[EncoderMAX] = {0};
+ static double EncDeg[EncoderMAX][2] = {0};
+
+ for(int i=0; i<EncoderMAX; i++)
+ switch(cnt) {
+ /* 最初の処理 */
+ case 0:
+ data[i] = 0;
+ CS[i] = 0;
+ CL[i] = 1;
+ break;
+ /* 最後の処理 */
+ case 25:
+ CS[i]=1;
+ /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
+ EncDeg[i][1] = EncDeg[i][0];
+ EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
+ /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
+ if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
+ EncDeg[i][1] -= 360;
+ /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
+ else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
+ EncDeg[i][1] += 360;
+ /* 差を求める*/
+ EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
+ break;
+ /* 通常の処理 */
+ default:
+ CL[i]=!CL[i];
+ /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
+ if(cnt != 1 && cnt % 2) {
+ data[i] |= (DO[i]==1);
+ data[i] = data[i] << 1;
+ }
+ break;
+ }
+ cnt++;
+ cnt%=26;
+}
+
+
+
+
+/*******************************************************************************
+ * @概要 P制御
+ * @引数 tar_x :目標のx座標
+ * @引数 tar_y :目標のy座標
+ * @引数 tar_rev: 目標角
+ * @返り値 なし
+*******************************************************************************/
+void Pro_control(int tar_x, int tar_y, int tar_theta)
+{
+ int log_distance = 0;
- for(int i=0;i<EncoderMAX;i++)
- switch(cnt){
- /* 最初の処理 */
- case 0:
- data[i] = 0; CS[i] = 0; CL[i] = 1;
- break;
- /* 最後の処理 */
- case 25:
- CS[i]=1;
- /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
- EncDeg[i][1] = EncDeg[i][0];
- EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
- /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
- if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
- EncDeg[i][1] -= 360;
- /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
- else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
- EncDeg[i][1] += 360;
- /* 差を求める*/
- EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
- break;
- /* 通常の処理 */
- default:
- CL[i]=!CL[i];
- /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
- if(cnt != 1 && cnt % 2){
- data[i] |= (DO[i]==1);
- data[i] = data[i] << 1;
- }
- break;
- }
- cnt++;
- cnt%=26;
+ /* 現在座標と目標座標との距離を記録しておく */
+ log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
+
+ int x_vel = 0;
+ int y_vel = 0;
+ int t_vel = 0;
+
+ double theta_tar=0.0;
+
+ while(1) {
+ x_vel = ABS((tar_x - NowLoc.X) * 0.06);
+ y_vel = ABS((tar_y - NowLoc.Y) * 0.06);
+
+ /* 最低を決める */
+ if (x_vel < 20)
+ x_vel = 20;
+ if (y_vel < 20)
+ y_vel = 20;
+ /* 100以内に収める */
+ x_vel = Rest(x_vel, 100);
+ y_vel = Rest(y_vel, 100);
+
+ /* 正負を決める */
+ if ((tar_x - NowLoc.X) < 0)
+ x_vel *= -1;
+ if ((tar_y - NowLoc.Y) < 0)
+ y_vel *= -1;
+
+ /* 残り距離計算 */
+ int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
+ /* 移動量に応じたthetaを求める */
+ theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - NowLoc.theta) + NowLoc.theta;;
+ /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
+ t_vel = (theta_tar - NowLoc.theta) * 1.5;
+
+ t_vel = Rest(t_vel, 20);
+
+ omuni.XmarkOmni_Move(x_vel,y_vel,t_vel);
+
+
+ /* 終了処理 */
+ if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
+ x_vel = 0;
+ y_vel = 0;
+ }
+ if (x_vel == 0 && y_vel == 0)
+ P_fin.start();
+ else
+ P_fin.reset();
+
+ if (P_fin.read_ms() > 100) {
+ /* タイマー停止 */
+ P_fin.stop();
+ P_fin.reset();
+
+ break;
+ }
+ }
}
\ No newline at end of file