a
Dependencies: mbed SBDBT arrc_mbed
Diff: main.cpp
- Revision:
- 0:e22d70b99d51
- Child:
- 1:0867d6d1421a
diff -r 000000000000 -r e22d70b99d51 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Mar 05 05:00:55 2022 +0000 @@ -0,0 +1,229 @@ +#include "mbed.h" +#include"scrp_slave.hpp" +#include "rotary_inc.hpp" +#include"cmath" +#include"gy521.hpp" + +#include "air.hpp" +#include "Servo.h" + + +#define goal_x 14000.0 +#define goal_y 15000.0 +#define period_r 3000.0 + +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)};//ロリコンピン +//PC_11 PC_10 PC_3 PC_2 +int target[4];//target変数 この変数の値を目指すように動く +int angler=90; +int speedr=1024; +bool auto_swich = false; +int step=0; + + + + + + +bool add(int id,int ppp){//速度を変えるやつ + if(auto_swich==false){ + target[id]=20*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 abc1(int a,int &b){ + return add(0,a); +} +bool abc2(int a,int &b){ + return add(1,a); +} +bool abc3(int a,int &b){ + return add(2,a); +} +bool abc4(int a,int &b){ + 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); +} +int main() { + slave.port2.printf("%d\n",1); + slave.addCMD(2,abc1); + slave.addCMD(3,abc2); + slave.addCMD(4,abc3); + slave.addCMD(5,abc4); + // slave.addCMD(6,abc5); + // slave.addCMD(7,abc6); + // slave.addCMD(8,abc7); + // slave.addCMD(9,abc8); + slave.addCMD(12,abc9); + slave.addCMD(13,abc10); + + + int i; + double x_period=0; + double y_period=0; + int after[4],before[4],before_parus[4],speed_pwm[4]; + double ca[4],P[4],I[4],D[4],integral[4]; + PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_6 PC_8 + PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 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; + } + Timer name; + name.start(); + gyro.start(); + double angle=0; + double goal_angle1=0; + double test_speed_auto=0; + int test_x=0; + int test_y=0; + int test_a=0; + while(1) { + angle=45.0+gyro.yaw; + + 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<20){ + step+=1; + }else if(step==20&&((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; + } + } + + for(i=0;i<4;i++){ + 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;} + motor_p[i]=ca[i]; + motor_m[i]=ca[i]*-1.0; + speed_pwm[i]=after[i]-before[i]; + before_parus[i]=target[i]/10.0-(after[i]-before[i]); + before[i]=after[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); + test_x=x_period; + test_y=y_period; + test_a=angle; + slave.port2.printf("%d\n",1); + slave.send1(255,30,test_x); + while(name.read_ms()<30){} + slave.send1(255,31,test_y); + while(name.read_ms()<60){} + slave.send1(255,32,test_a); + while(name.read_ms()<100){} + gyro.update(); + name.reset(); + } +}