go-roboB / Mbed 2 deprecated aa

a

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@0:e22d70b99d51, 2022-03-05 (annotated)

Committer:
darkumatar
Date:
Sat Mar 05 05:00:55 2022 +0000
Revision:
0:e22d70b99d51
Child:
1:0867d6d1421a
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
darkumatar 0:e22d70b99d51 1 #include "mbed.h"
darkumatar 0:e22d70b99d51 2 #include"scrp_slave.hpp"
darkumatar 0:e22d70b99d51 3 #include "rotary_inc.hpp"
darkumatar 0:e22d70b99d51 4 #include"cmath"
darkumatar 0:e22d70b99d51 5 #include"gy521.hpp"
darkumatar 0:e22d70b99d51 6
darkumatar 0:e22d70b99d51 7 #include "air.hpp"
darkumatar 0:e22d70b99d51 8 #include "Servo.h"
darkumatar 0:e22d70b99d51 9
darkumatar 0:e22d70b99d51 10
darkumatar 0:e22d70b99d51 11 #define goal_x 14000.0
darkumatar 0:e22d70b99d51 12 #define goal_y 15000.0
darkumatar 0:e22d70b99d51 13 #define period_r 3000.0
darkumatar 0:e22d70b99d51 14
darkumatar 0:e22d70b99d51 15 I2C i2d(PB_3,PB_10);
darkumatar 0:e22d70b99d51 16 GY521 gyro(i2d);
darkumatar 0:e22d70b99d51 17 ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定
darkumatar 0:e22d70b99d51 18 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)};//ロリコンピン
darkumatar 0:e22d70b99d51 19 //PC_11 PC_10 PC_3 PC_2
darkumatar 0:e22d70b99d51 20 int target[4];//target変数　この変数の値を目指すように動く
darkumatar 0:e22d70b99d51 21 int angler=90;
darkumatar 0:e22d70b99d51 22 int speedr=1024;
darkumatar 0:e22d70b99d51 23 bool auto_swich = false;
darkumatar 0:e22d70b99d51 24 int step=0;
darkumatar 0:e22d70b99d51 25
darkumatar 0:e22d70b99d51 26
darkumatar 0:e22d70b99d51 27
darkumatar 0:e22d70b99d51 28
darkumatar 0:e22d70b99d51 29
darkumatar 0:e22d70b99d51 30
darkumatar 0:e22d70b99d51 31 bool add(int id,int ppp){//速度を変えるやつ
darkumatar 0:e22d70b99d51 32 if(auto_swich==false){
darkumatar 0:e22d70b99d51 33 target[id]=20*ppp;
darkumatar 0:e22d70b99d51 34 }
darkumatar 0:e22d70b99d51 35 return true;
darkumatar 0:e22d70b99d51 36 }
darkumatar 0:e22d70b99d51 37
darkumatar 0:e22d70b99d51 38 bool anglez(int id,int ppp){//おまけで作ったやつ　アングルを決めてその方向へ走る
darkumatar 0:e22d70b99d51 39 angler=ppp;
darkumatar 0:e22d70b99d51 40 for(int i=0;i<4;i++){
darkumatar 0:e22d70b99d51 41 //target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
darkumatar 0:e22d70b99d51 42
darkumatar 0:e22d70b99d51 43 }
darkumatar 0:e22d70b99d51 44 return true;
darkumatar 0:e22d70b99d51 45 }
darkumatar 0:e22d70b99d51 46
darkumatar 0:e22d70b99d51 47 bool speedz(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:e22d70b99d51 48 speedr=ppp;
darkumatar 0:e22d70b99d51 49 for(int i=0;i<4;i++){
darkumatar 0:e22d70b99d51 50 target[i]=(sin(M_PI/180.0*(angler+90*i))+cos(M_PI/180.0*(angler+90*i)))*speedr;
darkumatar 0:e22d70b99d51 51
darkumatar 0:e22d70b99d51 52 }
darkumatar 0:e22d70b99d51 53 return true;
darkumatar 0:e22d70b99d51 54 }
darkumatar 0:e22d70b99d51 55
darkumatar 0:e22d70b99d51 56 bool auto_on(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:e22d70b99d51 57 if(ppp==1){
darkumatar 0:e22d70b99d51 58 auto_swich=true;
darkumatar 0:e22d70b99d51 59 step=0;
darkumatar 0:e22d70b99d51 60 }
darkumatar 0:e22d70b99d51 61 return true;
darkumatar 0:e22d70b99d51 62 }
darkumatar 0:e22d70b99d51 63
darkumatar 0:e22d70b99d51 64 bool auto_off(int id,int ppp){//おまけで作ったやつ　スピードを決めてその方向へ走る
darkumatar 0:e22d70b99d51 65 if(ppp==1){
darkumatar 0:e22d70b99d51 66 auto_swich=false;
darkumatar 0:e22d70b99d51 67 step=0;
darkumatar 0:e22d70b99d51 68 }
darkumatar 0:e22d70b99d51 69 return true;
darkumatar 0:e22d70b99d51 70 }
darkumatar 0:e22d70b99d51 71
darkumatar 0:e22d70b99d51 72 bool turn(int id,int ppp){//回転
darkumatar 0:e22d70b99d51 73 for(int i=0;i<4;i++){
darkumatar 0:e22d70b99d51 74 target[i]=ppp;
darkumatar 0:e22d70b99d51 75 }
darkumatar 0:e22d70b99d51 76 return true;
darkumatar 0:e22d70b99d51 77 }
darkumatar 0:e22d70b99d51 78
darkumatar 0:e22d70b99d51 79 bool pro(int id,int ppp){ //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
darkumatar 0:e22d70b99d51 80 int angle=ppp%1000;
darkumatar 0:e22d70b99d51 81 int speed=ppp/1000;
darkumatar 0:e22d70b99d51 82 for(int i=0;i<4;i++){
darkumatar 0:e22d70b99d51 83 target[i]=(sin(M_PI/180.0*(angle+90*i))+cos(M_PI/180.0*(angle+90*i)))*speed*100.0;
darkumatar 0:e22d70b99d51 84
darkumatar 0:e22d70b99d51 85 }
darkumatar 0:e22d70b99d51 86 return true;
darkumatar 0:e22d70b99d51 87 }
darkumatar 0:e22d70b99d51 88
darkumatar 0:e22d70b99d51 89 bool abc1(int a,int &b){
darkumatar 0:e22d70b99d51 90 return add(0,a);
darkumatar 0:e22d70b99d51 91 }
darkumatar 0:e22d70b99d51 92 bool abc2(int a,int &b){
darkumatar 0:e22d70b99d51 93 return add(1,a);
darkumatar 0:e22d70b99d51 94 }
darkumatar 0:e22d70b99d51 95 bool abc3(int a,int &b){
darkumatar 0:e22d70b99d51 96 return add(2,a);
darkumatar 0:e22d70b99d51 97 }
darkumatar 0:e22d70b99d51 98 bool abc4(int a,int &b){
darkumatar 0:e22d70b99d51 99 return add(3,a);
darkumatar 0:e22d70b99d51 100 }
darkumatar 0:e22d70b99d51 101 bool abc5(int a,int &b){
darkumatar 0:e22d70b99d51 102 return turn(0,a);
darkumatar 0:e22d70b99d51 103 }
darkumatar 0:e22d70b99d51 104 bool abc6(int a,int &b){
darkumatar 0:e22d70b99d51 105 return pro(0,a);
darkumatar 0:e22d70b99d51 106 }
darkumatar 0:e22d70b99d51 107 bool abc7(int a,int &b){
darkumatar 0:e22d70b99d51 108 return anglez(0,a);
darkumatar 0:e22d70b99d51 109 }
darkumatar 0:e22d70b99d51 110 bool abc8(int a,int &b){
darkumatar 0:e22d70b99d51 111 return speedz(0,a);
darkumatar 0:e22d70b99d51 112 }
darkumatar 0:e22d70b99d51 113 bool abc9(int a,int &b){
darkumatar 0:e22d70b99d51 114 return auto_on(0,a);
darkumatar 0:e22d70b99d51 115 }
darkumatar 0:e22d70b99d51 116 bool abc10(int a,int &b){
darkumatar 0:e22d70b99d51 117 return auto_off(0,a);
darkumatar 0:e22d70b99d51 118 }
darkumatar 0:e22d70b99d51 119 int main() {
darkumatar 0:e22d70b99d51 120 slave.port2.printf("%d\n",1);
darkumatar 0:e22d70b99d51 121 slave.addCMD(2,abc1);
darkumatar 0:e22d70b99d51 122 slave.addCMD(3,abc2);
darkumatar 0:e22d70b99d51 123 slave.addCMD(4,abc3);
darkumatar 0:e22d70b99d51 124 slave.addCMD(5,abc4);
darkumatar 0:e22d70b99d51 125 // slave.addCMD(6,abc5);
darkumatar 0:e22d70b99d51 126 // slave.addCMD(7,abc6);
darkumatar 0:e22d70b99d51 127 // slave.addCMD(8,abc7);
darkumatar 0:e22d70b99d51 128 // slave.addCMD(9,abc8);
darkumatar 0:e22d70b99d51 129 slave.addCMD(12,abc9);
darkumatar 0:e22d70b99d51 130 slave.addCMD(13,abc10);
darkumatar 0:e22d70b99d51 131
darkumatar 0:e22d70b99d51 132
darkumatar 0:e22d70b99d51 133 int i;
darkumatar 0:e22d70b99d51 134 double x_period=0;
darkumatar 0:e22d70b99d51 135 double y_period=0;
darkumatar 0:e22d70b99d51 136 int after[4],before[4],before_parus[4],speed_pwm[4];
darkumatar 0:e22d70b99d51 137 double ca[4],P[4],I[4],D[4],integral[4];
darkumatar 0:e22d70b99d51 138 PwmOut motor_p[4]{PwmOut(PB_1),PwmOut(PB_14),PwmOut(PC_8),PwmOut(PB_6)};//モーター宣言 PB_6 PC_8
darkumatar 0:e22d70b99d51 139 PwmOut motor_m[4]{PwmOut(PA_11),PwmOut(PB_13),PwmOut(PC_9),PwmOut(PB_7)};//モーター宣言 PB_7 PC_9
darkumatar 0:e22d70b99d51 140 for(i=0;i<4;i++){
darkumatar 0:e22d70b99d51 141 motor_p[i].period_us(2048);
darkumatar 0:e22d70b99d51 142 motor_m[i].period_us(2048);
darkumatar 0:e22d70b99d51 143 target[i]=0;
darkumatar 0:e22d70b99d51 144 after[i]=0;
darkumatar 0:e22d70b99d51 145 before[i]=0;
darkumatar 0:e22d70b99d51 146 ca[i]=0;
darkumatar 0:e22d70b99d51 147 P[i]=0;
darkumatar 0:e22d70b99d51 148 I[i]=0;
darkumatar 0:e22d70b99d51 149 D[i]=0;
darkumatar 0:e22d70b99d51 150 before_parus[i]=0;
darkumatar 0:e22d70b99d51 151 integral[i]=0;
darkumatar 0:e22d70b99d51 152 speed_pwm[i]=0;
darkumatar 0:e22d70b99d51 153 }
darkumatar 0:e22d70b99d51 154 Timer name;
darkumatar 0:e22d70b99d51 155 name.start();
darkumatar 0:e22d70b99d51 156 gyro.start();
darkumatar 0:e22d70b99d51 157 double angle=0;
darkumatar 0:e22d70b99d51 158 double goal_angle1=0;
darkumatar 0:e22d70b99d51 159 double test_speed_auto=0;
darkumatar 0:e22d70b99d51 160 int test_x=0;
darkumatar 0:e22d70b99d51 161 int test_y=0;
darkumatar 0:e22d70b99d51 162 int test_a=0;
darkumatar 0:e22d70b99d51 163 while(1) {
darkumatar 0:e22d70b99d51 164 angle=45.0+gyro.yaw;
darkumatar 0:e22d70b99d51 165
darkumatar 0:e22d70b99d51 166 if(auto_swich==true){
darkumatar 0:e22d70b99d51 167 goal_angle1=-1.0*atan((goal_x-x_period)/(goal_y-y_period))/M_PI*180.0;
darkumatar 0:e22d70b99d51 168 if(goal_angle1>0){
darkumatar 0:e22d70b99d51 169 goal_angle1-=180.0;
darkumatar 0:e22d70b99d51 170 }else{
darkumatar 0:e22d70b99d51 171 goal_angle1+=180.0;
darkumatar 0:e22d70b99d51 172 }
darkumatar 0:e22d70b99d51 173
darkumatar 0:e22d70b99d51 174 if(step==0&&((goal_angle1-gyro.yaw)>1||(goal_angle1-gyro.yaw)<-1)){
darkumatar 0:e22d70b99d51 175 test_speed_auto=(goal_angle1-gyro.yaw)*-20.0;
darkumatar 0:e22d70b99d51 176 if(test_speed_auto>2000){
darkumatar 0:e22d70b99d51 177 test_speed_auto=2000;
darkumatar 0:e22d70b99d51 178 }else if(test_speed_auto<-2000){
darkumatar 0:e22d70b99d51 179 test_speed_auto=-2000;
darkumatar 0:e22d70b99d51 180 }
darkumatar 0:e22d70b99d51 181 turn(0,(test_speed_auto)*1);
darkumatar 0:e22d70b99d51 182 }else if(step>=0&&step<20){
darkumatar 0:e22d70b99d51 183 step+=1;
darkumatar 0:e22d70b99d51 184 }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))){
darkumatar 0:e22d70b99d51 185 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;
darkumatar 0:e22d70b99d51 186 if(test_speed_auto>1000){
darkumatar 0:e22d70b99d51 187 test_speed_auto=1000;
darkumatar 0:e22d70b99d51 188 }else if(test_speed_auto<-1000){
darkumatar 0:e22d70b99d51 189 test_speed_auto=-1000;
darkumatar 0:e22d70b99d51 190 }
darkumatar 0:e22d70b99d51 191 speedz(0,(test_speed_auto)*1);
darkumatar 0:e22d70b99d51 192
darkumatar 0:e22d70b99d51 193 }else {
darkumatar 0:e22d70b99d51 194 auto_swich=false;
darkumatar 0:e22d70b99d51 195 }
darkumatar 0:e22d70b99d51 196 }
darkumatar 0:e22d70b99d51 197
darkumatar 0:e22d70b99d51 198 for(i=0;i<4;i++){
darkumatar 0:e22d70b99d51 199 after[i]=v[i].get();//ロリコンから値読み取り
darkumatar 0:e22d70b99d51 200 integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
darkumatar 0:e22d70b99d51 201 P[i]=0.05*(target[i]/10.0-(after[i]-before[i]))/102.40;//比例
darkumatar 0:e22d70b99d51 202 D[i]=0.0001*((((target[i]/10.0-(after[i]-before[i]))- before_parus[i])/0.1)/102.40);//微分
darkumatar 0:e22d70b99d51 203 I[i]=0.0001*integral[i];//積分
darkumatar 0:e22d70b99d51 204 ca[i]=ca[i]+P[i]+I[i]+D[i];//足し合わせる
darkumatar 0:e22d70b99d51 205 if(ca[i]>0.4){ca[i]=0.4;}
darkumatar 0:e22d70b99d51 206 if(ca[i]<-0.4){ca[i]=-0.4;}
darkumatar 0:e22d70b99d51 207 motor_p[i]=ca[i];
darkumatar 0:e22d70b99d51 208 motor_m[i]=ca[i]*-1.0;
darkumatar 0:e22d70b99d51 209 speed_pwm[i]=after[i]-before[i];
darkumatar 0:e22d70b99d51 210 before_parus[i]=target[i]/10.0-(after[i]-before[i]);
darkumatar 0:e22d70b99d51 211 before[i]=after[i];
darkumatar 0:e22d70b99d51 212 }
darkumatar 0:e22d70b99d51 213
darkumatar 0:e22d70b99d51 214 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);
darkumatar 0:e22d70b99d51 215 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);
darkumatar 0:e22d70b99d51 216 test_x=x_period;
darkumatar 0:e22d70b99d51 217 test_y=y_period;
darkumatar 0:e22d70b99d51 218 test_a=angle;
darkumatar 0:e22d70b99d51 219 slave.port2.printf("%d\n",1);
darkumatar 0:e22d70b99d51 220 slave.send1(255,30,test_x);
darkumatar 0:e22d70b99d51 221 while(name.read_ms()<30){}
darkumatar 0:e22d70b99d51 222 slave.send1(255,31,test_y);
darkumatar 0:e22d70b99d51 223 while(name.read_ms()<60){}
darkumatar 0:e22d70b99d51 224 slave.send1(255,32,test_a);
darkumatar 0:e22d70b99d51 225 while(name.read_ms()<100){}
darkumatar 0:e22d70b99d51 226 gyro.update();
darkumatar 0:e22d70b99d51 227 name.reset();
darkumatar 0:e22d70b99d51 228 }
darkumatar 0:e22d70b99d51 229 }