File content as of revision 24:9ee1440c6703:

#include "mbed.h"
#include "debug.h"
#include "microinfinity.h"
#include "sensors.h"
#include "moves.h"


int main()
{
    can1.frequency(1000000);
    SetupMoves();
    printf("reset standby\r\n");
    /*
    while(1) {
        get_dist_forward();
        get_dist_back();
        wait(0.1);
    }
    */
    reset();
    printf("bus standby\r\n");
    while(1) {
        if(bus_in.read() == 1) break;
    }
    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();
    //直進スタート
    motor_lo.setDutyLimit(0.6);
    motor_li.setDutyLimit(0.6);
    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(35.0);
    //段差乗り越え
    motor_lo.setDutyLimit(0.3);//0.5
    motor_li.setDutyLimit(0.3);
    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() > 80) 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);
    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
    //前進
    motor_lo.setDutyLimit(0.5);
    motor_li.setDutyLimit(0.5);
    for(int i=0; i<3; ++i) {
        while(get_dist_forward() > 65) {
            straightAndInfinity(90.0, 5.0);
        }
    }
    //ロープ前旋回
    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 \r\n",degree0,get_dist_forward());
    for(int i=0; i<3; ++i) {
        while(get_dist_forward() > 65) { //65
            straightAndInfinity(0, 5);
        }
    }
    printf("turn");
    turn(-90);

    while(get_dist_back() > 10.0) {}
    while(get_dist_back() < 30.0) {}

    printf("last spart!!!!!!!!");
    motor_lo.setDutyLimit(0.2);//0.5
    motor_li.setDutyLimit(0.2);

    for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);
    printf("wall standby");
    while(get_dist_back() < 250) {
        straightAndInfinity(-90, 5);
//        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);
//            DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
        }
    }
    hand_mode = GOAL;
    straight();
    printf("uhai!!!!!!!!!!!!!!!");
}