File content as of revision 4:9ba47e5db1e2:

#include "mbed.h"
#include "functions.h"
#include "foot.h"
#include "rori.h"
#include "peripheral.h"

#define UP 0
#define BACK 1
#define RIGHT 2
#define LEFT 3
#define RUP 4
#define LUP 5
#define RBACK 6
#define LBACK 7
#define RROLL 8
#define LROLL 9

//767ってちょっと765っぽいよね
//lets go fiver time!
int sewomukeru[10] = {
    BACK,UP,LEFT,RIGHT,LBACK,RBACK,LUP,RUP,LROLL,RROLL//reverce array
    };
//もはやJava
Serial serial(USBTX,USBRX);

DigitalOut outs(PF_6);
DigitalIn ins(PF_7);
DigitalOut outt(D0);
DigitalIn intt(D1);


//https://github.com/ARMmbed/mbed-os/blob/master/targets/TARGET_STM/TARGET_STM32F7/TARGET_STM32F767xI/TARGET_NUCLEO_F767ZI/PeripheralPins.c


double all = 0;
bool keter = false;
double gz;
Timer change_rm;
Timer move_tm;
Timer go_up_tm;
///////////定数たち//////////////
const float Min_ang = 0.1f;
const float Max_pwm = 0.15f;
const float Max_pwm_roll = 0.3f;
const float Min_pwm = 0.02f;
const float Gein = 0.04f;//0.05f
const int R = 103;//これ直径やんけ
const int Resolution = 1024;
const float RoriGein = 0.001f;
const int mae_dis = 250;
///////////////////////
int point_count = 0;
int array_count = 0;
int C_vari = 0;
/////////////////Move/////////////////////////
/*
int points[][3] = {
    {RIGHT,3700,-1},
    {LEFT,3700,0}
    };
*/
int Robox = getRobox();
//int C = (1750-Robox);
int C = 1550;

/*
int sheets[] = {
    C+100,C+1300,C+2500
    };
*/
int sheets[] = {
    1975,2775,3575//下から:1750,2775,3575
    };

//当たるのは1200mm
int nomal_towel[]{
    1975,2775,3575//2130,2930,3730
    };
int final_towel[]{
    C+410,C+1070,C+1730,C+2390
    };
float Cir = R * 3.14159f;
long rorix_l,roriy_l;
float pointx,pointy;
float rorix = 0;
float roriy = 0;
float off_rorix,off_roriy;
int sign;
bool x_flag,y_flag;
bool last_move_vertical,last_move_side;
float x_dis,y_dis;
bool flag_finish_move = false;

float mae_pwm = 1;

void setMove(int p0,int p1);
void get_rorivol();

float pwm;
bool activeMove = false;
RORI RX(PF_9,PF_8);
RORI RY(PA_4_ALT0,PB_0_ALT0);

int abspointx = 0;
int abspointy = 0; 
bool final = false;
long rorik_l = 0;
float rorik = 0;
float rorik_temp = 0;
float off_rorik = 0;

bool point_move = false;

bool go_up = false;
float dis_go_up = 0;
//////////////////////////////////////////////
bool moved_to_wall = false;
bool start_wall_time = false;
int from_mid = 0;
int wall_C = 0;
DigitalIn lim_right(PG_10);
DigitalIn lim_left(PG_15);
int l_r = 0;
int l_l = 0;

bool hensu_A = false;
/////////////////////////////////////////////
DigitalOut outColor(PG_4);
DigitalOut outSorT(PG_7);
////////////////////////////////////////////
DigitalOut gav_out(PD_10);
DigitalIn gav_in(PG_14);
///////////////////////////////////////////
float only_this_time = 0;
int main() {
    ins.mode(PullDown);
    lim_right.mode(PullDown);
    lim_left.mode(PullDown);
    setup_peri();
    int whichnum = read_peri();
    int points[whichnum][3];
    changeColor(&points[0][0]);
    
    outColor = getColor();
    outSorT = getPeriwhich();
    
    gav_in.mode(PullDown);
    //if(read_keter())keter=true;
    setMove(points[point_count][0],points[point_count][1]);
    serial.printf("%s\n","mpu_setup");
    if(!keter){
        mpu_setup();
        serial.printf("%s\n","get_off");
        get_offset();
        serial.printf("%d\n",rez()); 
        serial.printf("%s\n","move2");
    }
    Move2(0,0);
    change_rm.start();
    move_tm.start();
    go_up_tm.start();

    while(1) {
            serial.printf("%f",all);
            serial.printf("%s",":");
            if(!keter){
                readGyz(&gz);
                offset_adjust(&all,&gz);
                to_signed(&all);
            }
    /////////////傾き修正////////////////////////////

        if(!keter){
            pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
            if(!activeMove){
                if(pwm <= Min_pwm){
                    activeMove = true;
                    }
                
                if(all < Min_ang)Move2(8,pwm);
                else if(all > -Min_ang)Move2(9,pwm);
                //else Move2(9,0);
                /////////////////////////////////////
                
                serial.printf("%f",pwm);
                serial.printf("%s",":");
                serial.printf("%f",all);
                serial.printf("%s\n",":");
                
            }
        }else{
            if(!activeMove)activeMove = true; 
            }
        /////////ロリコン値取得///////
        if(activeMove){
            get_rorivol();
        }
        
        ///////符号反転処理////////
            x_dis = rorix-off_rorix;
            y_dis = roriy-off_roriy;

        /////移動処理/////////
        if(!flag_finish_move){
            float pwm = 0;
            float distance;
            if(points[point_count][0] == UP or points[point_count][0] == BACK){
                distance = pointy-x_dis;
                last_move_vertical = true;
                last_move_side = false;
            }else if(points[point_count][0] == LEFT or points[point_count][0] == RIGHT){
                distance = pointx-y_dis + wall_C;
                last_move_vertical = false;
                last_move_side = true;
            }
            //else if(points[point_count][0] >= 4 && points[point_count][0] <= 7)ななっめ移動
            if(x_flag or y_flag)distance *= -1;
            pwm = distance*RoriGein;
            pwm = (pwm < 0)?-pwm:pwm;//abs
            if(pwm < Min_pwm){
                    //次の処理へ
                    serial.printf("%d\n",move_tm.read_ms());
                    if(move_tm.read_ms() >= 600){
                        serial.printf("%s","waiting");
                        serial.printf("%d\n",point_count);
                        //////////////////////////////
                        pwm = 1;
                        float set0_dis;
                        float pwm_while;
                        bool whileactive = true;
                        only_this_time = rorix;
                        move_tm.reset();
                        while(move_tm.read_ms() < 600){
                            if(whileactive){
                                get_rorivol();
                                if(last_move_vertical){
                                    set0_dis = roriy-off_roriy;
                                    pwm = set0_dis*RoriGein;
                                    if(abs(pwm) >= Min_pwm)move_tm.reset();
                                    if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから
                                    if(pwm < 0){
                                        Move2(RIGHT,-pwm);
                                    }else{
                                        Move2(LEFT,pwm);
                                        }
                                }else if(last_move_side){
                                    if(points[point_count][2] == 0){
                                        serial.printf("%s\n","side");
                                        set0_dis = rorix-off_rorix;
                                        serial.printf("%f\n",set0_dis);
                                        pwm = set0_dis*RoriGein;
                                    }else{
                                        serial.printf("%s\n","only_this_side");
                                        set0_dis = abs(rorix - only_this_time);
                                        serial.printf("%f\n",mae_dis-set0_dis);
                                        pwm = (mae_dis-set0_dis)*RoriGein;
                                        }
                                    if(abs(pwm) >= Min_pwm)move_tm.reset();
                                    if(abs(pwm) > Max_pwm*0.9)pwm = Max_pwm;//ぐわんぐわんするから
                                    if(pwm < 0){
                                        Move2(UP,-pwm);
                                    }else{
                                        Move2(BACK,pwm);
                                        }
                                }
                            }else{
                                pwm_while = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
                                    if(pwm_while <= Min_pwm){
                                        whileactive = true;
                                    }
                                    if(all < Min_ang)Move2(8,pwm_while);
                                    else if(all > -Min_ang)Move2(9,pwm_while);
                                    else Move2(9,0);
                                }
                            if(whileactive){
                                if(change_rm.read_ms() > 500){
                                    whileactive = false;
                                    change_rm.reset();
                                    serial.printf("%s\n","change_rm_while");
                                    }
                                }
                        }
                        ////////////////////////
                        point_count++;
                        //////////////////////////////////////////////////////////////////
                        start_wall_time = false;
                        moved_to_wall = false;
                        /////////////////////////////////////////////////////////////////
                        Move2(1,0);
                        if(point_count >= sizeof(points)/sizeof(*points)){
                            flag_finish_move = true;
                        }else{
                            setMove(points[point_count][0],points[point_count][1]);
                        }
                   }
                }else{
                    if(pwm > Max_pwm)pwm = Max_pwm;
                    if(activeMove){
                        if(points[point_count][2] != 0){
                            bool issheets = (points[point_count][2] == 1)?1:0;
                            if(issheets){
                                /////////////////////////////////////////////////////////////////////////////////////////
                                if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
                                    moved_to_wall = true;
                                    }
                                /////////////////////////////////////////////////////////////////////////////////////////
                                if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
                                    if(array_count == 0)go_up = true;
                                    array_count++;
                                    point_move = true;
                                    activeMove = false;
                                    Move2(0,0);
                                    }
                            }else{
                                /////////////////////////////////////////////////////////////////////////////////////////
                                if(!moved_to_wall && points[point_count][1]-distance > 1000 && !start_wall_time){
                                    moved_to_wall = true;
                                    }
                                /////////////////////////////////////////////////////////////////////////////////////////
                                if(points[point_count][1]-distance > nomal_towel[array_count] && array_count != 3){
                                    if(array_count == 0)go_up = true;
                                    array_count++;
                                    point_move = true;
                                    activeMove = false;
                                    Move2(0,0);
                                    }
                                }
                            if(moved_to_wall){
                                l_r = lim_right.read();
                                l_l = lim_left.read();
                                if(!start_wall_time){
                                    if(l_r && l_l){
                                        start_wall_time = true;
                                        Move2(10,0);
                                    }else if(!l_r && l_l){
                                        serial.printf("%s\n","LSRIDE");
                                        Move2(RIGHT,0.1f);
                                    }else if(l_r && !l_l){
                                        Move2(LEFT,0.1f);
                                        serial.printf("%s\n","RSRIDE");
                                    }else{
                                        Move2(UP,0.1f);
                                        serial.printf("%s\n","UP");
                                    }
                                }else{
                                    if(!l_r && !l_l){
                                        moved_to_wall = false;
                                        hensu_A = true;
                                        wait(0.5f);
                                    }else if(l_r || l_l){
                                        serial.printf("%d",l_l);
                                        serial.printf("%s",":");
                                        serial.printf("%d\n",l_r);
                                        Move2(points[point_count][0],0.1f);
                                        }
                                }
                            }
                            if(hensu_A){
                                gav_out = 1;
                                while(!gav_in.read()){
                                    serial.printf("%d\n",gav_in.read());
                                    }
                                //wall_C = -1400-from_mid+points[point_count][1]-pointx+y_dis; 
                                points[point_count][1] = 1400 + distance;
                                gav_out = 0;
                                hensu_A = false;
                                }
                        if(point_move){
                            serial.printf("%s","maemae");
                            if(C_vari == 0){
                                off_rorik = rorix;
                                C_vari = 1;
                                }
                            if(go_up){
                                dis_go_up = abs(rorix-off_rorik);//進んだ距離
                                serial.printf("%f",mae_dis-dis_go_up);//残り
                                if(mae_pwm > Min_pwm){
                                    if(dis_go_up < mae_dis){
                                        mae_pwm = (mae_dis-dis_go_up)*RoriGein;
                                        if(mae_pwm > Max_pwm)mae_pwm = 0.1f;
                                        Move2(0,mae_pwm);//+-20mm
                                        go_up_tm.reset();
                                    }else{
                                        mae_pwm = (dis_go_up-mae_dis)*RoriGein;
                                        if(mae_pwm > Max_pwm)mae_pwm = 0.1f;
                                        Move2(1,mae_pwm);
                                        go_up_tm.reset();
                                    }
                                }
                            
                                if(go_up_tm.read_ms() > 300){
                                    go_up = false;
                                    Move2(10,0);
                                }
                            }else{
                                serial.printf("%s\n","owari");
                                outs = 1;
                                while(!ins.read())serial.printf("%d\n",ins.read());
                                outs = 0;
                                point_move = false;
                            }
      
                            }
                        }
                                
                        if(!point_move && !moved_to_wall && !go_up){
                            if(distance > 0)Move2(points[point_count][0],pwm);
                            else{
                             pwm = (-distance*RoriGein > Max_pwm)?Max_pwm:-distance*RoriGein;
                             Move2(sewomukeru[points[point_count][0]],pwm);
                            }
                        }
                        serial.printf("%f",pwm);
                        serial.printf("%s",":");
                        serial.printf("%f",distance);
                        serial.printf("%s",":");
                        serial.printf("%d",point_count);
                        serial.printf("%s",":");
                        serial.printf("%d",sizeof(points)/sizeof(*points));
                        serial.printf("%s",":");
                        serial.printf("%d",y_flag);
                        serial.printf("%s",":");
                        if(points[1][0] == RIGHT)serial.printf("%s","orange");
                        if(points[1][0] == LEFT)serial.printf("%s","blue");
                        serial.printf("%s",":");
                        if(points[0][1] > 5100)serial.printf("%s","sheets");
                        else serial.printf("%s","towel");
                        serial.printf("%s",":");
                        serial.printf("%f\n",points[point_count][1]-distance);                      
                    }
                    move_tm.reset();
                }

       }else{
        serial.printf("%s","finish");
        }
        
        if(activeMove){
            if(change_rm.read_ms() > 1500){
                activeMove = false;
                change_rm.reset();
                serial.printf("%s\n","change_rm");
            }
        }
        

   }
   
}


void setMove(int p0,int p1){
    trans(p0,p1,&pointx,&pointy,&sign);//一回だけ
    array_count = 0;
    off_rorix = rorix;
    off_roriy = roriy;
    abspointx = (pointx < 0)?-pointx:pointx;
    abspointy = (pointy < 0)?-pointy:pointy;
    //一回
    if((sign & 0b01) == 1)x_flag = true;//x < 0
    else x_flag = false;
    if((sign >> 1) == 1)y_flag = true;//y < 0
    else y_flag = false;

    }

void get_rorivol(){
    RX.read(&rorix_l);
    RY.read(&roriy_l);
    rorix = rorix_l;
    rorix = rorix/Resolution;//回転数
    rorix *= Cir;
    roriy = roriy_l;
    roriy = roriy/Resolution;
    roriy *= Cir;
}