Revision 0:1c12315a0341, committed 2022-04-01

Comitter:

darkumatar

Date:

Fri Apr 01 00:22:03 2022 +0000

Commit message:

right

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arrc_mbed.lib	Fri Apr 01 00:22:03 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/go-roboB/code/arrc_mbed/#7ea663f79c81

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 01 00:22:03 2022 +0000
@@ -0,0 +1,439 @@
+#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 -23800.0
+#define goal_x2 8000000
+#define goal_y2 -8000000
+#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;
+ 
+
+ 
+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);
+}
+
+ 
+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);
+    
+    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.015>ca[i]&&ca[i]>-0.015){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);
+        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();
+    }
+}
\ No newline at end of file