File content as of revision 10:01aa50804d85:

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

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


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

bool set_pos_flag_2 = false;
bool set_pos_flag = false;
bool redthrowing = false;
bool yellow_throw_flag = false;
bool taiki_flag = false;
int crosscount = 0;
bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
double power = 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_x_dist_2 = 0.0;
float tar_x_dist_3 = 0.0;
float tar_y_dist = 0.0;
bool ontheline = false;
int directmode = STRAIGHT;
int state = START;
int times = 0;
int redcount = 0;
int bluecount = 0;
int yellowcount = 0;
bool syokika_flag=false;
bool set_flag = false;
int encoder_count = 1000;
int linecountcheck = false;

int main() {
    timer.reset();
    timer.stop();
    throwing_relay=0;
    lamp=1;
    air=0.0;
    buzzer = 0.0;
    micro_switch.mode(PullUp);
    motor_pwm=0;
    motor_dir1=1;
    motor_dir2=1;
    servo1.init(0);//アーム
    servo2.init(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) {
        if(directmode == STRAIGHT){
            master.read(addr1,buff1,2);
            linecheck(buff1,linedata_1);
        }else if(directmode == RIGHT){
            master.read(addr2,buff2,2);
            linecheck(buff2,linedata_2);
        }else if(directmode == LEFT){
            master.read(addr3,buff3,2);
            linecheck(buff3,linedata_3);
        }
        
        x_dist = Enc_Carry_X.read_rotate();
        y_dist = Enc_Carry_Y.read_rotate();
        
        //PCからの命令
        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 == START){
            state = START;
        }else if(kbtread == STAND_BY_1){
            state = STAND_BY_1;
        }else if(kbtread == YELLOW_RECEIVE){
            state = YELLOW_RECEIVE;
            crosscount = 5;
            air = 1.0;
        }else if(kbtread == STAND_BY_2){
            state = STAND_BY_2;
            crosscount = 0;;
        }
        
//--------------手動--------------------------------------
        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){
//------------------初期位置から待機位置1------------------------------------------
                    case START:
                    if(crosscount == 0){
                        directmode = LEFT;
                        Linecount2();
                    }else if(crosscount == 1){
                        directmode = STRAIGHT;
                    }else if(crosscount == 2){
                        if(directmode == STRAIGHT){
                            directmode = LEFT;
                            prelinedata = 0;
                        }else if(directmode == LEFT){
                            directmode = LEFT;
                            Linecount();
                        }
                    }else if(crosscount == 3){
                        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(redthrowing == false){
                            if(redIn == 1.0){
                                buzzer = 1.0;
                                redcount++;
                            }else {
                                buzzer = 0.0;
                            }
                        }else if(redthrowing == true){
                            if(redIn == 1.0){
                                buzzer = 1.0;
                                bluecount++;
                            }else {
                                buzzer = 0.0;
                            }
                        }
                        if(redcount >= 200 ){
                            buzzer = 0;
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }else if(bluecount >= 200){
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }   
                    }                       
                    if(distance == 0.0 && redcount >= 200){
                        led2 = 1.0;
                        servo2.move(koshi_red_nage_degree);
                        servo2.move(koshi_red_nage_degree);
                        state = RED_RECEIVE;
                        directmode = STOP;
                        redcount = 0;
                        crosscount = 0;
                    }else if(distance == 0.0 && bluecount >= 200){
                        servo2.move(koshi_blue_nage_degree);
                        servo2.move(koshi_blue_nage_degree);
                        state = BLUE_RECEIVE_1;
                        directmode = STOP;
                        bluecount = 0;
                        crosscount = 0;
                    }
                    break;
//------------------待機位置1から投てき位置1----------------------------------------
                    case RED_RECEIVE:
                    led2 = 1.0;
                    if(crosscount == 0){
                        directmode = LEFT;
                    }else if(crosscount == 1){
                        directmode = STOP;
                        prelinedata = 0;
                        touteki(1.0,RED_TOUTEKI,0);
                        //syokikanomai2();
                        if(set_flag == false){
                            directmode = RIGHT;
                            Linecount();
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        syokikanomai2();
                        if(set_flag == true){
                            state = STAND_BY_1;
                            //taiki_flag = false;
                            crosscount = 0;
                            redthrowing = true;
                        }
                    }
                    break;
//------------------待機位置1から投てき位置2----------------------------------------
                    case BLUE_RECEIVE_1:
                    if(crosscount == 0){
                        directmode = RIGHT;
                    }else if(crosscount == 1){
                        directmode = STRAIGHT;
                    }else if(crosscount == 2){
                        directmode = LEFT;
                        Linecount();
                    }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(redIn == 1.0 && micro_switch == 1){
                            buzzer = 1.0;
                            bluecount++;
                        }else if(micro_switch == 0.0){
                            buzzer = 1.0;
                            yellowcount++;
                        }else{
                            buzzer = 0.0;
                        }
                        if(bluecount >= 200){
                            servo_hand.move(hand_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                            servo1.move(hiji_tsukami_degree);
                        }else if(yellowcount >= 500){
                            air = 1.0;
                        }
                    }else if(distance == 0.0 && bluecount >= 200){
                        servo2.move(koshi_blue_nage_degree);
                        servo2.move(koshi_blue_nage_degree);
                        state = BLUE_RECEIVE_2;
                        directmode = STOP;
                        bluecount = 0;
                        crosscount = 0;
                        buzzer = 0.0;
                    }
                    if(yellowcount >= 1000){
                        state = YELLOW_RECEIVE;
                        buzzer = 0.0;
                        directmode = STOP;
                        yellowcount = 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,BLUE_TOUTEKI,1);
                        //syokikanomai2();
                        if(set_flag == false){
                            directmode = RIGHT;
                            Linecount(); 
                        }
                    }else if(crosscount == 2){
                        directmode = STOP;
                        syokikanomai2();
                        if(set_flag == true){
                            state = STAND_BY_2;
                            //taiki_flag = false;
                            crosscount = 0;
                        }
                    }
                    break;
//------------------投てき位置3---------------------------------------------------
                    case YELLOW_RECEIVE:
                    if(crosscount != 5){
                        directmode = LEFT;
                        servo2.move(koshi_uke_degree);
                        power = POWER + 0.1;
                    }else{
                        directmode = STOP;
                        if(yellow_throw_flag == false){
                            yellow_throwing();
                        }else{
                            touteki(1.0,YELLOW_TOUTEKI,1.0);
                            syokikanomai2();   
                        }
                    }
                    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){
                        Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case RIGHT:
                    output = R_pid.compute((double)linedata_2[0]);
                    if(output >= 0){
                        Right(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        Right(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    case LEFT:
                    output = L_pid.compute((double)linedata_3[0]);
                    if(output >= 0){
                        Left(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
                    }else {
                        Left(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
                    }
                    break;
                    default:
                    break;
                }
            }
        }
        pc.printf("xdist = %f \r",x_dist);
        pc.printf("ydist = %f \r",y_dist);
        pc.printf("output  = %lf \n" ,output);
    }
}

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

void yellow_throwing(){
    if(yellow_throw_flag == false){
        timer.start();
    }
    int times = timer.read();
    if(times >= 2.0 && times < 3.0){
        servo_hand.move(hand_uke_degree);
    }else if(times >= 3.0 && times < 4.0){
        servo1.move(hiji_yellow_uke_degree);
    }else if(times >= 4.0 && times < 5.0){
        servo_hand.move(hand_tsukami_degree);
    }else if(times >=5.0 && times <5.5){
        servo2.move(koshi_nage_degree);
    }else if(times >= 5.5){
        yellow_throw_flag = true;
        timer.stop();
        timer.reset();
    }
}

void syokikanomai(void){
    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);
        servo1.move(hiji_nage_degree);//退避
        servo2.move(koshi_uke_degree);
        motor_dir1=0;
        motor_dir2=1;
        motor_pwm=0.12;
        if(Enc_Arm.read_z() == 1){
            syokika_flag=true;
            buzzer = 1.0;
        }
        
        if (syokika_flag==true) {
            if(encoder_count == SYOKIITI){
                motor_pwm=0;
                motor_dir1=1;
                motor_dir2=1;
                set_flag=true;  //投てき後，falseにする.
            }
        }
    }
    servo1.move(hiji_uke_degree);
    servo1.move(hiji_uke_degree);
}

void syokikanomai2(void){
    if(set_flag==false) {
        encoder_count= Enc_Arm.read_rotate();
        servo_hand.move(hand_uke_degree);
        servo1.move(hiji_nage_degree);//退避
        servo1.move(hiji_nage_degree);//退避
        if(yellow_throw_flag == false){
            servo2.move(koshi_uke_degree);
            servo2.move(koshi_uke_degree);
        }
        motor_dir1=0;
        motor_dir2=1;
        motor_pwm=0.12;
        if(Enc_Arm.read_z() == 1) {
            syokika_flag = true;
            buzzer = 1.0;
        }
        if(syokika_flag==true) {
            int set_pos;
            if(yellow_throw_flag == true) set_pos = YELLOW_SYOKIITI;
            else set_pos = SYOKIITI;
            if(encoder_count == set_pos){
                buzzer = 0.0;
                motor_pwm=0;
                motor_dir1=1;
                motor_dir2=1;
                if(yellow_throw_flag == false){
                    servo_hand.move(hand_uke_degree);
                    servo1.move(hiji_uke_degree);
                    servo1.move(hiji_uke_degree);
                    servo1.move(hiji_uke_degree);
                    servo1.move(hiji_uke_degree);
                }else{
                    servo_hand.move(hand_tsukami_degree);
                    servo2.move(koshi_uke_degree);
                    servo2.move(koshi_uke_degree);
                }
                set_flag = true;  //投てき後，falseにする.
                taiki_flag = true;
                yellow_throw_flag = false;
            }
        }
    }
}

void touteki(float armpower ,int pos, bool throwing_mode){
    if(set_flag == true){
        timer.start();
    }
    int times = timer.read();
    if(times >= 1.0 && times < 4.0){
        if(yellow_throw_flag == true){
            if(set_pos_flag_2 == false){
                if(Enc_Arm.read_rotate() < 180) {
                    motor_dir1 = 0;
                    motor_dir2 = 1;
                    motor_pwm = 0.15;
                }else{
                    motor_dir1=1;
                    motor_dir2=1;
                    motor_pwm=0;
                    set_pos_flag_2 = true;
                }   
            }
        }
    }
    if(times >= 4.0 && times < 5.0){
    servo_hand.move(hand_uke_degree);
    }    
    if(times >= 5.0 && times < 6.0){
    servo1.move(hiji_nage_degree);
    servo1.move(hiji_nage_degree);
    }
    if(times >= 6.0 && times < 9.0){
        lamp = 0.0;
        if(set_pos_flag == false){
            if(Enc_Arm.read_rotate() < pos) {
                motor_dir1 = 0;
                motor_dir2 = 1;
                motor_pwm = 0.15;
            }else{
                motor_dir1=1;
                motor_dir2=1;
                motor_pwm=0;
                set_pos_flag = true;
            }   
        }
    }
    
//----投てき-------------------------
    if(throwing_mode == 0.0){
        if(times >= 9.0 && times < 10.0){
            motor_dir1 = 0;
            motor_dir2 = 1;
            motor_pwm = armpower;
        }
    }else if(throwing_mode == 1.0){
        if(times >= 9.0 && times < 9.1){
            throwing_relay = 1.0;
        }else{
            throwing_relay = 0.0;    
        }
    }
    if(times >= 10.0 && times <11.0){
        motor_pwm = 0.0;
        motor_dir1 = 1;
        motor_dir2 = 1;
        buzzer = 0;
        lamp = 1.0;
        syokika_flag = false;
    }else if(times >= 11.0){
        set_flag = false;
        set_pos_flag = false;
        set_pos_flag_2 = false;
        timer.reset();
        timer.stop();
    }
}

void Linecount(void){
    if(directmode == RIGHT){
        if(linecountcheck == false){
            tar_x_dist_2 = x_dist;
            linecountcheck = true;
        }else{
            if((x_dist - tar_x_dist_2) > X_STOP_DIST_3){
                crosscount++;
                linecountcheck = false;
            }
        }
    }else if(directmode == LEFT){
        if(linecountcheck == false){
            tar_x_dist_2 = x_dist;
            linecountcheck = true;
        }else{
            if((tar_x_dist_2 - x_dist) >= X_STOP_DIST_2){
                crosscount++;
                linecountcheck = false;
            }
        }
    }
}

void Linecount2(void){
    if(directmode == LEFT){
        if(linecountcheck == false){
            tar_x_dist_3 = x_dist;
            linecountcheck = true;
        }else{
            if((tar_x_dist_3 - x_dist) >= X_STOP_DIST_4){
                crosscount++;
                linecountcheck = false;
            }
        }
    }
}
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];
}