File content as of revision 0:3dc012104243:

#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);
//RawSerial bt(PF_7,PF_6);
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;
///////////定数たち//////////////
const float Min_ang = 0.5f;
const float Max_pwm = 0.2f;
const float Max_pwm_roll = 0.3f;
const float Min_pwm = 0.02f;
const float Gein = 0.05f;//0.04f
const int R = 103;//これ直径やんけ
const int Resolution = 1024;
const float RoriGein = 0.001f;
///////////////////////
int point_count = 0;
int array_count = 0;
int C_vari = 0;
long last_x;
/*
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[] = {
    900,1700,3500
    };
int nomal_towel[]{
    C+600,C+1100,C+1600
    };
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 tempG = RoriGein*0.5f;
void setMove(int p0,int p1);
float pwm;
void rolling();
void void_read_gyro();
bool activeMove = false;
RORI RX(PF_9,PF_8);
//RORI RY(PA_4,PB_0);
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 move_lock1 = false;
bool move_lock2 = false;
bool go_up = false;
int main() {
    ins.mode(PullDown);
    
    setup_peri();
    int whichnum = read_peri();
    int points[whichnum][3];
    changeColor(&points[0][0]);

    if(read_keter())keter=true;
    //bt.putc('a');
    setMove(points[point_count][0],points[point_count][1]);
    serial.printf("%s\n","mpu_setup");
    //bt.putc('b');
    if(!keter){
        mpu_setup();
        serial.printf("%s\n","get_off");
        //bt.putc('c');
        get_offset();
        serial.printf("%s\n","move2");
        //bt.putc('d');
    }
    Move2(0,0);
    change_rm.start();
    move_tm.start();
    //bt.putc('e');
    /*

    */
    while(1) {
            serial.printf("%f",rorix);
            serial.printf("%s",":");
            if(!keter){
                readGyz(&gz);
                offset_adjust(&all,&gz);
                //bt.putc('f');
                //bt.puts(puc(all));
                //bt.putc('F');
                to_signed(&all);
            }
    /////////////傾き修正////////////////////////////

        if(!keter){
            pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
            if(!activeMove){
                if(pwm <= Min_pwm){
                    //bt.putc('k');
                    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){
            RX.read(&rorix_l);
            RY.read(&roriy_l);
            rorix = rorix_l;
            rorix = rorix/Resolution;//回転数
            rorix *= Cir;
            roriy = roriy_l;
            roriy = roriy/Resolution;
            roriy *= Cir;
        }
        //serial.printf("%f\n",roriy);
        
        ///////////////////////
        ///////符号反転処理////////
            x_dis = rorix-off_rorix;
            y_dis = roriy-off_roriy;
        //////////////////////
        /*
            bt.putc('g');
            bt.puts(puc(x_dis));
            bt.putc('G');
            bt.putc('h');
            bt.puts(puc(y_dis));
            bt.putc('H');
            */
        /////移動処理/////////
        if(!flag_finish_move){
            float pwm = 0;
            //##########################problem############################x,y are cross このままだと半分になる
            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;
                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);
                        //bt.putc('i');
                        //////////////////////////////
                        pwm = 1;
                        float set0_dis;
                        float pwm_while;
                        bool whileactive = true;
                        move_tm.reset();
                        while(move_tm.read_ms() < 600){
                            if(whileactive){
                                RX.read(&rorix_l);
                                RY.read(&roriy_l);
                                rorix = rorix_l;
                                rorix = rorix/Resolution;//回転数
                                rorix *= Cir;
                                roriy = roriy_l;
                                roriy = roriy/Resolution;
                                roriy *= Cir;
                                if(last_move_vertical){
                                    set0_dis = roriy-off_roriy;
                                    pwm = set0_dis*RoriGein;
                                    if(abs(pwm) >= Min_pwm)move_tm.reset();
                                    if(pwm < 0){
                                        Move2(RIGHT,-pwm);
                                    }else{
                                        Move2(LEFT,pwm);
                                        }
                                }else if(last_move_side){
                                    serial.printf("%s\n","side");
                                    set0_dis = rorix-off_rorix;
                                    serial.printf("%f\n",set0_dis);
                                    pwm = set0_dis*RoriGein;
                                    if(abs(pwm) >= Min_pwm)move_tm.reset();
                                    if(pwm < 0){
                                        Move2(UP,-pwm);
                                    }else{
                                        Move2(BACK,pwm);
                                        }
                                }
                            }else{
                                //serial.printf("%s\n","whileroll");
                                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++;
                        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{
                    //serial.printf("%f",pwm);
                    if(pwm > Max_pwm)pwm = Max_pwm;
                    if(activeMove){
                        
                        if(points[point_count][2] == 1){//しーつ
                            if(points[point_count][1]-distance > sheets[array_count] && array_count != 3){
                                array_count++;
                                Move2(10,0);
                                
                                }
                        }else if(points[point_count][2] == -1){//ばしゅたおる
                                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;
                                    }
                                }
                        if(!point_move){
                            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);
                            }
                        }else{
                            //Move2(10,0);
                            //wait(1);
                            serial.printf("%s\n","maemae");
                            if(C_vari == 0){
                                off_rorik = rorix;
                                C_vari = 1;
                                }
                            if(go_up){
                                if(abs(rorix - off_rorik) > 150){
                                    go_up = false;
                                    Move2(0,0);
                                    //if(array_count != 0)point_move = false;
                                }else{ 
                                    serial.printf("%f\n",rorik-off_rorik);//-4560
                                    Move2(0,0.1f);
                                }
                            }else{
                                serial.printf("%s\n","owari");
                                //Move2(10,0);
                                //wait(1);
                                
                                outs = 1;
                                while(!ins.read())serial.printf("%d\n",ins.read());
                                outs = 0;
                                point_move = false;
                            }
      
                        }
                        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\n","sheets");
                        else serial.printf("%s\n","towel");                      
                    }
                    move_tm.reset();
                }

       }else{
        serial.printf("%s","finish");
        }
        
        if(activeMove){
            if(change_rm.read_ms() > 1500){
                activeMove = false;
                change_rm.reset();
                //bt.putc('j');
                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 rolling(){
    while(pwm > Min_pwm){
        void_read_gyro();
        pwm = angle_adjust(Gein,all,Min_ang,Min_pwm,Max_pwm_roll);
        if(all < Min_ang)Move2(8,pwm);
        else if(all > -Min_ang)Move2(9,pwm);
        }
    }
    
void void_read_gyro(){
        readGyz(&gz);
        offset_adjust(&all,&gz);
        to_signed(&all);
    }