File content as of revision 21:14133581387b:

#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);

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");
    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();
    printf("dist:%.3f\r\n", get_dist_forward());
    //直進スタート
    motor_lo.setDutyLimit(0.6);//0.5
    motor_li.setDutyLimit(0.6);
    for(int i=0; i<3; ++i) {
        while(get_dist_back() < 290) {
            straightAndInfinity(0, 5);
        }
    }
    //段差前旋回
    motor_lo.setDutyLimit(0.3);//0.5
    motor_li.setDutyLimit(0.3);
    turn(40.0);
    //段差乗り越え
    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);
    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("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
        }
    }
    //ロープ前旋回
    printf("before roop deg:%.3f\n\r",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());
    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");
    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);
        printf("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());
        }
    }
    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++;
    }
}