File content as of revision 0:a3056628cb39:

#include "mbed.h"
#include"scrp_slave.hpp"
#include "rotary_inc.hpp"
#include"cmath"
#include"gy521.hpp"
#include "neopixel.h"
 
 
#define goal_x -17700.0
#define goal_y 28800.0//23800.0


#define goal_x2 -8000000
#define goal_y2 10000000
#define period_r 8000.0
NeoPixelOut npx(PB_4,7);
I2C i2d(PB_3,PB_10);
GY521 gyro(i2d);
ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定
RotaryInc v[4]{RotaryInc(PC_5,PA_12,1,256,4),RotaryInc(PA_9,PA_8,1,256,4),RotaryInc(PC_3,PC_2,1,256,4),RotaryInc(PC_11,PC_10,1,256,4)};//ロリコンピン
RotaryInc rote[4]{RotaryInc(PA_15,PA_14,1,256,4),RotaryInc(PA_7,PA_6,1,256,4),RotaryInc(PC_1,PC_0,1,256,4),RotaryInc(PA_13,PC_4,1,256,4)};//読み取り用ピン
int target[4];//target変数　この変数の値を目指すように動く
int angler=90;
int speedr=1024;
bool auto_swich = false;
bool limit_switch = true;
int step=0;
int auto_mode=0;

int direct_xx=0;
int direct_yy=0;
int direct_turn=0;

DigitalOut Led1(PB_2);
DigitalOut Led2(PC_6);
DigitalOut Led3(PB_15);
DigitalOut Led4(PA_10);
int check_tepu=0;
 
bool limi=true;
bool liset_abc=false;

 
DigitalIn limitB(PB_12,PullUp);

 
 
bool add(int id,int ppp){//速度を変えるやつ
    Led4.write(0);
    if(auto_swich==false&&limi==true){
        target[id]=30*ppp;
    }
    return true;
}
 
bool anglez(int id,int ppp){//おまけで作ったやつ　アングルを決めてその方向へ走る
    angler=ppp;
    for(int i=0;i<4;i++){
    //target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
    
    }    
    return true;
}
 
bool speedz(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
     speedr=ppp;
     for(int i=0;i<4;i++){
    target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
    
    }
    return true;
}

bool auto_on(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
    if(ppp==1){
    auto_swich=true;
    step=0;
    }
    return true;
}
 
bool auto_off(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
    if(ppp==1){
    auto_swich=false;
    step=0;
    }
    return true;
}
 
bool turn(int id,int ppp){//回転
    for(int i=0;i<4;i++){
    target[i]=ppp;   
    } 
    return true;
}
 
bool pro(int id,int ppp){  //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
    int angle=ppp%1000;
    int speed=ppp/1000;
    for(int i=0;i<4;i++){
    target[i]=(sin(M_PI/180.0*(angle+90*i))+cos(M_PI/180.0*(angle+90*i)))*speed*100.0;
    
    }
    return true;
}
bool pro2(){  //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
    double anglert=-135.000000000;
    Led4.write(1);
    for(int i=0;i<4;i++){
    target[i]=(direct_yy*sin(M_PI/180.0*(90*i - gyro.yaw - anglert))-direct_xx*cos(M_PI/180.0*(90*i - gyro.yaw - anglert))+direct_turn)*30.0;
    
    }
    return true;
}
bool pro3(double goal_angle_ppp,int goal_x_speed,int goal_y_speed){
    double anglert=-135.000000000;
    for(int i=0;i<4;i++){
    target[i]=(goal_y_speed*sin(M_PI/180.0*(90*i - gyro.yaw - anglert))-goal_x_speed*cos(M_PI/180.0*(90*i - gyro.yaw - anglert))+goal_angle_ppp)*30.0;
    
    }
    return true;
}
int test_limit(int test_limilimi){
    test_limilimi=test_limilimi/100;
    if(test_limilimi>64){
        test_limilimi=64;    
    }
    if(test_limilimi<-64){
        test_limilimi=-64;    
    }
    return test_limilimi;
}

double test_ang(double test_limilimi){
    test_limilimi=test_limilimi*2;
    if(test_limilimi>40){
        test_limilimi=40;    
    }
    if(test_limilimi<-40){
        test_limilimi=-40;    
    }
    return test_limilimi;
}

bool limit2(int id,int ppp){
    if(ppp==1){
        limit_switch=false;
    }
    return true;
}

bool direct_x(int id,int ppp){
    direct_xx=ppp;
    pro2();
    return true;
}
bool direct_y(int id,int ppp){
    direct_yy=ppp;
    pro2();
    return true;
}

bool direct_tu(int id,int ppp){
    direct_turn=ppp;
    pro2();
    return true;
}

bool  limit(int a,int &b){
    return limit2(0,a);
}
bool  abc1(int a,int &b){
    check_tepu+=1;
    return add(0,a);
}
bool  abc2(int a,int &b){
    check_tepu+=1;
    return add(1,a);
}
bool  abc3(int a,int &b){
    check_tepu+=1;
    return add(2,a);
}
bool  abc4(int a,int &b){
    check_tepu+=1;
    return add(3,a);
}

bool  abc5(int a,int &b){
    return turn(0,a);
}
/*
bool  abc6(int a,int &b){
    return pro(0,a);
}
bool  abc7(int a,int &b){
    return anglez(0,a);
}
bool  abc8(int a,int &b){
    return speedz(0,a);
}
*/
bool  abc9(int a,int &b){
    return auto_on(0,a);
}
bool  abc10(int a,int &b){
    return auto_off(0,a);
}
bool  abc11(int a,int &b){
    check_tepu+=1;
    return direct_x(0,a);
}
bool  abc12(int a,int &b){
    check_tepu+=1;
    return direct_y(0,a);
}
bool  abc13(int a,int &b){
    check_tepu+=1;
    return direct_tu(0,a);
}
bool  abc14(int a,int &b){
    if(a==1){
        liset_abc=true;
    }else{
        liset_abc=false;
    }
    return true;
}

 
int main() {
   Led1.write(1);
   Led2.write(0);
    slave.addCMD(1,limit);
    slave.addCMD(2,abc1);
    slave.addCMD(3,abc2);
    slave.addCMD(4,abc3);
    slave.addCMD(5,abc4);
    slave.addCMD(12,abc9);
    slave.addCMD(13,abc10);
    slave.addCMD(31,abc11);
    slave.addCMD(32,abc12);
    slave.addCMD(33,abc13);
    slave.addCMD(34,abc14);
    int i;    
    double x_period=0;
    double y_period=0;
    int after[4],before[4],before_parus[4],speed_pwm[4],after_pwm[4],before_pwm[4];
    double ca[4],P[4],I[4],D[4],integral[4];
    PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_6 PC_8
    PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_7 PC_9
    for(i=0;i<4;i++){
    motor_p[i].period_us(2048);
    motor_m[i].period_us(2048);
    target[i]=0;
    after[i]=0;
    before[i]=0;
    ca[i]=0;
    P[i]=0;
    I[i]=0;
    D[i]=0;
    before_parus[i]=0;
    integral[i]=0;
    speed_pwm[i]=0;
    after_pwm[i]=0;
    before_pwm[i]=0;
   }
    Timer name;
    Timer limilimi;
    limilimi.start();
    name.start();
    gyro.start();
    double angle=0;
    double goal_angle1=0;
    double test_speed_auto=0;
    Led1.write(1);
    bool limilimi_swich=false;
    while(limit_switch==true) {
    angle=gyro.yaw-90.00000;
    if(auto_swich==true){
            
            
            
            /*
            
            goal_angle1=-1.0*atan((goal_x-x_period)/(goal_y-y_period))/M_PI*180.0;
            if(goal_angle1>0){
                goal_angle1-=180.0;
            }else{
                goal_angle1+=180.0;
            }    
        if(step==0&&((goal_angle1-gyro.yaw)>1||(goal_angle1-gyro.yaw)<-1)){
            test_speed_auto=(goal_angle1-gyro.yaw)*-20.0;
            if(test_speed_auto>2000){
                test_speed_auto=2000;    
            }else if(test_speed_auto<-2000){
                test_speed_auto=-2000;    
            }
            turn(0,(test_speed_auto)*1);
        }else if(step>=0&&step<3){
            step+=1;    
        }else if(step==3&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)||  (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){
            test_speed_auto=-1.0*(sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r))/10.0;
            if(test_speed_auto>1000){
                test_speed_auto=1000;    
            }else if(test_speed_auto<-1000){
                test_speed_auto=-1000;    
            }
            speedz(0,(test_speed_auto)*1);
            
        }else {
            auto_swich=false;
        }
    
    */
    goal_angle1=-1.0*atan((goal_x2-x_period)/(goal_y2-y_period))/M_PI*180.0;
    
            if(goal_angle1>0){
                goal_angle1-=180.0;
            }else{
                goal_angle1+=180.0;
            }
            
            
                
        if(step==0&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))>1000)||  (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))<-1000))){
            pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period));
        }else if(step>=0&&step<60){
            for(i=0;i<4;i++){
            target[i]=0;
            motor_p[i]=0;
            motor_m[i]=0;
        }
        pro3(-test_ang(goal_angle1-gyro.yaw),test_limit(goal_x-x_period),test_limit(goal_y-y_period));
        }else if(step==60&&((sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)>1000)||  (sqrt((goal_y-y_period)*(goal_y-y_period)+(goal_x-x_period)*(goal_x-x_period))-sqrt(period_r*period_r)<-1000))){
            
            
        }else {
            auto_swich=false;
        }
    
    
    }
    
    /*
        if(limitB){
            limi=true;
            limilimi_swich=false;
        }else{
            int arc=0;
            for(i=0;i<4;i++){
                arc+=target[i]*sin((45.0+90.0*i)/90.0*M_PI);
            }
            if(arc>0){
            }
            if(limilimi_swich==false){
                limilimi_swich=true;
                limilimi.reset();
            }
        }
        if(limilimi.read_ms()<500&&!limitB){
            limi=false;
            for(i=0;i<4;i++){
                target[i] =0;   
            }
        }else {
            limi=true;
        }
        
        */
        for(i=0;i<4;i++){
            after_pwm[i]=rote[i].get();
            after[i]=v[i].get();//ロリコンから値読み取り
            integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
            P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例
            D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分
            I[i]=0.0001*integral[i];//積分
            ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる
            if(ca[i]>0.4){ca[i]=0.4;}
            if(ca[i]<-0.4){ca[i]=-0.4;}
            if(0.03>ca[i]&&ca[i]>-0.03){ca[i]=0;}
            motor_p[i]=ca[i];
            motor_m[i]=ca[i]*-1.0;
            speed_pwm[i]=after_pwm[i]-before_pwm[i];
            before_parus[i]=target[i]/10.0-(after[i]-before[i]);
            before[i]=after[i];
            before_pwm[i]=after_pwm[i];
        }
        /*
        for(i=0;i<4;i++){
            x_period+=speed_pwm[i]*cos(M_PI/180.0*(angle + 90.0*i));
            y_period+=speed_pwm[i]*sin(M_PI/180.0*(angle + 90.0*i));
        }
        */
        x_period+=(((speed_pwm[0]-speed_pwm[2])*cos(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*cos(M_PI/180.0*(angle+90.0))*(-1.0))/2);
        y_period+=(((speed_pwm[0]-speed_pwm[2])*sin(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*sin(M_PI/180.0*(angle+90.0))*(-1.0))/2);
        
        if(liset_abc==true){
            x_period=0;
            y_period=0;
            gyro.reset(0);
            
        }
        
        slave.port2.printf("%d\n",1);
        while(name.read_ms()<30){}
        slave.send1(255,16,x_period*1);
        while(name.read_ms()<60){}
        slave.send1(255,17,y_period*1);
        while(name.read_ms()<100){}
        slave.send1(255,18,gyro.yaw*1);
        for(i = 0; i < 4; i++){
            npx.global_scale = 0.05f;
            npx.normalize = false;
            if(check_tepu>0){
                npx.setPixelColor(i,0xFF0000);
            }else {
                npx.setPixelColor(i,0x0000FF);
            }
        }
        for(i = 4; i < 7; i++){
        npx.global_scale = 0.05f;
        npx.normalize = false;
            if(auto_swich){
                npx.setPixelColor(i,0xFFFFFF);
            }else {
                npx.setPixelColor(i,0x00FF00);
            }
        }
         npx.show();
        if(check_tepu==0){
            auto_mode+=1;
            Led2.write(1);
        }else {
            auto_mode=0;  
            Led2.write(0);  
        }
        if(auto_mode>1){
            limit_switch=false;
        }
        check_tepu=0;   
        gyro.update();
        name.reset();
    }
    while(1){
        Led3.write(1);
        for(i=0;i<4;i++){
            target[i]=0;
            motor_p[i]=0;
            motor_m[i]=0;
            npx.global_scale = 0.05f;
            npx.normalize = false;
            npx.setPixelColor(i,0x00FFFF);
        }
        npx.show();
    }
}