Diff: main.cpp

Revision:

22:0ed9de464f40

Parent:

21:14133581387b

Child:

24:9ee1440c6703

--- a/main.cpp	Fri May 03 02:21:47 2019 +0000
+++ b/main.cpp	Fri May 03 09:34:05 2019 +0000
@@ -1,110 +1,63 @@
 #include "mbed.h"
 #include "debug.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
-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;
-CAN can1(pin_can_rd,pin_can_td);
-
-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);
+#include "moves.h"
 
-DigitalOut led4(LED4);
-
-
-
-enum WalkMode {
-    BACK,
-    STOP,
-    FORWARD,
-};
-enum EVENT {
-    NORMAL,
-    GEREGE,
-    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();
-
-void can_send(int mode, float duty);
-void straight();
-void turnLeft();
-void curveLeft();
-void turnRight();
-void curveRight();
-void turn(float target_degree);//回転角, 収束許容誤差
-void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
-void wait_gerege();
 
 int main()
 {
-
-    setup();
-    printf("reset standby\n\r");
+    can1.frequency(1000000);
+    SetupMoves();
+    printf("reset standby\r\n");
+    /*
+    while(1) {
+        get_dist_forward();
+        get_dist_back();
+        wait(0.1);
+    }
+    */
     reset();
-    printf("bus standby\n\r");
+    printf("bus standby\r\n");
     while(1) {
         if(bus_in.read() == 1) break;
     }
-    printf("bus is %d\n\r", bus_in.read());
+    printf("bus is %d\r\n", bus_in.read());
     wait(0.5);
+    motor_lo.setDutyLimit(0.3);
+    motor_li.setDutyLimit(0.3);
     straight();
     wait_gerege();
     hand_mode = GEREGE;
     set_gyro();
-    printf("dist:%.3f\r\n", get_dist_forward());
     //直進スタート
-    motor_lo.setDutyLimit(0.6);//0.5
+    motor_lo.setDutyLimit(0.6);
     motor_li.setDutyLimit(0.6);
-    for(int i=0; i<3; ++i) {
-        while(get_dist_back() < 290) {
+    for(int i = 0; i < 25; i++)
+        straightAndInfinity(0, 5);
+        
+    for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
+        lowpassfilter[i].SetOldWeight(0);
+    for(int i=0; i<2; ++i) {
+        while(get_dist_back() < 320) {
             straightAndInfinity(0, 5);
         }
     }
     //段差前旋回
     motor_lo.setDutyLimit(0.3);//0.5
     motor_li.setDutyLimit(0.3);
-    turn(40.0);
+    turn(35.0);
     //段差乗り越え
     motor_lo.setDutyLimit(0.3);//0.5
     motor_li.setDutyLimit(0.3);
-    for(int i= 0; i<5; ++i) straight();
+    while(get_dist_back() < 80) {
+        straightAndInfinity(0, 5);
+    }
+    for(int i= 0; i<10; ++i) straight();
     //filter切る
     for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
         lowpassfilter[i].SetOldWeight(0);
-    while(get_dist_back() > 40) straight();
+    while(get_dist_back() > 50) straight();
     //filter軽く
     for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
         lowpassfilter[i].SetOldWeight(kOldWeightLight);
@@ -118,23 +71,20 @@
     for(int i=0; i<3; ++i) {
         while(get_dist_forward() > 65) {
             straightAndInfinity(90.0, 5.0);
-            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
         }
     }
     //ロープ前旋回
-    printf("before roop deg:%.3f\n\r",degree0);
+    printf("before roop deg:%.3f\r\n",degree0);
     turn(0);
 
     //ロープ
     while(mode4.read() == 0) {
-
         straightAndInfinity(0, 5);
     }
-    printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward());
+    printf("go to uhai deg:%.3f forward dist:%.3f \r\n",degree0,get_dist_forward());
     for(int i=0; i<3; ++i) {
         while(get_dist_forward() > 65) { //65
             straightAndInfinity(0, 5);
-            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
     }
     printf("turn");
@@ -144,7 +94,6 @@
     while(get_dist_back() < 30.0) {}
 
     printf("last spart!!!!!!!!");
-
     motor_lo.setDutyLimit(0.2);//0.5
     motor_li.setDutyLimit(0.2);
 
@@ -152,188 +101,15 @@
     printf("wall standby");
     while(get_dist_back() < 250) {
         straightAndInfinity(-90, 5);
-        printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+//        DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
     }
     for(int i=0; i<2; ++i) {
         while(get_dist_forward() > 65) {
             straightAndInfinity(-90, 5);
-            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+//            DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
         }
     }
     hand_mode = GOAL;
     straight();
     printf("uhai!!!!!!!!!!!!!!!");
-}
-void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
-{
-    if(target_degree - degree0 > 0) {
-        while(target_degree - degree0 > 0)
-            turnLeft();
-    } else {
-        while(target_degree - degree0 < 0)
-            turnRight();
-    }
-    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
-}
-void straightAndInfinity(float target_degree, float tolerance_degree)
-{
-    if(target_degree - degree0 > tolerance_degree) curveLeft();
-    else if(degree0 - target_degree > tolerance_degree) curveRight();
-    else straight();
-}
-void straight()
-{
-    can_send(FORWARD, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+step_num_l*180);
-    leg_li.setTargetPose(180+step_num_l*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    while(1) {
-        if(bus_in.read() == 1) break;
-    }
-    step_num_l++;
-    step_num_r++;
-}
-void turnLeft()
-{
-    can_send(FORWARD, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+(step_num_l-2)*180);
-    leg_li.setTargetPose(180+(step_num_l-2)*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    while(1) {
-        if(bus_in.read() == 1) break;
-    }
-    step_num_r++;
-    step_num_l--;
-}
-void curveLeft()
-{
-    can_send(FORWARD, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+(step_num_l-1)*180);
-    leg_li.setTargetPose(180+(step_num_l-1)*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    while(1) {
-        if(bus_in.read() == 1) break;
-    }
-    step_num_r++;
-}
-void turnRight()
-{
-    can_send(BACK, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+step_num_l*180);
-    leg_li.setTargetPose(180+step_num_l*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    while(1) {
-        if(bus_in.read() == 1) break;
-    }
-    step_num_r--;
-    step_num_l++;
-}
-void curveRight()
-{
-    can_send(STOP, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+step_num_l*180);
-    leg_li.setTargetPose(180+step_num_l*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    while(1) {
-        if(bus_in.read() == 1) break;
-    }
-    step_num_l++;
-}
-
-
-void setup()
-{
-    can1.frequency(1000000);
-    motor_lo_f.period_us(100);
-    motor_lo_b.period_us(100);
-    motor_li_f.period_us(100);
-    motor_li_b.period_us(100);
-
-    hand.mode(PullUp);
-    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);
-
-}
-
-void set_gyro()
-{
-    device.baud(115200);
-    device.format(8,Serial::None,1);
-    device.attach(dev_rx, Serial::RxIrq);
-    wait(0.05);
-    theta0=degree0;
-    check_gyro();
-}
-
-
-//////////////////////////////////////can
-void can_send(int mode, float duty)
-{
-    char data[2]= {0};
-    int send=mode * 10 + (int)(duty * 10.0);
-    //printf("duty is %.3f\n\r",duty);
-    data[0]=send & 0b11111111;
-    data[1]=hand_mode;
-
-    if(can1.write(CANMessage(0,data,2)))led4=1;
-    else led4=0;
-}
-void reset()
-{
-    while(switch_lo.read()) {
-        motor_lo.output(0.3);
-    }
-    ec_lo.reset();
-    motor_lo.output(0.0);
-    while(switch_li.read()) {
-        motor_li.output(0.3);
-    }
-    ec_li.reset();
-    motor_li.output(0.0);
-}
-
-
-
-void wait_gerege()
-{
-    int i = 0;
-    while(i!=100) {
-        if(hand.read()==0)i++;
-    }
-}
+}
\ No newline at end of file