Diff: main.cpp

Revision:

21:14133581387b

Parent:

20:e30e6e175991

Child:

22:0ed9de464f40

--- a/main.cpp	Fri May 03 01:46:30 2019 +0000
+++ b/main.cpp	Fri May 03 02:21:47 2019 +0000
@@ -1,10 +1,18 @@
 #include "mbed.h"
 #include "debug.h"
-#include "pin.h"
+#include "pinnames.h"
 #include "microinfinity.h"
 #include "robot.h"
+#include "sensors.h"
 #define Pi 3.14159265359 //円周率π
 
+const float kOldWeightLight = 0.3; //filterの重み.軽いver
+
+
+PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
+PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
+PwmOut motor_li_f(pin_pwm[1][0]); //モータ正転
+PwmOut motor_li_b(pin_pwm[1][1]); //モータ逆転
 //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
@@ -12,10 +20,23 @@
       motor_li(&motor_li_f, &motor_li_b); //forward,backのピンを代入
 OneLeg leg_lo, leg_li;
 Robot robot;
+CAN can1(pin_can_rd,pin_can_td);
 
-//filter [0]:forward, [1]: back
-const float kOldWeight = 0.5;
-LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
+DigitalIn bus_in(pin_bus);
+
+const int kResolution=500;
+Ec ec_lo(p7,p8,NC,kResolution,0.01);
+Ec ec_li(p6,p5,NC,kResolution,0.01);
+
+
+DigitalIn hand(p20);
+DigitalIn switch_lo(p25);
+DigitalIn switch_li(p26);
+DigitalIn mode4(p27);
+
+DigitalOut led4(LED4);
+
+
 
 enum WalkMode {
     BACK,
@@ -31,11 +52,12 @@
 int hand_mode=NORMAL;
 int step_num_l, step_num_r;
 
+
+
 void reset();
 void setup();
 void set_gyro();
-double get_dist_forward();
-double get_dist_back();
+
 void can_send(int mode, float duty);
 void straight();
 void turnLeft();
@@ -45,7 +67,6 @@
 void turn(float target_degree);//回転角, 収束許容誤差
 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
 void wait_gerege();
-float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
 
 int main()
 {
@@ -80,7 +101,13 @@
     motor_lo.setDutyLimit(0.3);//0.5
     motor_li.setDutyLimit(0.3);
     for(int i= 0; i<5; ++i) straight();
+    //filter切る
+    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
+        lowpassfilter[i].SetOldWeight(0);
     while(get_dist_back() > 40) straight();
+    //filter軽く
+    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
+        lowpassfilter[i].SetOldWeight(kOldWeightLight);
     //段差後旋回
     printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     turn(90.0);
@@ -302,26 +329,6 @@
 }
 
 
-float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
-{
-    sensor.start();
-    float raw_data = sensor.get_dist_cm();
-    if(raw_data < 0.1)//0.1以下なら前回の値を返す
-        return filter.GetData();
-    filter.SetData(raw_data);
-    float dist = filter.GetData();
-//    DEBUG("raw %.3f, filter %.3f\r\n", raw_data,dist);
-    return dist;
-}
-
-double get_dist_forward()
-{
-    return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
-}
-double get_dist_back()
-{
-    return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
-}
 
 void wait_gerege()
 {