hirromu takaki
nhk 2019 syudou B
Dependencies: R1370 gyro Controller ikarashiMDC
Diff: main.cpp
- Revision:
- 1:c26a135080b1
- Parent:
- 0:09ef6f968eef
- Child:
- 2:2b635b7e0bb6
--- a/main.cpp Mon Jun 10 14:25:27 2019 +0000
+++ b/main.cpp Sat Jun 22 00:40:58 2019 +0000
@@ -1,102 +1,175 @@
/*
- * // が縦に連続して並んでいたらそこはひとまとまりになっていると考えて
+ * 改良しました
*
- * 変数名は変えていいし、タイマー関連で テストで使った と書いてあるものは必要ない
+ *
*
* 何かわからないことがあったらその都度聞いて
*/
-#include "mbed.h" //
-#include "gyro.h" //
-#include "ikarashiMDC.h" //
-#include "R1370.h" // 必須
-Serial pc(USBTX, USBRX, 115200); //
-Serial serial(PC_6,PC_7); //
-DigitalOut serialcontrol(D2); //
-R1370 R1370(PB_6,PA_10); //
+#include "mbed.h"
+#include "gyro.h"
+#include "ikarashiMDC.h"
+#include "R1370.h"
+#include "controller.h"
+#define PI 3.141593
+
+Controller pad(PC_10, PC_11, 208);
+R1370 R1370(PB_6,PA_10);
+Serial pc(USBTX, USBRX, 115200);
+Serial serial(PC_6, PC_7, 115200);
+DigitalOut serialcontrol(D10);
gyro omni(4); //オムニホイールの数を入力
-Timer rad,spin; //これは試すときに使ったやつ
-
-ikarashiMDC ikarashi[] { //
- ikarashiMDC(&serialcontrol,2,0,SM,&serial), //
- ikarashiMDC(&serialcontrol,2,1,SM,&serial), //
- ikarashiMDC(&serialcontrol,2,2,SM,&serial), //ロボットの使うMDCに応じて変える事
- ikarashiMDC(&serialcontrol,2,3,SM,&serial) //
-}; //
+/*ロボットの使うMDCに応じて変える事*/
+ikarashiMDC ikarashi[] = {
+ ikarashiMDC(&serialcontrol,2,0,SM,&serial),
+ ikarashiMDC(&serialcontrol,2,1,SM,&serial),
+ ikarashiMDC(&serialcontrol,2,2,SM,&serial),
+ ikarashiMDC(&serialcontrol,2,3,SM,&serial)
+};
int main()
{
- double acc[4] = {0,0,0,0},angle,control; //accはアクセラレート：加速の略 angleはジャイロセンサの値を入れる controlは角度調整に必要
-
- serial.baud(115200); //
- ikarashi[0].braking = true; //必須
-
- for(int i = 0; i < 4; i++) //タイヤ角を設定 必須
- { //omni.setRad(タイヤの番号,タイヤのついてる角度radian値)
- omni.setRad(i, PI * (i*2+1) / 4); //このプログラムでは4輪の設定をしている
- } //もし4輪ならこのままでいい
-
- rad.start(); //テストするときに使ったやつ
- spin.start(); //
-
- omni.setIdeal(0); //目的の角度を0度に設定 必須
+ int b[11];//自作コントローラー関連
+ double stickRad[2],stickNorn[2],raded[2],Norned[2];
+
+ bool b7 = 1,b9 = 1;//b[7]とb[9]のボタン判断
+
+ double acc[4] = {0,0,0,0},angle,control,con,func,gain = 0;//accはアクセラレート：加速の略 angleはジャイロセンサの値を入れる control,conは角度調整に必要
+ double idealAngle = 0,puls = 0.1;//funcは台形制御に必要 gainは台形制御の計算に使う idealAngleは角度セットに使う
+
+ ikarashi[0].braking = true;//必須
+/*タイヤ角を設定 必須
+omni.setRad(タイヤの番号,タイヤのついてる角度radian値)
+このプログラムでは4輪の設定をしている*/
+ for(int i = 0; i < 4; i++)
+ {
+ omni.setRad(i, PI * (i*2+1) / 4);
+ }
+
+ omni.setIdeal(0);//目的の角度を0度に設定 必須
+
+ omni.setErrorRange(2);//誤差範囲を設定 +-入力値分の誤差が出る ロボットが止まらない時は大きめに設定すること
+
+ omni.limitPower(0.5);//回転速度制限 ここで入力した速さよりも回転は速くならない 正の実数で１より大きくしないこと 必須
+
while(1)
{
- if(R1370.update() == 0){ //ジャイロセンサが動いていれば
- angle = R1370.getAngle(); //ジャイロの出した角度を保存 必須
-
- if(spin < 5) //ここから
+ if(R1370.update() == 0 && pad.receiveState() == 1)//ジャイロセンサが動いてかつコントローラーと通信していれば
+ {
+ angle = R1370.getAngle();//ジャイロの出した角度を保存 必須
+
+ for(int i = 0; i < 13; i++){
+ b[i] = pad.getButton1(i);//0 = 左下 1 = 左上 2 = 十字左 3 = 上 4 = 右 5 = 下 6 = 右下 7 = L2 8 = L1 9 = R2 10 = R1 11 R3 12 = L3
+ }
+ for(int i = 0; i < 2; i++){
+ stickRad[i] = PI - pad.getRadian(i);
+ stickNorn[i] = pad.getNorm(i);
+ }
+ pc.printf("%d %d\n\r",b[7],b[9]);
+/*一度ボタンを押すとその方向に90度回転*/
+ if(b[7] != b7)
{
- omni.setIdeal(0); //目的の角度を0度に設定
- }
- else if(spin < 10)
- {
- omni.setIdeal(90); //90度に設定
+ if(b7 == 1)
+ {
+ idealAngle -= 90;
+ gain = 0;
+ }
+ b7 = b[7];
}
- else if (spin < 15)
+ if(b[9] != b9)
{
- omni.setIdeal(180);//180度に設定
+ if(b9 == 1)
+ {
+ idealAngle += 90;
+ gain = 0;
+ }
+ b9 = b[9];
}
- else if (spin < 20)
+ if(idealAngle > 360)
+ {
+ idealAngle = 90;
+ }
+ if(idealAngle <= 0)
{
- omni.setIdeal(-90); //-90度に設定 設定範囲は180 ~ -179
+ idealAngle = 360;
}
- else if (spin < 25)
- {
- omni.setIdeal(45); //45度に設定
- }
- else
+ omni.setIdeal(idealAngle - 180);
+
+//omni.cAngle(ジャイロの値)
+//ジャイロの角度を渡す 調整用の速度をもらう
+ control = omni.cAngle(angle);
+//omni.pAngle(ジャイロの値,比例ゲイン) こっちは比例制御
+ con = omni.pAngle(angle,2);
+/*比例ゲインは１が等倍速、２が倍速で、0.5が1/2倍速といった感じに速度が変わる
+ちょうどいい比例ゲインは試しながら見つける事
+この二つはどちらか一方のみを使うこと
+どっちも使ってみてから決めてほしい*/
+
+ if(fabs(raded[0] - stickRad[0]) > 2 || fabs(raded[1] - stickRad[1]) > 2 || fabs(Norned[0] - stickNorn[0]) > 0.2 || fabs(Norned[1] - stickNorn[1] > 0.2) )
{
- spin.reset();
- } //ここまでテストで使ったやつ 条件式は不要だが角度設定で必要
-
- control = omni.cAngle(angle); //ジャイロの角度を渡す 調整用の速度をもらう 必須
-
- if(rad > (PI * 2 + 1)) //
- { //
- rad.reset(); //テストで使ったやつ
- } //
-
- for(int i = 0; i < 4; i++) //
- { //
- acc[i] = omni.hMove(10,i) + control; //omni.hMove(角度radian,タイヤ番号) で速度を返す 角度が10なら平行移動しない 必須
- } //+controlで角度がずれたら修正
-
- for(int i = 0; i < 4;i++) //
- { //モーターに出力 必須
- ikarashi[i].setSpeed(acc[i]); //
- } //
-
-
- pc.printf("%f ",angle); //ジャイロセンサの出してる角度を表示
-
- pc.printf("%f,%f,%f,%f\r\n",acc[0],acc[1],acc[2],acc[3]); //各モーターの出力を表示
+ gain = 0;//もしスティックが大きく動いたらgain = 0
+ }
+
+ func = omni.lFunc(0.25,gain);//一次関数の値を取り出す omni.lFunc(比例定数,gain)
+
+ raded[0] = stickRad[0]; //値比較用
+ raded[1] = stickRad[1]; //
+ Norned[0] = stickNorn[0];//
+ Norned[1] = stickNorn[1];//
+
+/* スティックが傾いていないまたはどちらも傾いていると平行移動しない*/
+ if( ((stickNorn[0] == 0) && (stickNorn[1] == 0)) || ((stickNorn[0] != 0) && (stickNorn[1] != 0)) )
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ acc[i] = (omni.hMove(10,i) + con) * func;
+ }//+controlまたは +conで角度がずれたら修正 この時は比例制御を使った//omni.hMove(角度radian,タイヤ番号) で速度を返す 角度が10なら平行移動しない 必須
+ gain += puls;//どんどんモーターの値が大きくなる
+
+ }
+ else if(stickNorn[0] != 0 && stickNorn[1] == 0) //stick[0]だけを動かしているとき
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ acc[i] = (omni.hMove(stickRad[0], i) * stickNorn[0] + con) * func; //方向がロボットの向きによって変化する
+ }
+ gain += puls;//どんどん（ry
+ }
+
+ else //stick[1]だけを動かしているとき
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ acc[i] = (omni.hMove(stickRad[1] + (angle * (PI / 180)), i) * stickNorn[1] + con) * func;//ロボットの向きに左右されずに動く
+ }
+ gain += puls;//どんどん（ry
+ }
+
+ for(int i = 0; i < 4;i++) //モーターに出力 必須
+ {
+ if(acc[i] > 0.9)
+ {
+ acc[i] = 0.9;
+ }
+ else if(acc[i] < -0.9)
+ {
+ acc[i] = -0.9;
+ }
+ ikarashi[i].setSpeed(acc[i]);
+ pc.printf("|%f| ",acc[i]);
+ }
}
- else //ジャイロが動いていなかったら
- { //
- pc.printf("error"); //errorを表示して動かない
- } //
+
+ else//ジャイロ,コントローラーが動いていなかったら
+ {
+ pc.printf("error\n\r"); //errorを表示して動かない
+
+ for(int i = 0; i < 4;i++)
+ {
+ ikarashi[i].setSpeed(0);//モーターに0 必須
+ }
+ }
}
}