![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
大会本番用
Dependencies: mbed SpeedController hcsr04 Encoder CruizCore_R1370P
main.cpp
- Committer:
- maxnagazumi
- Date:
- 2020-03-11
- Revision:
- 0:c0e9bbc27454
- Child:
- 1:a692014d8e41
File content as of revision 0:c0e9bbc27454:
#include "mbed.h" #include "EC.h" #include "SpeedController.h" #include "math.h" #include "R1370P.h" #include"hcsr04.h" #define RESOLUTION 500 CAN can1(PB_5,PB_13); Ec2multi ec[]= { Ec2multi(PC_5,PB_2,RESOLUTION), Ec2multi(PA_11,PB_1,RESOLUTION), Ec2multi(PB_12,PB_15,RESOLUTION), Ec2multi(PC_4,PB_14,RESOLUTION) }; //2逓倍用class Ec2multi ecXY[]= { Ec2multi(PC_6,PB_8,RESOLUTION), Ec2multi(PC_8,PB_9,RESOLUTION) }; SpeedControl motor[]= { SpeedControl(PA_5,PC_7,50,ec[0]), SpeedControl(PC_9,PA_1,50,ec[1]), SpeedControl(PA_10,PB_4,50,ec[2]), SpeedControl(PA_9,PA_7,50,ec[3]) }; DigitalIn button(USER_BUTTON); Serial pc(USBTX, USBRX); // tx, rx R1370P gyro(PC_10,PC_11); // tx, rx HCSR04 echo[]= { HCSR04(PC_0,PC_12)//A ,HCSR04(PA_15,PB_7)//A ,HCSR04(PH_1,PB_0)// B ,HCSR04(PC_3,PB_10)//B }; Ticker ticker; //自己位置取得 double theta=0; class Location { public: Location():x_(0),y_(0) { for(int i =0; i<2; i++) { old_count[i]=0; } } void calXY() { double ec_count[2]= {}; double ax,ay,bx,by; double atheta,btheta; atheta = (45+theta)/180*3.14; btheta = (135+theta)/180*3.14; ec_count[0]=ecXY[0].getCount(); ec_count[1]=ecXY[1].getCount(); ax = (ec_count[0]-old_count[0])*cos(atheta); ay = (ec_count[0]-old_count[0])*sin(atheta); bx = (ec_count[1]-old_count[1])*cos(btheta); by = (ec_count[1]-old_count[1])*sin(btheta); x_=x_+ax + bx; y_=y_+ay + by; old_count[0]=ec_count[0]; old_count[1]=ec_count[1]; } double getX() { return x_; } double getY() { return y_; } private: double x_; double y_; double old_count[2]; }; //目的地決定 int plot[][2]= { {0,0} ,{800,13500} ,{1000,15500} ,{8654,16500} ,{16000,16500} }; double aimTheta[]= {//目標角度を指定 0,0,0,0,0,0,0,0,0,0,0,0 }; double zMin[]= { //速度の最少を指定 2,2,2, 2,2,2, 5,5,5, 5,5,5 }; //出力を計算 int x,y; class WheelOmega { public: WheelOmega(): max_(0),vx_(0),vy_(0),theta_(0) { for(int i=0; i<4; i++) { omega[i]=0; } } void setOmega(double max,double k) { max_=max; k_=k; } void setVxy(double vx,double vy,double aimtheta_) { vx_=vx; vy_=vy; theta_=aimtheta_ - theta; if(theta_>30) {//目標角度まで30度以上空いていたら補正、係数調整のため30は適当 theta_=30; } if(theta_<-30) { theta_=-30; } } void calOmega() { double theta_rad=45/180*3.14; omega[0]=max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad) + theta_*k_; omega[1]=-max_*vx_*cos(theta_rad)-max_*vy_*cos(theta_rad)+theta_*k_; omega[2]=-max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_; omega[3]=max_*vx_*cos(theta_rad)+max_*vy_*cos(theta_rad)+theta_*k_; }; double getOmega(int i) { return omega[i]; } private: double max_,vx_,vy_,theta_,k_; double omega[4]; }; WheelOmega omega; //パラメタ処理 double pControl(double distance_,double zMin,double newtime) { double z,zMax,olddistance,oldtime; double diftime_; diftime_ = newtime - oldtime; oldtime= newtime; z=0.004*distance_ - 0.1*(olddistance-distance_)/diftime_; zMax=2; if(z>zMax) { z=zMax; } if(z<zMin) { z=zMin; } if(newtime<1) { z=z*newtime; } olddistance = distance_; return z; } //超音波 class Sonic { private: Sonic() { for(int i=0; i<4; i++) { sonic_cm[i]=0; } } void cal_sonic() { for(int i=0; i<4; i++) { echo[i].start(); } for(int i=0; i<4; i++) { sonic_cm[i] =echo[i].get_dist_cm(); } } double get_sonic(int i) { return sonic_cm[i]; } private: double sonic_cm[4]; }; //出力 //int a=0; //int j=0; void motorOut() { for(int i=0; i<4; i++) { motor[i].Sc(omega.getOmega(i)); } } int main() { can1.frequency(1000000); gyro.initialize(); //main関数の最初に一度だけ実行 gyro.acc_offset(); double angle; angle=gyro.getAngle(); double z; printf("start\r\n"); motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290); motor[1].setEquation(0.008878,-0.016622,-0.009702,-0.015806); motor[2].setEquation(0.008637,-0.016537,-0.009397,-0.012159); motor[3].setEquation(0.008096,-0.014822,-0.008801,-0.016645); motor[0].setDutyLimit(0.4); motor[1].setDutyLimit(0.4); motor[2].setDutyLimit(0.4); motor[3].setDutyLimit(0.4); motor[0].setPDparam( 0.1790, 0.00560); motor[1].setPDparam( 0.1705, 0.00620); motor[2].setPDparam( 0.1790, 0.00620); motor[3].setPDparam( 0.1680, 0.00560); while(1) { printf("waiting\r\n"); if(button==0) { wait(1); ticker.attach(motorOut,0.05); break; } } int n=1,dx,dy,aimX,aimY; double vx_,vy_,vx,vy,newtime,distance; Location location; Timer time; time.start(); while(1) { //自己位置取得 theta=gyro.getAngle()-angle; //角度の値を受け取る location.calXY(); x=location.getX(); y=location.getY(); printf("X=%d,Y=%d,theta=%5.3f z=%5.3f %f\r\n",x,y,theta,z,time.read()); //目的地決定(syuusoku check) aimX = plot[n][0]; aimY = plot[n][1]; //出力を計算(kitai xy); dx=aimX-x; dy=aimY-y; vx_=dx/sqrt((double)dx*dx+dy*dy); vy_=dy/sqrt((double)dx*dx+dy*dy); vx=vx_*cos(theta/180*3.14)+vy_*sin(theta/180*3.14); vy=-vx_*sin(theta/180*3.14)+vy_*cos(theta/180*3.14); //四輪の出力計算 newtime=time.read(); distance = sqrt((float)dx*dx+dy*dy); z=pControl(distance,zMin[n],newtime); omega.setOmega(z,0.05); omega.setVxy(vx,vy,aimTheta[n]); omega.calOmega(); //ゴール判定 if(distance<800) { n++; printf("reach%d\r\n",n); time.reset(); } if(n>=5) { for(int j=0; j<4; j++) { motor[j].Sc(0); } printf("fin\r\n"); ticker.detach(); } } }