Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 6:040d001acb12
- Parent:
- 5:fcc79e507610
- Child:
- 8:b79d21c8178b
--- a/main.cpp Sat Aug 24 00:30:20 2019 +0000
+++ b/main.cpp Sat Aug 24 06:06:36 2019 +0000
@@ -1,56 +1,8 @@
-/*今日 やること*/
-
-/******************************************************************************/
#include "main.h"
-//#include "gyro.h"
-#include <math.h>
-
-/******************************************************************************/
-#define CIRCLE sbdbt.CIRCLE
-#define SQUARE sbdbt.SQUARE
-#define CROSS sbdbt.CROSS
-#define TRIANGLE sbdbt.TRIANGLE
-#define LEFTkey sbdbt.LEFTkey
-#define RIGHTkey sbdbt.RIGHTkey
-#define UPkey sbdbt.UPkey
-#define DOWNkey sbdbt.DOWNkey
-
-/******************************************************************************/
/* 型定義 --------------------------------------------------------------------*/
-MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
- { 8, 0, 0, 0, {0}}, //足回り
- { 9, 1, 0, 0, {0}},
- {10, 0, 0, 0, {0}},
- {11, 1, 0, 0, {0}},
- {12, 0, 0, 0, {0}}, // 洗濯物回収
- {13, 1, 0, 0, {0}}, // 上下機構
- {14, 0, 0, 0, {0}}, // 洗濯物排出
- {15, 1, 0, 0, {0}} // 左右移動
-};
-
-IN_I2C_Data_TypeDef In_Data[] = {
- {1,0,0}, // 洗濯物排出(初期位置)
- {1,1,0}, // 洗濯物排出(停止)
- {1,2,0}, // 腕(最大 タオル)
- {1,3,0}, // 腕(初期位置)
- {3,0,0}, // 上下機構(上)
- {3,1,0}, // (下)
- {3,2,0}, // 腕(シーツ兼シャツ）
-};
-
-OUT_I2C_Data_TypeDef Out_Data[] = {
- {0,0,0}, // 腕の上下
- {0,1,0}, // 爪
- {0,2,0}, // タオルの先
- {0,3,0}, // タオル展開
- {1,0,0}, // タオルの中央
-};
/* 関数宣言 ------------------------------------------------------------------*/
-void mecanum_Move(void);
-void mecanum_Turn(void);
-void mecanum_Stop(void);
/* 変数宣言 ------------------------------------------------------------------*/
//かご回転用
@@ -69,14 +21,11 @@
//タオル横展開用
int unfold_f=0;
-//90度回転のフラグ
-int left_f=0, right_f=0;
+//y_deg: ジャイロの生の値
+//yaw: 実際に使う角度
+double y_deg[2], yaw=0;;
-//y_deg: 起動時から絶対の角度
-//yaw: 相対角 初期状態が0になるようにする
-//offset: 初期状態を0にするために引く値
-double y_deg[2], offset;
-double yaw=0;
+//ジャイロさんぷりんぐ用
double sum=0, ave=0;
int offset_cnt=0;
@@ -89,36 +38,18 @@
//short_lim: 腕手前側
int short_lim, max_lim, start_lim;
-//機体の前方を変更する
-int dir=0;
+//足回りのpwm値
+int duty[4];
-int limit_up,limit_down; // リミットスイッチ
-int slide_start,slide_stop; //かごのリミット
+// 足回り
+int lx,ly,rx;
-int lx,ly,rx,L,R; // 足回り
-int g;
+int limit_up,limit_down; // リミットスイッチ
+int slide_start,slide_stop; //かごのリミット
int as=0;
-/*クラス宣言-------------------------------------------------------------------*/
-Ticker flipper1; // 割込み
-Y_I2C i2c(PB_9, PB_8); // I2C
-DigitalOut myled(LED1); // オンボードLED
-SBDBT sbdbt(PA_0, PA_1, 9600); // SDBDT
-MyMPU6050 mpu(PC_9, PA_8); // ジャイロセンサ
-
-DigitalOut blue(PA_10);
-DigitalOut green(PB_4);
-DigitalOut red(PB_5);
-
-//半自動の時に使う
-Timer tim;
-
/*----------------------------------- main -----------------------------------*/
-/* @brief メインプログラム
-* @param なし
-* @retval なし
-*/
int main()
{
//flipper1.attach(&flip1, 0.001); // 割り込み
@@ -126,103 +57,53 @@
while(1) {
//自動系のタイマースタート
tim.start();
+ drift_tim.start();
mpu.loop();
+ //ジャイロから値を取得
y_deg[1] = y_deg[0];
y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
-
+
//180～-180をまたいだ時
if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
if(sbdbt.State_check()) {
for(int i=0; i<7; i++) i2c.In(In_Data,i);
-
+
//誤差をさんぷりんぐ
if(offset_cnt<1000) {
+ PALETTE(RED);
if(offset_cnt!=0) {
sum+=(y_deg[0]-y_deg[1]);
ave=sum/offset_cnt;
}
offset_cnt++;
- } else yaw += (y_deg[0]-y_deg[1])-ave;
-// yaw=90*dir;
-// yaw%=360;
- dif_val = y_deg[0]-Turn_val;
+ } else {
+ PALETTE(GREEN);
+ yaw -= (y_deg[0]-y_deg[1])-ave;
+ }
+
+ dif_val = yaw-Turn_val;
+ //制限
+ if(dif_val>=30) dif_val=30;
+ else if(dif_val<=-30) dif_val=-30;
lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
- rx = (sbdbt.RX - 64)*100/64;
- L = lx+ly;
- R = ly-lx;
-
- if(sbdbt.LX !=64 || sbdbt.LY !=64) {
-
- mecanum_Move();
+ rx = (sbdbt.RX - 64)*30/64;
- // 以下，ジャイロセンサーのプログラム
- if(g) {
- if(abs(ly) > 10) {
- if(ly>10) {
- (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
- (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val;
- } else {
- (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
- (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val;
- }
- }
-
- if(abs(lx) > 10) {
- if(lx>10) {
- (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
- (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val;
- } else {
- (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
- (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val;
- }
- }
+ //メカナム基本動作
+ mecanum_Move(lx, ly, rx);
- if((abs(ly)-abs(lx)) > 0) {
- (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0);
- (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1);
- } else {
- (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0);
- (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2);
- }
- } // if(g)
- }// if(LX !=64 || LY !=64)
- else if(sbdbt.RX != 64) { // 旋回
- Turn_val=y_deg[0];
- mecanum_Turn();
- } else {
- mecanum_Stop();
- }
+ //旋回している間タイマーをリセット
+ if(sbdbt.RX != 64) drift_tim.reset();
- // 90°旋回
- if(left_f) {
- if(abs(yaw)<60) {
- for(int i=0; i<4; i++) {
- MD_SET_PWM(MD_Data, i,60);
- MD_SET_DRIVE(MD_Data, i,MD_REVERSE);
- }
- } else {
- left_f=0;
- Turn_val=y_deg[0];
- mecanum_Stop();
- }
- }
- if(right_f) {
- if(yaw>-60) {
- for(int i=0; i<4; i++) {
- MD_SET_PWM(MD_Data, i,60);
- MD_SET_DRIVE(MD_Data, i,MD_FORWARD);
- }
- } else {
- right_f=0;
- Turn_val=y_deg[0];
- mecanum_Stop();
- }
- }
+ //旋回して慣性で動いた後の角度に補正する
+ if(drift_tim.read_ms()<500) Turn_val=yaw;
+
+ //角度補正
+ AngleCorrection(yaw, Turn_val);
/*---ハンガーかけるやつ----------------------------------------------*/
if(sbdbt.SELECT)as=1;//selectキー
@@ -248,6 +129,7 @@
//タオルつかむ 四角
if(grab_f || sgrab_f) {
+ PALETTE(BLUE);
switch(grab) {
//腕を前に動かす
case 0:
@@ -255,6 +137,7 @@
i2c.Out_Set(Out_Data,1,1);
MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
MD_SET_PWM(MD_Data,4,40);
+ //sgrab_fは途中のリミットで止まる
if((sgrab_f && short_lim) || max_lim==1) {
grab++;
tim.reset();
@@ -305,6 +188,7 @@
//タオル引く 丸
else if(pull_f) {
+ PALETTE(CYAN);
switch(pull) {
//腕を前に動かす
case 0:
@@ -348,6 +232,7 @@
//かごを倒す
else if(kago_f) {
+ PALETTE(MAGENTA);
//腕の先を閉じておく
i2c.Out_Set(Out_Data,1,0);
@@ -436,7 +321,8 @@
// pc.printf("pull %2d%2d ",pull,pull_f);
// pc.printf("grab %2d%2d ",grab,grab_f);
// pc.printf("kago %2d%2d ",kago,kago_f);
- pc.printf("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw);
+// pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
+ pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val);
// pc.printf("%2d ",dir);
pc.printf("\n\r");
@@ -455,29 +341,13 @@
/* ----------------ボタン系--------------- */
- //補正角リセット
+ //角度リセット
if(sbdbt.L1) {
- Turn_val=y_deg[0];
- offset=y_deg[0];
- }
- //90度回転して補正の角度を固定する
- if(LEFTkey) {
- if(once==0) {
- dir++;
-// left_f++;
-// left_f%=2;
- once++;
- }
- } else if(RIGHTkey) {
- if(once==0) {
- dir--;
-// right_f++;
-// right_f%=2;
- once++;
- }
+ yaw=0;
+ Turn_val=0;
}
//かごを倒す
- else if(CROSS) {
+ if(CROSS) {
if(once==0) {
kago_f++;
kago_f%=2;
@@ -521,63 +391,33 @@
} // int main()
/* メカナムの基本移動 */
-void mecanum_Move(void)
+void mecanum_Move(int lx, int ly, int rx)
{
- int val1,val2;
-
- MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE);
- MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD);
- MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE);
- MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD);
-
- val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) );
- val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) );
-
- MD_SET_PWM(MD_Data,0,val1);
- MD_SET_PWM(MD_Data,1,val2);
- MD_SET_PWM(MD_Data,2,val2);
- MD_SET_PWM(MD_Data,3,val1);
-
+ duty[0]=lx+ly;
+ duty[1]=-(-lx+ly);
+ duty[2]=-lx+ly;
+ duty[3]=-(lx+ly);
- if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) {
- MD_SET_PWM(MD_Data, 0, 0);
- g = 0;
- } else if( MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) {
- MD_SET_PWM(MD_Data, 1, 0);
- g = 0;
- }
-
- // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動
- else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) {
- MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1));
- g = 1;
-
- } else {
- MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0));
- g = 1;
- }
-
- MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1));
- MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0));
-
-}
-
-/* メカナム旋回 */
-void mecanum_Turn(void)
-{
for(int i=0; i<4; i++) {
- MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64);
- MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD);
+ duty[i]+=rx;
+ MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
+ MD_SET_PWM(MD_Data, i, abs(duty[i]));
}
}
-/* 停止 */
-void mecanum_Stop(void)
+void AngleCorrection(double n_angle, double t_angle)
{
+ double dif=-(t_angle-n_angle);
+
+ if(dif>=30) dif=30;
+ else if(dif<=-30) dif=-30;
+
for(int i=0; i<4; i++) {
- MD_SET_PWM(MD_Data, i, 0);
- MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
+ duty[i]+=dif;
+ MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
+ MD_SET_PWM(MD_Data, i, abs(duty[i]));
}
+
}
/*操作法*/
@@ -588,18 +428,15 @@
□ ボタン == 回収（タオル）
上 ボタン == ハンガーかけ機構 上昇
下 ボタン == ハンガーかけ機構 下降
-左 ボタン == 90回転
-右 ボタン == 90回転
L1 ボタン == リセット（足回り）
L2 ボタン == タオル掛け
-R1 ボタン == 押しながら回転でゆっくりになる
-R2 ボタン == タオルひっぱる奴
+R2 ボタン == タオル真ん中掴む
select == ハンガー掛け 最大まで上昇
start == ハンガー展開
*/
/*その他注意点*/
/*
-動かすときはジャイロの値を安定させるため15秒から20秒待つ
+動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
あまり急発進をさせない（たまに暴走）
暴走したときはコントローラーを動かしたら治るはず