master
Dependencies: arrc_mbed
main.cpp
- Committer:
- darkumatar
- Date:
- 2022-04-13
- Revision:
- 2:2feb9be14c08
- Parent:
- 1:9f57d402ea10
- Child:
- 3:a35f7cab9de2
File content as of revision 2:2feb9be14c08:
#include "mbed.h" #include"scrp_slave.hpp" #include "rotary_inc.hpp" #include"cmath" #include"gy521.hpp" #include "neopixel.h" bool the_left=false; double goal_x = -17700.0; double goal_y = 28800.0; int goal_x2 = -8000000; int goal_y2 = 10000000; #define goal_x_right -17700.0 #define goal_y_right 28800.0//23800.0 #define goal_x2_right -8000000 #define goal_y2_right 10000000 #define goal_x_left -17700.0 #define goal_y_left -23800.0 #define goal_x2_left -8000000 #define goal_y2_left -8000000 #define period_r 8000.0 NeoPixelOut npx(PB_5,30); 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変数 この変数の値を目指すように動く bool auto_swich = false; bool limit_switch = true; int step=0; int auto_mode=0; int side_change=0; int ty=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 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 pro2(){ //アングルとスピードを同時に決める 下から3桁で角度を決める(0~360) それ以外でスピードを決める これなに?私もわからん double anglert=-135.000000000+180.0*side_change; 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; ty=0; 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 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; ty=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); side_change=0; goal_x=goal_x_right; goal_y=goal_y_right; goal_x2=goal_x2_right; goal_y2=goal_y2_right; if(the_left==true){ side_change=1; goal_x=goal_x_left; goal_y=goal_y_left; goal_x2=goal_x2_left; goal_y2=goal_y2_left; } 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; name.start(); gyro.start(); double angle=0; double goal_angle1=0; Led1.write(1); while(limit_switch==true) { angle=gyro.yaw-90.00000; if(auto_swich==true){ goal_angle1=-1.0*atan((goal_x2-x_period)/(goal_y2-y_period))/M_PI*180.0; if(side_change==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; } } 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]; } 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 < 10; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(ty==0){ npx.setPixelColor(i,0xFF0000); }else { npx.setPixelColor(i,0x0000FF); } } for(i = 10; i < 20; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(auto_swich){ npx.setPixelColor(i,0xFFFFFF); }else { npx.setPixelColor(i,0x00FF00); } } for(i = 20; i < 30; i++){ npx.global_scale = 0.05f; npx.normalize = false; if(side_change==0){ npx.setPixelColor(i,0xFF00FF); }else { npx.setPixelColor(i,0xFFFF00); } } 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); } for(i=4;i<30;i++){ npx.global_scale = 0.05f; npx.normalize = false; npx.setPixelColor(i,0x00FFFF); } npx.show(); } }