Diff: main.cpp

Revision:

18:d0a6771fa38d

Parent:

17:2b3fa9b1a05b

Child:

19:b162e7f4cc06

diff -r 2b3fa9b1a05b -r d0a6771fa38d main.cpp
--- a/main.cpp	Thu May 02 09:34:57 2019 +0000
+++ b/main.cpp	Thu May 02 09:39:31 2019 +0000
@@ -14,6 +14,14 @@
 */
 #define Pi 3.14159265359 //円周率π
 
+//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
+PIDcontroller pid_lo(0.01, 0.000, 0.000);
+PIDcontroller pid_li(0.01, 0.000, 0.000);    //Kp.Ki,Kd
+Motor motor_lo(&motor_lo_f, &motor_lo_b),
+      motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
+OneLeg leg_lo, leg_li;
+Robot robot;
+
 enum WalkMode {
     BACK,
     STOP,
@@ -25,8 +33,9 @@
     GOAL,
 };
 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
+int hand_mode=NORMAL;
+int step_num_l, step_num_r;
 
-////////////関数
 void reset();
 void setup();
 void set_gyro();
@@ -40,69 +49,25 @@
 void curveRight();
 void turn(float target_degree);//回転角, 収束許容誤差
 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
-
 void wait_gerege();
 
-////////////定数
-
-////////////変数
-int hand_mode=NORMAL;
-
-//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
-//しかし変更を多々行うためポインタ渡しにしてある
-//文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
-PIDcontroller pid_lo(0.01, 0.000, 0.000);
-PIDcontroller pid_li(0.01, 0.000, 0.000);    //Kp.Ki,Kd
-Motor motor_lo(&motor_lo_f, &motor_lo_b),
-      motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
-OneLeg leg_lo, leg_li;
-Robot robot;
-
-int step_num_l, step_num_r;
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 int main()
 {
     setup();
-
-    pid_lo.setTolerance(10);
-    pid_li.setTolerance(10);
-    motor_lo.setEncoder(&ec_lo);
-    motor_lo.setResolution(1000);
-    motor_li.setEncoder(&ec_li);
-    motor_li.setResolution(600);
-    leg_lo.setMotor(&motor_lo);
-    leg_lo.setPIDcontroller(&pid_lo);
-    leg_li.setMotor(&motor_li);
-    leg_li.setPIDcontroller(&pid_li);
-    robot.setLeg(&leg_lo, &leg_li);
-    robot.setTickerTime(0.01); //モータ出力間隔 0.01
-    motor_lo.setDutyLimit(0.5);//0.5
-    motor_li.setDutyLimit(0.5);
     printf("reset standby\n\r");
     reset();
     printf("bus standby\n\r");
-
     while(1) {
         if(bus_in.read() == 1) break;
     }
     printf("bus is %d\n\r", bus_in.read());
-
     wait(0.5);
-
     straight();
-
     wait_gerege();
-
     hand_mode = GEREGE;
-
     set_gyro();
-    //Sample
-    //スタート直進
     printf("dist:%.3f\r\n", get_dist_forward());
-
+    //直進スタート
     for(int i=0;i<3;++i){
         while(get_dist_back() < 280)
         {
@@ -180,8 +145,6 @@
     hand_mode = GOAL;
     straight();
     printf("uhai!!!!!!!!!!!!!!!");
-
-    
 }
 void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
 {
@@ -292,6 +255,21 @@
     switch_lo.mode(PullUp);
     switch_li.mode(PullUp);
     mode4.mode(PullUp);
+    
+    pid_lo.setTolerance(10);
+    pid_li.setTolerance(10);
+    motor_lo.setEncoder(&ec_lo);
+    motor_lo.setResolution(1000);
+    motor_li.setEncoder(&ec_li);
+    motor_li.setResolution(600);
+    leg_lo.setMotor(&motor_lo);
+    leg_lo.setPIDcontroller(&pid_lo);
+    leg_li.setMotor(&motor_li);
+    leg_li.setPIDcontroller(&pid_li);
+    robot.setLeg(&leg_lo, &leg_li);
+    robot.setTickerTime(0.01); //モータ出力間隔 0.01
+    motor_lo.setDutyLimit(0.5);//0.5
+    motor_li.setDutyLimit(0.5);
 
 }