File content as of revision 5:c5a2a5cf600d:

#include "mbed.h"
#include "config.h"
#include "moter.h"
#include "PID.h"
#include "encoder.h"
#include "encoder2.h"
#include "Serialservo.h"
#include "XQ_servo.h"

Serial pc(USBTX, USBRX);
Serial kbtpc(PC_12 ,PD_2);//serial5
I2C master(sda, scl);
DigitalOut led2(LED2);//チェック用
PID S_pid(SP, SI, SD, 0.0);
PID R_pid(RP, RI, RD, 0.0);
PID L_pid(LP, LI, LD, 0.0);

Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
Encoder2 Enc_Arm(PF_1,PA_0,PA_1);

DigitalOut buzzer(PA_10);
DigitalOut lamp(PC_3);
Timer timer;
Serialservo servo1(PC_10, PC_11);//アームの肘
Serialservo servo2(PC_10, PC_11);//回転軸
XQ_servo servo_hand(PB_5);
DigitalOut dpin(PC_9);
DigitalIn distance(PC_2);
DigitalIn redIn(PC_4);
DigitalIn blueIn(PB_2);
DigitalIn micro_switch(PC_0);
DigitalOut air(PB_4);

DigitalOut motor_dir1(PA_8); 
DigitalOut motor_dir2(PA_9);
PwmOut motor_pwm(PB_3);

void linecheck(char *buff ,int data[2]);
void LineCheck(int dmode);
void syokikanomai(void);
void syokikanomai2(void);
void pre_touteki(void);
void touteki(float armpower, int pos, bool thro_mode);

PwmOut pwm_pins[] = {
    PwmOut( PC_8 ),
    PwmOut( PC_6 ),
    PwmOut( PB_14 ),
    PwmOut( PB_13 )
};

DigitalOut dir_pins_1[] = {
    DigitalOut( PC_5 ),
    DigitalOut( PA_7 ),
    DigitalOut( PB_6 ),
    DigitalOut( PB_1)
};

DigitalOut dir_pins_2[] = {
    DigitalOut( PB_12 ),
    DigitalOut( PA_6 ),
    DigitalOut( PC_7 ),
    DigitalOut( PB_15)
};

int crosscount = 0;
bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
double power = POWER;
double rpower = POWER;
double lpower = POWER;
int kbtread = 0;
int count = 0;
int counter = 0;
int linedata_1[2] = {0};
int linedata_2[2] = {0};
int linedata_3[2] = {0};
int prelinedata = 0;
double output = 0.0;
char buff1[2];
char buff2[2];
char buff3[2];
float x_dist = 0.0;
float y_dist = 0.0;
float tar_x_dist = 0.0;
float tar_y_dist = 0.0;
bool ontheline = false;
int directmode = STRAIGHT;
int state = START;
int times = 0;
bool throwing = false;
int receive = NOT_RECEIVE;
int redcount = 0;
int bluecount = 0;
int yellowcount = 0;
bool syokika_flag=false;
bool set_flag = false;
int encoder_count = 1000;

int main() {
    timer.reset();
    timer.stop();
    servo1.init(0);//アーム
    servo2.init(1);//回転軸
    dpin=0;
    lamp=1;
    air=1.0;
    buzzer = 0.0;
    //motor_relay = 0.0;
    micro_switch.mode(PullUp);
    
    motor_pwm=0;
    motor_dir1=1;     //dir1,dir2はこの組み合わせじゃないと死ぬ
    motor_dir2=1;
    
    S_pid.init();
    R_pid.init();
    L_pid.init();
    pwm_pins[0].period(0.00005);
    pwm_pins[1].period(0.00005);
    pwm_pins[2].period(0.00005);
    pwm_pins[3].period(0.00005);
    motor_pwm.period(0.00005);
    pc.baud(115200);
    kbtpc.baud(9600);
    master.frequency(100000);
    lamp = 1.0;    
//-------初期化----------------------------------------
    syokikanomai();
    buzzer = 0;
//-----------メインループ----------------------------------------め
    while (true) {
        master.read(addr1,buff1,2);
        linecheck(buff1,linedata_1);
        master.read(addr2,buff2,2);
        linecheck(buff2,linedata_2);
        master.read(addr3,buff3,2);
        linecheck(buff3,linedata_3);
        x_dist = Enc_Carry_X.read_rotate();
        y_dist = Enc_Carry_Y.read_rotate();
        kbtpc.putc(state);
        if(kbtpc.readable()){
            kbtread = kbtpc.getc();
        }
        if(kbtread == 255){
            mode = true;
        }else if(kbtread == 150){
            mode = false;
        }else if(kbtread == 100){
            stopflag = true;
        }else if(kbtread == 200){
            stopflag = false;
        }else if(kbtread == STRAIGHT){
            directmode = STRAIGHT;
        }else if(kbtread == RIGHT){
            directmode = RIGHT;
        }else if(kbtread == LEFT){
            directmode = LEFT;
        }else if(kbtread == MANUAL){
            state = MANUAL;
        }else if(kbtread == START){
            state = START;
        }else if(kbtread == START_TEST){
            state = START_TEST;
        }
//--------------手動--------------------------------------
        if(mode == true){
            led2 = 1.0;
            if(kbtread == STRAIGHT){
                Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == BACK){
                Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == RIGHT){
                Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else if(kbtread == LEFT){
                Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
            }else{
                Stop(pwm_pins, dir_pins_1, dir_pins_2);
            }
//--------------自動---------------------------------
        }else if(mode == false){
            led2 = 0.0;
            if(stopflag == true){
                Stop(pwm_pins, dir_pins_1, dir_pins_2);
            }else{
                LineCheck(directmode);
                switch(state){
//------------------任意に進行方向を決めれる----------------------------------------
                    case MANUAL:
                    if(crosscount == 1){
                        directmode = STOP;
                        Stop(pwm_pins, dir_pins_1, dir_pins_2);
                        crosscount = 0;
                    }
                    break;
//------------------テスト用--------------------------------------------------------
                    case START_TEST:
                    if(crosscount == 0){
                        directmode = RIGHT;
                    }else if(crosscount == 1){
                        directmode = STOP;
                        state = STAND_BY_TEST;
                        crosscount = 0;
                        prelinedata = 0;
                    }
                    break;
//------------------テスト用待機位置-----------------------------------------------
                    case STAND_BY_TEST:
                    directmode = STOP;
                    if(distance == 1.0){
                        led2 = 1.0;
                        if(redIn == 1.0){
                            buzzer = 1.0;
                            redcount++;
                        }else if(blueIn == 1.0){
                            buzzer = 1.0;
                            bluecount++;
                        }else{
                            buzzer = 0.0;
                        }
                        if(redcount >= 50){
                            servo_hand.move(hand_tsukami_degree);
                            //pre_touteki();
                        }else if(bluecount >= 50){
                            servo_hand.move(hand_tsukami_degree);
                            //pre_touteki();
                        }
                    }else if(distance == 0.0 && redcount >= 50){
                        servo2.move(koshi_nage_degree);
                        state = THROWING_TEST;
                        directmode = STOP;
                        led2 = 0.0;
                        redcount = 0;
                        crosscount = 0;
                    }else if(distance == 0.0 && bluecount >= 50){
                        servo2.move(koshi_nage_degree);
                        state = THROWING_TEST;
                        directmode = STOP;
                        bluecount = 0;
                        led2 = 0.0;
                        crosscount = 0;
                    }
                    break;
//------------------------------------------------------------------------------
                    case THROWING_TEST:
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1){
                        directmode = STOP;
                        touteki(0.8, 190, 0);
                        //syokikanomai();
                        buzzer = 0.0;
                    }
                    break;
//------------------初期位置から待機位置1------------------------------------------
                    case START:
                    if(crosscount == 0){
                        directmode = STRAIGHT;
                    }else if(crosscount == 1 && directmode == STRAIGHT){
                        directmode = LEFT;
                        prelinedata = 0;
                    }else if(crosscount == 1 && directmode == LEFT){
                        directmode = LEFT;
                    }else if(crosscount == 2){
                        directmode = STOP;
                        state = STAND_BY_1;
                        crosscount = 0;
                        prelinedata = 0;
                    }
                    break;
//------------------待機位置1----------------------------------------------------
                    case STAND_BY_1:
                    crosscount = 0;
                    directmode = STOP;
                    if(distance == 1.0){
                        if(redIn == 1.0){
                            buzzer = 1.0;
                            redcount++;
                        }else if(blueIn == 1.0){
                            buzzer = 1.0;
                            bluecount++;
                        }else{
                            buzzer = 0.0;
                        }
                        if(redcount >= 100 ){
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }else if(bluecount >= 100){
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }
                    }else if(distance == 0.0 && redcount >= 100){
                        servo2.move(koshi_nage_degree);
                        state = RED_RECEIVE;
                        directmode = STOP;
                        redcount = 0;
                        crosscount = 0;
                    }else if(distance == 0.0 && bluecount >= 100){
                        state = BLUE_RECEIVE_1;
                        directmode = STOP;
                        bluecount = 0;
                        crosscount = 0;
                    }
                    break;
//------------------待機位置1から投てき位置1----------------------------------------
                    case RED_RECEIVE:
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1){
                        directmode = STOP;
                        prelinedata = 0;
                        touteki(0.8,190,0);
                        if(set_flag == false){
                            directmode = RIGHT;
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        syokikanomai2();
                        if(set_flag == true){
                            state = STAND_BY_1;
                            //throwing = false;
                            crosscount = 0;
                        }
                    }
                    break;
//------------------待機位置1から投てき位置2----------------------------------------
                    case BLUE_RECEIVE_1:
                    if(crosscount == 0){
                        directmode = RIGHT;
                    }else if(crosscount == 1){
                        directmode = STRAIGHT;
                    }else if(crosscount == 2){
                        directmode = LEFT;
                    }else if(crosscount == 3){
                        directmode = STOP;
                        prelinedata = 0;
                        state = BLUE_RECEIVE_2;
                        crosscount = 0;
                    }
                    break;
//------------------待機位置2----------------------------------------------------
                    case STAND_BY_2:
                    crosscount = 0;
                    directmode = STOP;
                    if(distance == 1.0){
                        if(blueIn == 1.0){
                            buzzer = 1.0;
                            bluecount++;
                        }else if(micro_switch == 1.0){
                            buzzer = 1.0;
                            yellowcount++;
                        }else{
                            buzzer = 0.0;
                        }
                        if(bluecount >= 100){
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }else if(yellowcount >= 500){
                            air = 0.0;
                        }
                    }else if(distance == 0.0 && bluecount >= 100){
                        servo2.move(koshi_nage_degree);
                        state = BLUE_RECEIVE_2;
                        directmode = STOP;
                        bluecount = 0;
                        crosscount = 0;
                    }
                    break;
//------------------投てき位置2---------------------------------------------------
                    case BLUE_RECEIVE_2:
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1){
                        directmode = STOP;
                        prelinedata = 0;
                        touteki(1.0,190,1);
                        if(set_flag == false){
                            directmode = RIGHT;
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        syokikanomai2();
                        if(set_flag == true){
                            state = STAND_BY_2;
                            crosscount = 0;
                        }
                    }                   
                    break;
//------------------投てき位置3---------------------------------------------------
                    case YELLOW_RECEIVE:
                    if(crosscount != 4){
                        directmode = LEFT;
                    }else{
                        directmode = STOP;
                    }
                    break;
                    default:
                    break;
                }
//--------------進行方向---------------------------------------------------------
                switch(directmode){
                    case STOP:
                    Stop(pwm_pins, dir_pins_1, dir_pins_2);
                    break;
                    case STRAIGHT:
                    output = S_pid.compute((double)linedata_1[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case RIGHT:
                    output = R_pid.compute((double)linedata_2[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case LEFT:
                    output = L_pid.compute((double)linedata_3[0]);
                    if(output >= 0){
                        rpower = power;
                        lpower = power + output;
                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        rpower = power + (-1 * output);
                        lpower = power;
                        Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    default:
                    break;
                }
            }
        }
        //pc.printf("kbtread = %d \r" ,kbtread); 
        //pc.printf("buff1[0] = %d \r" ,buff1[0]);
        //pc.printf("buff2[0] = %d \r" ,buff2[0]);
        //pc.printf("line1 = %d \r" ,linedata_1[0]);
        //pc.printf("line2 = %d \r" ,linedata_3[1]);
        pc.printf("xdist = %f \r",x_dist);
        //pc.printf("tar_xdist = %f \r",tar_x_dist);
        //pc.printf("ontheline = %d \r",ontheline);
        
        pc.printf("ydist = %f \r",y_dist);
        //pc.printf("tary = %f \r",tar_y_dist);
        //pc.printf("stopflag = %d \r",stopflag);
        //pc.printf("cross = %d \r",crosscount);
        //pc.printf("buff[1] = %d" ,buff1[1]);
        //pc.printf("line    = %d" ,line);
        //pc.printf("rpower    = %lf" ,rpower);
        //pc.printf("lpower    = %lf" ,lpower);  
        //pc.printf("dicmode = %d \r",directmode);
        pc.printf("output  = %lf \n" ,output);
    }
}

//---------------初期化---------------------------


void syokikanomai(void){
    //timer.start();
    //int times = timer.read();
    syokika_flag = false;
    while (set_flag==false) {
        encoder_count= Enc_Arm.read_rotate(); 
        servo_hand.move(hand_uke_degree);
        servo1.move(hiji_nage_degree);//退避
        servo2.move(koshi_uke_degree);
        //motor_relay = 0;
        motor_dir1=0;
        motor_dir2=1;
        motor_pwm=0.15;
 
        if(Enc_Arm.read_z() == 1) {
            syokika_flag=true;
            buzzer = 1.0;
        }
        
        if (syokika_flag==true) {
            if(encoder_count == 170) {
                motor_pwm=0;
                motor_dir1=1;
                motor_dir2=1;
                set_flag=true;  //投てき後，falseにする.
            }
        }
    }
    //if(times >= 1.0){
    //wait(0.5);
    //servo_hand.move(hand_uke_degree);
    servo1.move(hiji_uke_degree);
    servo1.move(hiji_uke_degree);
    //servo2.move(koshi_uke_degree);
    //}
}

void syokikanomai2(void){
    //timer.start();
    //int times = timer.read();
    //syokika_flag = false;
    if(set_flag==false) {
        encoder_count= Enc_Arm.read_rotate(); 
        servo_hand.move(hand_uke_degree);
        servo1.move(hiji_nage_degree);//退避
        servo2.move(koshi_uke_degree);
        //motor_relay = 0;
        motor_dir1=0;
        motor_dir2=1;
        motor_pwm=0.15;
 
        if(Enc_Arm.read_z() == 1) {
            syokika_flag = true;
            buzzer = 1.0;
        }
        
        if (syokika_flag==true) {
            if(encoder_count == 170) {
                motor_pwm=0;
                motor_dir1=1;
                motor_dir2=1;
                servo1.move(hiji_uke_degree);
                servo1.move(hiji_uke_degree);
                //syokika_flag = false;
                set_flag = true;  //投てき後，falseにする.
            }
        }
    }
    //if(times >= 1.0){
    //wait(0.5);
    //servo_hand.move(hand_uke_degree);
    //servo2.move(koshi_uke_degree);
    //}
}

void pre_touteki(void){
    /*
    while(Enc_Arm.read_rotate()<pos) {
        motor_dir1 = 0;
        motor_dir2 = 1;
        motor_pwm = 0.15;
    }
    motor_dir1=1;
    motor_dir2=1;
    motor_pwm=0;
    */
    //wait(1.0);
    servo_hand.move(hand_tsukami_degree);
    
    //wait(1.0);
    servo2.move(koshi_nage_degree);
    //servo1.move(hiji_tsukami_degree);
}

void touteki(float armpower ,int pos, bool thro_mode){
    if(set_flag == true){
        timer.start();
    }
    int times = timer.read(); 
    //wait(1.0);
    if(times >= 1.0 && times < 2.0){
    servo_hand.move(hand_uke_degree);
    }
    //wait(1.0);
    //lamp = 0.0;
    
    if(times >= 2.0 && times < 3.0){
    servo1.move(hiji_nage_degree);
    }
    
    //wait(1.0);
    if(times >= 3.0 && times < 5.0){
        lamp = 0.0;
        while(Enc_Arm.read_rotate() < pos) {
            motor_dir1 = 0;
            motor_dir2 = 1;
            motor_pwm = 0.15;
        }
    }
    motor_dir1=1;
    motor_dir2=1;
    motor_pwm=0;
    //wait(2.0);
//----投てき-------------------------
    if(thro_mode == 0.0){
        if(times >= 5.0 && times < 6.0){
            motor_dir1 = 0;
            motor_dir2 = 1;
            motor_pwm = armpower;
        }
    }else if(thro_mode == 1.0){
        if(times >= 5.0 && times < 5.5){
            dpin = 1.0;
        }else{
            dpin = 0.0;
        }
    }
    //wait(1.0);
    if(times >= 6.0 && times <7.0){
        motor_pwm = 0.0;
        motor_dir1 = 0;
        motor_dir2 = 0;
        buzzer = 0;
        lamp = 1.0;
        set_flag = false;
        timer.reset();
        timer.stop();
    }
}

void LineCheck(int dmode){
    if(dmode == STRAIGHT){
        if(linedata_1[1] == 1){
            ontheline = true;
            tar_y_dist = y_dist;
        }
        if(ontheline == true){
            if((y_dist - tar_y_dist) >= Y_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == RIGHT){
        if(linedata_2[1] == 1){
            ontheline = true;
            tar_x_dist = x_dist;
        }
        if(ontheline == true){
            if((x_dist - tar_x_dist) >= X_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }else if(dmode == LEFT){
        if(linedata_3[1] == 1){
            ontheline = true;
            tar_x_dist = x_dist;
        }
        if(ontheline == true){
            if((tar_x_dist - x_dist) >= X_STOP_DIST){
                ontheline = false;
                crosscount++;
            }
        }
    }
}

void linecheck(char *buff, int data[2]){
    if(buff[0] == 24){
        data[0] = 0;
        data[1] = 0;
    }else if(buff[0] == 56){
        data[0] = 1;
        data[1] = 0;
    }else if(buff[0] == 48){
        data[0] = 2;
        data[1] = 0;
    }else if(buff[0] == 112){
        data[0] = 3;
        data[1] = 0;
    }else if(buff[0] == 96){
        data[0] = 4;
        data[1] = 0;
    }else if(buff[0] == 224){
        data[0] = 5;
        data[1] = 0;
    }else if(buff[0] == 192){
        data[0] = 6;
        data[1] = 0;
    }else if(buff[0] == 128){
        data[0] = 7;
        data[1] = 0;
    }else if(buff[0] == 28){
        data[0] = -1;
        data[1] = 0;
    }else if(buff[0] == 12){
        data[0] = -2;
        data[1] = 0;
    }else if(buff[0] == 14){
        data[0] = -3;
        data[1] = 0;
    }else if(buff[0] == 6){
        data[0] = -4;
        data[1] = 0;
    }else if(buff[0] == 7){
        data[0] = -5;
        data[1] = 0;
    }else if(buff[0] == 3){
        data[0] = -6;
        data[1] = 0;
    }else if(buff[0] == 1){
        data[0] = -7;
        data[1] = 0;
    }else if(buff[0] == 255){
        data[0] = prelinedata;
        data[1] = 1;
    }else if(buff[0] == 0){
        data[0] = prelinedata;
        data[1] = 0;
    }else{
        data[0] = prelinedata;
        data[1] = 0;
    }
    prelinedata = data[0];
}