go-roboB
a
Dependencies: mbed SBDBT arrc_mbed
Diff: main.cpp
- Revision:
- 0:e22d70b99d51
- Child:
- 1:0867d6d1421a
--- /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();
+ }
+}