ハヤト タジリ
佐賀大deラボ機
Dependencies: mbed MAIDROBO BNO055_fusion Ping
Diff: main.cpp
- Revision:
- 1:0d7cc59d485e
- Parent:
- 0:6c2e6a485519
--- a/main.cpp Thu Oct 18 09:59:24 2018 +0000
+++ b/main.cpp Tue Jul 28 17:11:21 2020 +0000
@@ -3,502 +3,721 @@
#include "Ping.h"
#include "Motor.h"
-Serial pico(D1,D0);
-I2C i2c(D14,D15);
-Ping uss(A1);
-
-//タイマー繰り返し割り込み
-Ticker uss_time;
-
-//ボール追従アルゴリズム
-float r = 0.2; //半径の調整(0~0.7)
-const float Pi = 3.14159265;
-
-//PID関連
-Timer TimePID;
-double dtdeg, preTimedeg, Pdeg, Ideg, Ddeg, prePdeg,Bnodeg, A ,P;//方位PID用変数
-double dtpow, preTimepow, Ppow, Ipow, Dpow, prePpow, B;//パワー用PID
-const float kpdeg = 0.01;//方位センサーPID定数入力 0.005以上はあげない
-const float kideg = 0.01;
-const float kddeg = 0.005;
-const float kppow = 1.5;//パワーPID定数
-const float kipow = 0.5;
-const float kdpow = 0.1;
-
-//モーター
-Motor motor(PB_4,PB_5,PB_3,PA_10,PC_7,PA_9,PA_8,PB_10);
+//constant
+#define GK 1
+#define FW 0
+#define PI 3.1415926
-//赤外線センサー関連
-AnalogIn balldegree(A3);//0のときボール無し10~100のときボール有
-float ballbase,ball,nowball,Godeg;//ボール角度用変数,進行方向
-AnalogIn balldistance(A2);//0~1で距離を表示ボール無いとき
-DigitalIn holdsensor(PC_3);//ホールドセンサ
-
-
-
+/*調整するときに読んでね*/
+/*ロボットを調整する項目は3つあります
+ * 以下の調整は試合前に必ずに行ってください
+ *
+ * 一つ目はラインセンサの調整です
+ * ロボットを起動し、USBケーブルを挿しこんだ後ターミナルを起動して'd'を押してください
+ * 各種センサ値が表示されます
+ * コートの緑のマット上でLine_front ~ Line_rightの数値が
+ * 75前後 になるように、
+ * ロボットの底面についている可変抵抗(青いやつ)を小さいドライバーで回して調整してください
+ * 可変抵抗を時計回りに回すと数値が上がります
+ * キーボード'r'でセンサの値の取得を一時停止することができます
+ *
+ * ２つ目は回り込み半径の調整です
+ * Rを調整することでボールに対してどれくらいの半径を描いて回り込むか調整することができます
+ * 上記と同様にターミナルソフトを起動してキーボード'd'を押すと
+ * 現在のセンサの数値を見ることができますのでBall distance:
+ * の数値を参考にして最適なRを決定してください(distanceの数値=Rにする)
+ * キーボード'r'でセンサの値の取得を一時停止することができます
+ *
+ * ３つ目はスピードとPIDゲイン値の調整です
+ * まずラインから出ないようにspeedを調整します
+ * そのあとロボットが常に前に向くように(傾いても元の方向にすぐに戻るように)
+ * tp ti tdを調整してください
+ * P→D→I(またはP→I→D)の順番に調整すると良いかもしれません
+ *
+ * 以上のことが完了してからプログラムの作成を開始してください
+ */
-//スイッチ類
-DigitalIn sw_right(PD_2);//プログラム開始スイッチ//タクト
-DigitalIn sw_left(PC_11);//ジャイロリセット//タクト
-DigitalIn swdebug(PC_10);//デバッグモードスイッチhighでon
-DigitalIn swkick(PC_12);//キーパーモードフォワードモード
-
-
+/*BNO055モードについて
+ * ジャイロセンサーには様々なモードがありますが
+ * 基本的にNDOFモードを使って下さい
+ * 会場の磁場環境がひどい場合はIMUモードを使ってください
+ * ロボットの電源を起動した直後にキャリブレーションを行ってください
+ * キャリブレーションをすることでオウンゴールを防ぐことができます。
+ */
+/*change Mode IMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF*/
-//プロトタイプ宣言
-//****************************************************************//
-//関数名:balldeg(赤外線ボールの角度,赤外線ボールの距離)//
-//返り値:進行方向
-//****************************************************************//
-float balldeg(float ADeg, float ADis);
-
-
-
+//パラメータ調整項目
+#define MODE MODE_NDOF//ジャイロのモード
+const int mode_robot = FW;//FW:その場で停止 GK:ゴール前までもどる
+const int R = 100; //ロボット回り込み半径(0~255
+const int speed = 100; //(0~100)の間で調整
+const double tp = 1.5; //比例ゲイン
+const double ti = 1.5; //積分ゲイン
+const double td = 0.25; //微分ゲイン
-//*****************************************************************//
-//関数名:PIDpower(赤外線ボールの角度)
-//返り値:パワーの操作量を返す
-//*****************************************************************//
-float PIDpower(float BDeg);//パワーの操作量を返す(引数は上と同様)
-
+//TerminalDisplay(TeraTerm)
+Serial pc(SERIAL_TX, SERIAL_RX);
-
-
-//*****************************************************************//
-//関数名:PIDdegree(ジャイロの基準値)//////////////////
-//返り値:方位の修正角度
-//*****************************************************************//
-float PIDdegree(int aimdegree);//方位修正操作量を返す
+//voltage
+AnalogIn voltage(PC_4);
-
-//*****************************************************************//
-//関数名:talk(プリセット番号を入力)
-//返り値:無し
-//*****************************************************************//
-void talk(int pat);
-
+//UltraSonicSensor
+DigitalIn change(PA_6);
+Ping uss_left(PB_3);
+Ping uss_right(PA_7);
+Ping uss_back(PB_6);
+Timer timer_USS;
+Timer timer_volt;
-
+//BallSensor&Linesensor
+AnalogIn ball_degree(PA_4);
+AnalogIn ball_distance(PB_0);
+Serial sensor(PC_10, PC_11, 115200);
-//*****************************************************************//
-//関数名:call_cm(距離(整数)0~300)
-//返り値:無し
-//*****************************************************************//
-void call_cm(int range);
+//Motor
+Motor motor(PA_8,PB_4,PB_10, PB_15,PB_14,PB_13, PB_2,PB_1,PA_9, PA_12,PA_11,PA_5);
+
+//Timer
+Timer timer_PID;
-
-
-//******************************************************//
-//関数名:uss_check()
-//返り値無し
-//概要:タイマー繰り返し割り込みで使う
-//*******************************************************//
-int uss_check();
-
-
-
-//BNO055
-BNO055 imu(i2c,PC_8);
+//GyroSensor
+I2C i2c(D14, D15);
+BNO055 imu(i2c, PC_8);
BNO055_ID_INF_TypeDef bno055_id_inf;
-BNO055_EULER_TypeDef euler_angles;
+BNO055_EULER_TypeDef euler_angles;
-
+//HoldCheckSensor
+DigitalIn hold_check(PC_3);
-//その他
-DigitalIn ZigIn(PA_11);//通信
-DigitalOut ZigOut(PB_12);
+//kicker
+DigitalOut kick(PA_13);
+
+//ToggleSwitch
+DigitalIn sw_start(PD_2); //program start switch
+DigitalIn sw_reset(PC_12); //gyro sensor reset switch
+DigitalIn sw_kick(USER_BUTTON);
-
-
-int range;//超音波センサの距離グローバル変数
+//declear prototype (function list)
+void uss_send_and_read(); //超音波センサ読み込み
+int PID(double kp, double ki, double kd, int target, int degree, int reset = 0); //姿勢制御のPIDで計算する
+int get_ball_degree(int A = 0); //ボールの角度を-180~180(ボールなし999)で取得する
+int get_ball_distance(int A = 0); //ボールの距離を0~255で取得する
+int get_line_degree(); //反応したラインの方向(角度)を0~315(反応なし999)で取得する
+int turn(int degree, int distance, int target = 0, int angle = 0); //回り込みの進行方向を計算する
+int get_uss_range(char c); //超音波センサの距離を取得する 引数('l','r','b')を代入するとその方向の値が得られる
+bool check_voltage(); //電圧をチェックする
-
-//****************************************************/////
-//////////////////////プログラム主文/////////////////////////
-//*****************************************************/////
+/*****************************************************************/
+/**********************main function******************************/
+/*****************************************************************/
int main()
{
- //***************************************************////
- /////////////////////////初期設定/////////////////////////
- //***************************************************////
+ //while(imu.chip_ready() == 0);//imu set
+//**************************************************************//
+////////////////////////initialize setting////////////////////////
+//**************************************************************//
+ wait_ms(50);
+ kick = 0; //キックを解除する
- //////////モーターpwm周期設定////////////
- pico.baud(9600);
- pico.printf("?");
- motor.setPwmPeriod(0.03);
+ /*ultra sonic sensor set speed*/
+ uss_right.Set_Speed_of_Sound(32); //(cm/ms)
+ uss_left.Set_Speed_of_Sound(32); //(cm/ms)
+ uss_back.Set_Speed_of_Sound(32); //(cm/ms)
+
+ /*motor pwm frequency set*/
+ motor.setPwmPeriod(0.00005);
- /////////////モードチェンジIMU,COMPASS,M4G,NDOF_FMC_OFF,NDOF////////
+ /*Variable*/
+ int rotation, move, distance, degree, direction;
+ uint8_t config_profile[24];
+
+ motor.omniWheels(0, 0, 0);
+
+ /*gyro sensor */
imu.reset();
- imu.change_fusion_mode(MODE_IMU);
- wait(0.1);
+ wait_ms(100);
+ imu.change_fusion_mode(MODE);
+ wait_ms(200);
imu.get_Euler_Angles(&euler_angles);
- float basedegree = 0;//HMC6352read()かeuler_angles.h(ただしIMUのときは0)
- TimePID.start();
- TimePID.reset();//ジャイロリセット
- preTimedeg = TimePID.read();
- motor.omni4Wheels(0,0,0);
- /////////////////////////////////////////////////////////////////
+ int init_degree = (int) euler_angles.h;
-
-
-
+ while (1) {
+//***************************************************************//
+////////////////Play mode////////////
+//***************************************************************//
+ timer_USS.reset();
+ timer_USS.start();
+ timer_PID.reset();
+ timer_PID.start();
+ timer_volt.reset();
+ timer_volt.start();
+ PID(tp, ti, td, init_degree, init_degree, 1); //PIDの積分値をリセットする
- //************************************************************/////
- ////////////////////プログラムスタート////////////////////////////////
- ///////////////////////////////////////////////////////////////////
- //************************************************************/////
- int degreeB,range,direction;
- while(1) {
- uss.Send();
- wait_ms(40);
- range = uss.Read_cm();
- degreeB = (balldegree.read() - 0.1)*400 - 185;
- if(sw_right == 1) {
- if(balldegree.read() > 0.05) {
- if(degreeB <= -20) {
- motor.omni4Wheels(0,100,30);
- direction = 1;
- }
- else if(degreeB >= 20) {
- motor.omni4Wheels(0,100,-30);
- direction = 2;
- }
- else {
- if(range <= 100) {
- motor.omni4Wheels(0,0,0);
- talk(13);
- }
- else {
- motor.omni4Wheels(0,100,-1*degreeB/60);
+ while (sw_start == 0 && check_voltage() == 1) {
+ imu.get_Euler_Angles(&euler_angles); //ジャイロの値をしゅとくする
+ rotation = PID(tp, ti, td, init_degree, (int) euler_angles.h); //PIDを計算する
+ ///////////////////////
+ ////*ラインの制御*///////
+ ////////////////////
+ direction = get_line_degree();
+ if (direction != 999) {
+ int tmp = direction;
+
+ /*ラインが反応したときコートの中に戻る制御プログラム*/
+ while (direction != 999 && sw_start == 0 && check_voltage() == 1) {
+
+ imu.get_Euler_Angles(&euler_angles);
+ rotation = PID(tp, ti, td, init_degree,
+ (int) euler_angles.h, 0);
+ tmp = direction; //踏んだラインの方向を保存
+ if (direction == 90) {
+ if (get_uss_range('b') >= 50) {
+ direction = 240;
+ }
+ } else if (direction == 270) {
+ if (get_uss_range('b') >= 50) {
+ direction = 120;
+ }
+ } else if (direction <= 179) {
+ direction = direction + 180;
+ } else if (direction >= 180) {
+ direction = direction - 180;
}
+ motor.omniWheels(direction, speed, rotation);
+ direction = get_line_degree();
}
- }
- else {
- if(direction == 1){
- motor.omni4Wheels(0,0,100);
- }
- else if(direction == 2){
- motor.omni4Wheels(0,0,-100);
+
+ /*コートの中に戻った後、ボールが移動するまでその場で待機する*/
+ if (tmp >= 180) {
+ tmp = tmp - 360;
}
- else{
- motor.omni4Wheels(0,0,0);
+ degree = get_ball_degree();
+ while ((degree > (tmp - 45)) && (degree < (tmp + 45))
+ && sw_start == 0) {
+ imu.get_Euler_Angles(&euler_angles);
+ rotation = PID(tp, ti, td, init_degree,
+ (int) euler_angles.h);
+ motor.omniWheels(0, 0, rotation);
+ degree = get_ball_degree();
}
}
- }
- else {
- if(range <= 200) {
- talk(10);
- }
- direction = 0;
- motor.omni4Wheels(0,0,0);
- }
-
- }
-
-
+ ////////////////////////////////////
+ /*ボールを追いかけるときの制御プログラム*///////
+ ///////////////////////////////////
+ else {
+ int P = (int) euler_angles.h - init_degree; //proportional
+ if (P <= -180) { //convert to -180 ~ 180
+ P = P + 360;
+ }
+ if (P >= 180) {
+ P = P - 360;
+ }
+ /*ロボット向き修正*/
+ if (P <= -35) {
+ motor.setPower(60, 60, 60,60);
+ } else if (P >= 35) {
+ motor.setPower(-60, -60, -60,-60);
+ } else {
+ /*ボールの角度と距離を取得する*/
+ degree = get_ball_degree();
+ distance = get_ball_distance();
-}
-
-
+ ////////////////////
+ /*ボールが見つからないとき*/
+ ////////////////////
+ if (distance >= 240) {
+ /*
+ * GKモードの場合、ゴール前まで戻ってくる
+ * FWモードの場合、その場で停止する
+ *
+ */
+ switch (mode_robot) {
-//******************************************************////////
-///////////進行方向の角度を返す(ボールの角度,ボールの距離)////////////
-//********************************************************/////
-float balldeg(float ADeg, float ADis)
-{
+ case GK: { //GKモード
+ int uss_l = get_uss_range('l'), uss_r = get_uss_range('r'); //超音波センサの値を取得する
+ static float Vx = 0, Vy = 0;
- ball = ADeg - 185;//範囲は-180~180
-
-
- nowball = ball; //範囲は-360~360
+ Vy = get_uss_range('b') - 45; //y座標を取る
+ if ((uss_l + uss_r) > 120) { //ロボットが他のロボットと干渉していないときx座標を取る
+ Vx = 182 * (float) uss_l
+ / (float) (uss_l + uss_r) - 91;
+ } else {
+ Vx = 0; //x軸上で座標0にいる判定にする
+ }
- if(nowball> 180) { //範囲を-180~180に変換
- nowball = -1 * (360 - nowball);
- } else if(nowball< -180) {
- nowball = 360 - (-1 * nowball);
- }
+ if (Vy >= 0) { //ゴールの前にいないとき
+ int direction_of_going = (int) ((180.0 / PI)
+ * acos(Vy / sqrt(Vx * Vx + Vy * Vy))); //進行方向を計算する
+ if (Vx >= 20) { //右側にいるとき
+ direction_of_going = 180
+ + direction_of_going;
+ motor.omniWheels(direction_of_going,
+ speed / 2, rotation); //左後ろに移動
+ } else if (Vx <= -20) { //左側にいるとき
+ direction_of_going = 180
+ - direction_of_going;
+ motor.omniWheels(direction_of_going,
+ speed / 2, rotation); //右後ろに移動
+ } else { //真ん中付近にいるとき
+ motor.omniWheels(180, speed / 2, rotation); //後ろへ移動
+ }
+ } else { //ゴールの前にいるとき
+ if (Vx > 30) { //右側にいるとき
+ motor.omniWheels(-90, speed / 2, rotation); //左へ移動
+ } else if (Vx < -30) { //左側にいるとき
+ motor.omniWheels(90, speed / 2, rotation); //右へ移動
+ } else { //真ん中にいるとき
+ motor.omniWheels(0, 0, rotation); //停止
+ }
+ }
+ break;
+ }
- if((nowball >= -55) && (nowball <= 55)) { //ボールが前にあるとき
- return(2*ball);
- } else if((nowball <= -150) && (nowball >= 150)) {
- if(nowball >= 0) {
- Godeg = ball + 90;
- return(Godeg);
- } else {
- Godeg = ball - 90;
- return(Godeg);
- }
- } else {
- if(ADis <= r) { //ボールとの距離が近いとき(半径)
- if(nowball >= 0) {
- Godeg = ball + (180 - 180/Pi*asin(ADis / r));
- return(Godeg);
- } else {
- Godeg = nowball - (180 - 180/Pi*asin(ADis / r));
- return(Godeg);
- }
- } else {
- if(nowball >= 0) {
- Godeg = ball + 180/Pi * asin(r / ADis);//数値は半径
- return(Godeg);
- } else {
- Godeg = ball - 180/Pi * asin(r / ADis);
- return(Godeg);
+ case FW: //FWモード
+ motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
+ break;
+ default:
+ motor.omniWheels(0, 0, rotation); //ボールがないとき停止する
+ break;
+ }
+ }
+ /////////////////
+ /*ボールが見つかった時*/
+ /////////////////
+ else {
+
+ /*ホールドセンサーが反応したとき*/
+ static int count = 0; //タイマー代わりのカウンター
+ move = turn(degree, distance); //回り込み方向を計算する
+ motor.omniWheels(move, speed, rotation);
+ if (hold_check.read() == 0) { //0
+ if (count > 50) { //ホールドしてから0.1秒過ぎたとき時
+ kick = 1; //キックする
+ wait_ms(50); //キック待機時間
+ kick = 0; //キックを解除する
+ count = 0; //カウンタを0にする
+ } else {
+ wait_us(1); //マイクロ秒
+ count++;
+ }
+ } else {
+ count = 0;
+ }
+ }
+ }
}
}
- }
-}
+ timer_PID.stop();
+//***************************************************************//
+////////////////////////Gyro reset mode////////////////////////////
+//***************************************************************//
+ if (sw_reset == 0) {
+ imu.get_Euler_Angles(&euler_angles);
+ init_degree = (int) euler_angles.h;
+ }
+ motor.omniWheels(0, 0, 0);
-//**********************************************////
-///////////////PIDでパワーレベルを返す/////////////////
-//***********************************************///
-float PIDpower(float BDeg)
-{
+//***************************************************************//
+//////////////////////////debug mode///////////////////////////////
+//***************************************************************//
+ if (pc.readable() > 0) {
+ char command = pc.getc();
+ //if 'c' is pressed,calibration mode will start.
+ if (command == 'c') {
+ while (1) {
+ if (pc.readable() > 0) {
+ if (pc.getc() == 'r') {
+ break;
+ }
+ }
+ uint8_t status = 0, temp[3];
+ status = imu.read_calib_status();
- dtpow = TimePID.read() - preTimepow;//微小時間
- preTimepow = TimePID.read();
+ temp[0] = status << 2;
+ temp[1] = status << 4;
+ temp[2] = status << 6;
+ //pc.printf("%d\r\n",status);
+ pc.printf("SYS:%d, GYRO:%d, ACC:%d, MAG:%d \r", status >> 6,
+ temp[0] >> 6, temp[1] >> 6, temp[2] >> 6);
- Ppow = BDeg - 185; //ボールの角度
- Ipow += Ppow * dtpow; //積分
- if(Ppow < 0) {
- Ppow = Ppow*-1;
- }
- Dpow = (Ppow - prePpow)/dtpow;//微分
- prePpow = Ppow;
-
- B = Ppow * kppow + Ipow * kipow + Dpow * kdpow; //操作量
-
- if((Ppow >= -15) && (Ppow <= 15)) { //ボールが前にあるとき
- Ipow = 0;//積分初期化
- return(90);//パワー最大値
- } else if((Ppow >= 90)&&(Ppow <=-90)) {
- return(90);
- } else {
- if(B <= 60) {
- return(60);//パワー最小値
- } else if(B >= 90) {
- return(90);//パワー最大値
- } else {
- return(B);
+ //キャリブレーションプロファイル読みとり
+ if (temp[2] >> 6 == 3 && temp[0] >> 6 == 3 && status >> 6 == 3) {
+ imu.change_fusion_mode(CONFIGMODE);
+ for (int i = 0; i < 22; i++) {
+ config_profile[i] = imu.read_reg0(i + 0x55);
+ }
+ kick = 1;
+ wait_ms(200);
+ kick = 0;
+ break;
+ }
+ wait_ms(100);
+ }
+ imu.change_fusion_mode(MODE);
+ }
+ //if 'd' is pressed,debug mode will start.
+ if (command == 'd') {
+ while (1) {
+ if (pc.readable() > 0) {
+ if (pc.getc() == 'r') { //if 'r' is pressed,debug mode will be end.
+ break;
+ }
+ }
+ pc.printf(
+ "////////////////////////////////////////////\r\n");
+ pc.printf(
+ "/*******************debug******************/\r\n");
+ pc.printf(
+ "////////////////////////////////////////////\r\n\n");
+ imu.get_Euler_Angles(&euler_angles);
+ pc.printf("Gyro degree: %d \r\n\n", (int) euler_angles.h);
+ pc.printf("Ball degree: %d \r\n", get_ball_degree());
+ pc.printf("Ball distance: %d \r\n\n", get_ball_distance());
+ pc.printf("Line sensor: %d \r\n", get_line_degree());
+ pc.printf("Hold sensor: %d \r\n\n", hold_check.read());
+ pc.printf("USS left: %d cm\r\n", get_uss_range('l'));
+ pc.printf("USS right: %d cm\r\n", get_uss_range('r'));
+ pc.printf("USS back: %d cm\r\n\n", get_uss_range('b'));
+ sensor.putc('D'); //request Line data
+ while (sensor.readable() == 0)
+ ;
+ int f = sensor.getc();
+ while (sensor.readable() == 0)
+ ;
+ int b = sensor.getc();
+ while (sensor.readable() == 0)
+ ;
+ int r = sensor.getc();
+ while (sensor.readable() == 0)
+ ;
+ int l = sensor.getc();
+ pc.printf("Line front: %d\r\n", f);
+ pc.printf("Line back: %d\r\n", b);
+ pc.printf("Line left: %d\r\n", l);
+ pc.printf("Line right: %d\r\n\n", r);
+ pc.printf("batey voltage: %f\r\n", voltage.read() * 10.2);//8.15
+ pc.printf("\f");
+ wait_ms(300);
+ }
+ }
}
+//****************************************************************//
+//////////////////////calibration mode/////////////////////////////////////
+//****************************************************************//
+ if (sw_kick.read() != 1) {
+ uint8_t status = 0, temp[3] = { 0, 0, 0 };
+ /*ジャイロキャリブレーション*/
+ while (1) {
+ status = imu.read_calib_status();
+ temp[0] = status << 2;
+ if (temp[0] >> 6 == 3) {
+ kick = 1;
+ wait_ms(200);
+ kick = 0;
+ break;
+ }
+ }
+ /*コンパスキャリブレーション*/
+ /*(IMUモードの時は必要ないのでコメントアウトして)*/
+ while (MODE == MODE_NDOF) {
+ status = imu.read_calib_status();
+ temp[2] = status << 6;
+ pc.printf("%d\r",temp[2]>>6);
+ if (temp[2]>>6 == 3 && status >>6 == 3) {
+ break;
+ }
+ }
+ /*キャリブレーションプロファイル取得*/
+ imu.change_fusion_mode(CONFIGMODE);
+ wait_ms(100);
+ for (int i = 0; i < 22; i++) {
+ config_profile[i] = imu.read_reg0(i + 0x55);
+ }
+ imu.change_fusion_mode(MODE);
+ wait_ms(200);
+ kick = 1;
+ wait_ms(200);
+ kick = 0;
+ }
+ /**********************end main function**************************/
}
}
-//*****************************************************///
-///////////PID目標値(角度)を代入すると操作量を返す/////////////
-//*****************************************************///
-float PIDdegree(int aimdegree)
+/////////////////////////////////////////////////////////
+/*PID feedback*/
+//////////////////////////////////////////////////////////
+int PID(double kp, double ki, double kd, int target, int degree, int reset)
{
- dtdeg = TimePID.read() - preTimedeg;//微小時間
- preTimedeg = TimePID.read();
-
- imu.get_Euler_Angles(&euler_angles);//bno055角度取得
- Bnodeg = euler_angles.h;
- Pdeg = Bnodeg - aimdegree;//比例bno055
+ static double dt, time, pretime;
+ static double P = 0, OP = 0, I = 0, D = 0, re = 0;
- if(Pdeg > 180) { //角度値を-180~180に修正
- Pdeg = -1 * (360 - Pdeg);
- } else if(Pdeg < -180) {
- Pdeg = 360 - (-1 * Pdeg);
- }
- if(Bnodeg > 180) { //角度値を-180~180に修正
- Bnodeg = -1 * (360 - Bnodeg);
- } else if(Pdeg < -180) {
- Bnodeg = 360 - (-1 * Bnodeg);
- }
- Ideg += Pdeg * dtdeg;//積分
- Ddeg = (Pdeg - prePdeg)/dtdeg;//微分
- prePdeg = Pdeg;
- if((Pdeg > -2) && (Pdeg < 2)) {
- Pdeg = 0;
- Ideg = 0;
- Ddeg = 0;
-
+ if (reset == 1) {
+ I = 0;
+ time = 0;
+ pretime = 0;
}
- A = Pdeg * kpdeg + Ideg * kideg + Ddeg * kddeg;
+ time = timer_PID.read();
+ dt = time - pretime; //time
+ P = degree - target; //proportional
+ if (P <= -180) { //convert to -180 ~ 180
+ P = P + 360;
+ }
+ if (P >= 180) {
+ P = P - 360;
+ }
+ D = (P - OP) / dt; //differential
+ I += (P + OP) * dt / 2; //Integral
+ pretime = time;
+ OP = P;
+ re = -1 * (kp * P + ki * I + kd * D);
- if(A > 1) {
- A = 1;
- } else if(A < -1) {
- A = -1;
+ if (P >= -5 && P <= 5) {
+ I = 0;
}
- return(A);
+ if (re >= 60) {
+ re = 60;
+ } else if (re <= -60) {
+ re = -60;
+ }
+
+ return re;
}
-
-//************************************************************************//
-//関数名:talk(プリセット番号を入力)
-//返り値:無し
-//************************************************************************//
-
-void talk (int pat)
+/////////////////////////////////////
+/*turn control*/
+////////////////////////////////////
+int turn(int degree, int distance, int target, int angle)
{
- switch (pat) {
- case 0:
-
- pico.printf("hajimema'site\r");
- wait(2);
- break;
- case 1:
+ int going;
- pico.printf("watasiwa/meidoro'bo akemityandayo-?\r");
- wait(3);
- break;
- case 2:
-
- pico.printf("mataki'te;ne-?\r");
- wait(3);
- break;
- case 3:
-
- pico.printf("yo'-koso o-punkya'npasue\r");
- wait(3);
- break;
- case 4:
-
- pico.printf("yamete-?\r");
- wait(3);
- break;
- case 5:
-
- pico.printf("ame' do'-zo?\r");
- wait(3);
- break;
- case 6:
+ angle = angle - target; //proportional
+ if (angle <= -180) { //convert to -180 ~ 180
+ angle = angle + 360;
+ }
+ if (angle >= 180) {
+ angle = angle - 360;
+ }
- pico.printf("ko'uko'useinomina'san konnitiwa\r");
- wait(3);
- break;
- case 7:
-
- pico.printf("wa'tasiwane? ta'jirisenpaigatuku'ttandayo'.\r");
- wait(5);
- break;
- case 8:
-
- pico.printf("ze'hi ;iwatekenritudaigakunikitekudasai omatisi'teimasu.\r");
- wait(3);
- break;
- case 9:
-
- pico.printf("ariga'to-.\r");
- wait(3);
- break;
- case 10:
+ degree = degree + angle; //ジャイロの値を加算
+ if (degree >= -45 && degree <= 45) { //ボールがまえにあるとき
+ going = 3 * degree;
+ } else if (degree <= -160 && degree >= 160) { //ボールが真後ろにある時
+ if (degree >= 0) {
+ going = degree + 90;
+ } else {
+ going = degree - 90;
+ }
+ } else {
+ if (distance <= R) { //ボールとの距離が近すぎるとき
+ if (degree >= 0) {
+ going = degree + 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
+ } else {
+ going = degree - 90; //(int)(180.0 - ((180.0/PI)*asin((double)distance / (double)R)));
+ }
+ } else { //ボールとの距離が離れているとき
+ if (degree >= 0) {
+ going = degree
+ + (int) ((180.0 / PI)
+ * asin((double) R / (double) distance));
+ } else {
+ going = degree
+ - (int) ((180.0 / PI)
+ * asin((double) R / (double) distance));
+ }
+ }
+ }
+ if (get_uss_range('b') <= 60) { //後ろの壁に近いとき
+ if (going > 90) {
+ going = 90;
+ } else if (going < -90) {
+ going = -90;
+ }
+ }
+ return going - angle;
+}
- pico.printf("ohayo'-go'zaimasu.\r");
- wait(3);
- break;
- case 11:
+//////////////////////////////
+/*get ball degree*/
+//////////////////////////////
+int get_ball_degree(int A)
+{
+ static int value = 0;
- pico.printf("konnitiwa'\r");
- wait(3);
- break;
- case 12:
+ if (A == 1) { //analog read
+ if (ball_distance.read() >= (double) 0.95) {
+ return 999;
+ } else {
+ return (int) (ball_degree.read() * 360 - 185);
+ }
+ } else { //serial read
- pico.printf("kyo'umoitinitiotukaresa'ma;\r");
- wait(3);
- break;
+ /*get ball degree*/
+ int count = 0;
+ sensor.putc('B');
+ char buffer[2];
+ for (int i = 0; i < 2; i++) {
+ while (sensor.readable() == 0) { //wait
+ if (count >= 10000) {
+ return value;
+ }
+ wait_us(1);
+ count++;
+ }
+ buffer[i] = sensor.getc(); //get
+ }
+ value = ((uint16_t) buffer[0] << 8) + (uint8_t) buffer[1] - 180;
- case 13:
- pico.printf("o\mataseshimashita gosyujinsa'ma;\r");
- wait(5);
- break;
-
-
+ return value;
}
}
-//*****************************************************************//
-//関数名:call_cm(距離(整数)0~300)
-//返り値:無し
-//*****************************************************************//
-void call_cm(int range)
+//////////////////////////////
+/*get ball distance*/
+//////////////////////////////
+int get_ball_distance(int A)
{
- if(range>=300) {
- pico.printf("sanbyaku\r");
- } else if(range>=200) {
- range= range-200 ;
- pico.printf("nihyaku");
- } else if (range>=100) {
- range= range-100;
- pico.printf("hyaku");
- }
+ static int value = 0;
+ if (A == 1) { //analog read
+ return (uint8_t) (ball_distance.read() * 255);
+ } else { //serial read
+ int count = 0;
- if (range>=90) {
- range= range-90;
- pico.printf("kyuujyu-");
- } else if(range>=80) {
- range= range-80;
- pico.printf("hatijyu-");
- } else if(range>=70) {
- range= range-70;
- pico.printf("nanajyu-");
- } else if(range>=60) {
- range= range-60;
- pico.printf("rokujyu-");
- } else if(range>=50) {
- range= range-50;
- pico.printf("gojyu-");
- } else if(range>=40) {
- range= range-40;
- pico.printf("yonjyu-");
- } else if(range>=30) {
- range= range-30;
- pico.printf("sanjyu-");
- } else if(range>=20) {
- range= range-20;
- pico.printf("nijyu-");
- } else if(range>=10) {
- range= range-10;
- pico.printf("jyu-");
- }
-
- if(range==9) {
- pico.printf("kyuu\r");
- wait(3);
- } else if(range==8) {
- pico.printf("hati\r");
- wait(3);
- } else if(range==7) {
- pico.printf("nana\r");
- wait(3);
- } else if(range==6) {
- pico.printf("roku\r");
- wait(3);
- } else if(range==5) {
- pico.printf("go\r");
- wait(3);
- } else if(range==4) {
- pico.printf("yon\r");
- wait(3);
- } else if(range==3) {
- pico.printf("san\r");
- wait(3);
- } else if(range==2) {
- pico.printf("ni\r");
- wait(3);
- } else if(range==1) {
- pico.printf("iti\r");
- wait(3);
- } else {
- pico.printf("\r");
- wait(3);
+ /*get ball distance*/
+ sensor.putc('C');
+ while (sensor.readable() == 0) { //wait
+ if (count >= 10000) {
+ return value;
+ }
+ wait_us(1);
+ count++;
+ }
+ value = (int) sensor.getc(); //get
+ return value;
}
}
-//******************************************************//
-//関数名:uss_check()
-//返り値無し
-//概要:タイマー繰り返し割り込みで使う
-//*******************************************************//
-int uss_check()
+//////////////////////////////
+/*get line degree*/
+//////////////////////////////
+int get_line_degree()
{
- int range = uss.Read_cm();
- uss.Send();
- return range;
-}
\ No newline at end of file
+ static char value = 0;
+ static int degree = 0;
+ int count = 0;
+
+ /*get line value*/
+ sensor.putc('A');
+ while (sensor.readable() == 0) { //wait
+ if (count >= 10000) {
+ return degree;
+ }
+ wait_us(1);
+ count++;
+ }
+ value = sensor.getc(); //get
+
+ switch (value) {
+ case 'N':
+ degree = 999;
+ return 999;
+ //break;
+ case '0':
+ degree = 0;
+ return 0;
+ //break;
+ case '1':
+ degree = 45;
+ return 45;
+ //break;
+ case '2':
+ degree = 90;
+ return 90;
+ //break;
+ case '3':
+ degree = 135;
+ return 135;
+ //break;
+ case '4':
+ degree = 180;
+ return 180;
+ //break;
+ case '5':
+ degree = 225;
+ return 225;
+ //break;
+ case '6':
+ degree = 270;
+ return 270;
+ //break;
+ case '7':
+ degree = 315;
+ return 315;
+ //break;
+ default:
+ degree = 999;
+ return 999;
+ //break;
+ }
+}
+
+//////////////////////////////////////
+/*USS reading*/
+//////////////////////////////////////
+/////////////////////////////
+/*get uss range cm*/
+////////////////////////////
+int get_uss_range(char c)
+{
+ static int count = 0;
+ static int l = 0, r = 0, b = 0;
+
+ if (timer_USS.read() >= 0.04) {
+ l = uss_left.Read_cm();
+ r = uss_right.Read_cm();
+ b = uss_back.Read_cm();
+ uss_left.Send();
+ uss_right.Send();
+ uss_back.Send();
+ timer_USS.reset();
+ }
+
+ if (c == 'l' || c == 'L') {
+ return l;
+ } else if (c == 'r' || c == 'R') {
+ return r;
+ } else if (c == 'b' || c == 'B') {
+ return b;
+ } else {
+ return 999;
+ }
+}
+
+//////////////////////////////////////
+/*Battery check voltage*/
+//////////////////////////////////////
+bool check_voltage()
+{
+ static float sum[5] = { 8, 8, 8, 8, 8 };
+ static float Ave = 8;
+ static bool S = 1;
+ if (timer_volt.read() > 0.2) {
+ for (int i = 4; i > 0; i--) {
+ sum[i] = sum[i - 1];
+ }
+ sum[0] = voltage.read() * 9.9;
+ Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
+ if (Ave < 7.0) { //7.0V以下で自動遮断
+ S = 0;
+ } else {
+ S = 1;
+
+ }
+ timer_volt.reset();
+ }
+ return S;
+}
+